From: Joseph Coffland Date: Sat, 14 Jan 2017 01:51:44 +0000 (-0800) Subject: Moved files to avr/ X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=529e11d6fa39855ed9c967303395b98dbccd94de;p=bbctrl-firmware Moved files to avr/ --- diff --git a/.gitignore b/.gitignore deleted file mode 100755 index f1a408a..0000000 --- a/.gitignore +++ /dev/null @@ -1,11 +0,0 @@ -# Backup files -*~ -\#* - -build - -/*.eep -/*.hex -/*.elf -/*.lss -/*.map diff --git a/CODE_TAG b/CODE_TAG deleted file mode 100644 index 6a26c85..0000000 --- a/CODE_TAG +++ /dev/null @@ -1,22 +0,0 @@ - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016, Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/LICENSE b/LICENSE deleted file mode 100644 index b08b657..0000000 --- a/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/Makefile b/Makefile deleted file mode 100644 index ed6910d..0000000 --- a/Makefile +++ /dev/null @@ -1,149 +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) - -CFLAGS += $(COMMON) -CFLAGS += -Wall -Werror # -Wno-error=unused-function -CFLAGS += -Wno-error=strict-aliasing # for _invsqrt -CFLAGS += -std=gnu99 -DF_CPU=$(CLOCK)UL -O3 #-funroll-loops -CFLAGS += -funsigned-bitfields -fpack-struct -fshort-enums -funsigned-char -CFLAGS += -MD -MP -MT $@ -MF build/dep/$(@F).d -CFLAGS += -Isrc -DVERSION=\"$(VERSION)\" - -# Linker flags -LDFLAGS += $(COMMON) -Wl,-u,vfprintf -lprintf_flt -lm -LIBS += -lm - -# EEPROM flags -EEFLAGS += -j .eeprom -EEFLAGS += --set-section-flags=.eeprom="alloc,load" -EEFLAGS += --change-section-lma .eeprom=0 --no-change-warnings - -# Programming flags -PROGRAMMER = avrispmkII -#PROGRAMMER = jtag3pdi -PDEV = usb -AVRDUDE_OPTS = -c $(PROGRAMMER) -p $(MCU) -P $(PDEV) - -FUSE0=0xff -FUSE1=0x00 -FUSE2=0xbe -FUSE4=0xff -FUSE5=0xeb - -# SRC -SRC = $(wildcard src/*.c) -SRC += $(wildcard src/plan/*.c) -OBJ = $(patsubst src/%.c,build/%.o,$(SRC)) - -# Boot SRC -BOOT_SRC = $(wildcard src/xboot/*.S) -BOOT_SRC += $(wildcard src/xboot/*.c) -BOOT_OBJ = $(patsubst src/%.c,build/%.o,$(BOOT_SRC)) -BOOT_OBJ := $(patsubst src/%.S,build/%.o,$(BOOT_OBJ)) - -BOOT_LDFLAGS = $(LDFLAGS) -Wl,--section-start=.text=0x030000 - -# Build -all: $(TARGET) $(PROJECT).hex $(PROJECT).eep $(PROJECT).lss xboot.hex \ - boot-size size - -# Compile -build/%.o: src/%.c - @mkdir -p $(shell dirname $@) - $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $< - -build/%.o: src/%.S - @mkdir -p $(shell dirname $@) - $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $< - -# Link -$(TARGET): $(OBJ) - $(CC) $(LDFLAGS) $(OBJ) $(LIBS) -o $@ - -xboot.elf: $(BOOT_OBJ) - $(CC) $(BOOT_LDFLAGS) $(BOOT_OBJ) -o $@ - -%.hex: %.elf - avr-objcopy -O ihex -R .eeprom -R .fuse -R .lock -R .signature $< $@ - -%.eep: %.elf - avr-objcopy $(EEFLAGS) -O ihex $< $@ - -%.lss: %.elf - avr-objdump -h -S $< > $@ - -_size: - @for X in A B C; do\ - echo '****************************************************************' ;\ - avr-size -$$X --mcu=$(MCU) $(SIZE_TARGET) ;\ - done - -boot-size: xboot.elf - @$(MAKE) SIZE_TARGET=$< _size - -size: $(TARGET) - @$(MAKE) SIZE_TARGET=$< _size - -# Program -init: - $(MAKE) erase - -$(MAKE) fuses - $(MAKE) program-boot - $(MAKE) program - -reset: - avrdude $(AVRDUDE_OPTS) - -erase: - avrdude $(AVRDUDE_OPTS) -e - -program: $(PROJECT).hex - avrdude $(AVRDUDE_OPTS) -U flash:w:$(PROJECT).hex:i - -verify: $(PROJECT).hex - avrdude $(AVRDUDE_OPTS) -U flash:v:$(PROJECT).hex:i - -program-boot: xboot.hex - avrdude $(AVRDUDE_OPTS) -U flash:w:xboot.hex:i - -fuses: - avrdude $(AVRDUDE_OPTS) -U fuse0:w:$(FUSE0):m -U fuse1:w:$(FUSE1):m \ - -U fuse2:w:$(FUSE2):m -U fuse4:w:$(FUSE4):m -U fuse5:w:$(FUSE5):m - -read_fuses: - avrdude $(AVRDUDE_OPTS) -q -q -U fuse0:r:-:h -U fuse1:r:-:h -U fuse2:r:-:h \ - -U fuse4:r:-:h -U fuse5:r:-:h - -signature: - avrdude $(AVRDUDE_OPTS) -q -q -U signature:r:-:h - -prodsig: - avrdude $(AVRDUDE_OPTS) -q -q -U prodsig:r:-:h - -usersig: - avrdude $(AVRDUDE_OPTS) -q -q -U usersig:r:-:h - -# Clean -tidy: - rm -f $(shell find -name \*~ -o -name \#\*) - -clean: tidy - rm -rf $(PROJECT).elf $(PROJECT).hex $(PROJECT).eep $(PROJECT).lss \ - $(PROJECT).map build xboot.hex xboot.elf - -.PHONY: tidy clean size all reset erase program fuses read_fuses prodsig -.PHONY: signature usersig - -# Dependencies --include $(shell mkdir -p build/dep) $(wildcard build/dep/*) diff --git a/MoveLifecycle.md b/MoveLifecycle.md deleted file mode 100644 index f6d781b..0000000 --- a/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 differencing is used to map -the trapezoid computed during the planning stage to a fifth-degree Bezier -polynomial S-curve. The curve is used to find the appropriate velocity at the -next target position. - -``_exec_aline_segment()`` is called for each non-zero section to convert the -computed target position to target steps by calling ``mp_kinematics()``. The -move fragment is then passed to the stepper driver by a call to -``st_prep_line()``. When a segment is complete ``_exec_aline_segment()`` -returns ``STAT_OK`` indicating the next segment should be loaded. When all -non-zero segments have been executed, the move is complete. - -## Pulse generation -Calls to ``st_prep_line()`` prepare short (~5ms) move fragements for pluse -generation by the stepper driver. Move time in clock ticks is computed from -travel in steps and move duration. Then ``motor_prep_move()`` is called for -each motor. ``motor_prep_move()`` may perform step correction and enable the -motor. It then computes the optimal step clock divisor, clock ticks and sets -the move direction, taking the motor's configuration in to account. - -The stepper timer ISR, after ending the previous move, calls -``motor_load_move()`` on each motor. This sets up and starts the motor clocks, -sets the motor direction lines and accumulates and resets the step encoders. -After (re)starting the motor clocks the ISR signals a lower level interrupt to -call ``mp_exec_move()`` and load the next move fragment. diff --git a/README.md b/README.md deleted file mode 100644 index 24ba38a..0000000 --- a/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/.gitignore b/avr/.gitignore new file mode 100755 index 0000000..f1a408a --- /dev/null +++ b/avr/.gitignore @@ -0,0 +1,11 @@ +# Backup files +*~ +\#* + +build + +/*.eep +/*.hex +/*.elf +/*.lss +/*.map diff --git a/avr/CODE_TAG b/avr/CODE_TAG new file mode 100644 index 0000000..6a26c85 --- /dev/null +++ b/avr/CODE_TAG @@ -0,0 +1,22 @@ + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016, Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..b08b657 --- /dev/null +++ b/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/avr/Makefile b/avr/Makefile new file mode 100644 index 0000000..ed6910d --- /dev/null +++ b/avr/Makefile @@ -0,0 +1,149 @@ +# Makefile for the project Bulidbotics firmware +PROJECT = bbctrl-avr-firmware +MCU = atxmega192a3u +CLOCK = 32000000 +VERSION = 0.3.1 + +TARGET = $(PROJECT).elf + +# Compile flags +CC = avr-gcc +CPP = avr-g++ + +COMMON = -mmcu=$(MCU) + +CFLAGS += $(COMMON) +CFLAGS += -Wall -Werror # -Wno-error=unused-function +CFLAGS += -Wno-error=strict-aliasing # for _invsqrt +CFLAGS += -std=gnu99 -DF_CPU=$(CLOCK)UL -O3 #-funroll-loops +CFLAGS += -funsigned-bitfields -fpack-struct -fshort-enums -funsigned-char +CFLAGS += -MD -MP -MT $@ -MF build/dep/$(@F).d +CFLAGS += -Isrc -DVERSION=\"$(VERSION)\" + +# Linker flags +LDFLAGS += $(COMMON) -Wl,-u,vfprintf -lprintf_flt -lm +LIBS += -lm + +# EEPROM flags +EEFLAGS += -j .eeprom +EEFLAGS += --set-section-flags=.eeprom="alloc,load" +EEFLAGS += --change-section-lma .eeprom=0 --no-change-warnings + +# Programming flags +PROGRAMMER = avrispmkII +#PROGRAMMER = jtag3pdi +PDEV = usb +AVRDUDE_OPTS = -c $(PROGRAMMER) -p $(MCU) -P $(PDEV) + +FUSE0=0xff +FUSE1=0x00 +FUSE2=0xbe +FUSE4=0xff +FUSE5=0xeb + +# SRC +SRC = $(wildcard src/*.c) +SRC += $(wildcard src/plan/*.c) +OBJ = $(patsubst src/%.c,build/%.o,$(SRC)) + +# Boot SRC +BOOT_SRC = $(wildcard src/xboot/*.S) +BOOT_SRC += $(wildcard src/xboot/*.c) +BOOT_OBJ = $(patsubst src/%.c,build/%.o,$(BOOT_SRC)) +BOOT_OBJ := $(patsubst src/%.S,build/%.o,$(BOOT_OBJ)) + +BOOT_LDFLAGS = $(LDFLAGS) -Wl,--section-start=.text=0x030000 + +# Build +all: $(TARGET) $(PROJECT).hex $(PROJECT).eep $(PROJECT).lss xboot.hex \ + boot-size size + +# Compile +build/%.o: src/%.c + @mkdir -p $(shell dirname $@) + $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $< + +build/%.o: src/%.S + @mkdir -p $(shell dirname $@) + $(CC) $(INCLUDES) $(CFLAGS) -c -o $@ $< + +# Link +$(TARGET): $(OBJ) + $(CC) $(LDFLAGS) $(OBJ) $(LIBS) -o $@ + +xboot.elf: $(BOOT_OBJ) + $(CC) $(BOOT_LDFLAGS) $(BOOT_OBJ) -o $@ + +%.hex: %.elf + avr-objcopy -O ihex -R .eeprom -R .fuse -R .lock -R .signature $< $@ + +%.eep: %.elf + avr-objcopy $(EEFLAGS) -O ihex $< $@ + +%.lss: %.elf + avr-objdump -h -S $< > $@ + +_size: + @for X in A B C; do\ + echo '****************************************************************' ;\ + avr-size -$$X --mcu=$(MCU) $(SIZE_TARGET) ;\ + done + +boot-size: xboot.elf + @$(MAKE) SIZE_TARGET=$< _size + +size: $(TARGET) + @$(MAKE) SIZE_TARGET=$< _size + +# Program +init: + $(MAKE) erase + -$(MAKE) fuses + $(MAKE) program-boot + $(MAKE) program + +reset: + avrdude $(AVRDUDE_OPTS) + +erase: + avrdude $(AVRDUDE_OPTS) -e + +program: $(PROJECT).hex + avrdude $(AVRDUDE_OPTS) -U flash:w:$(PROJECT).hex:i + +verify: $(PROJECT).hex + avrdude $(AVRDUDE_OPTS) -U flash:v:$(PROJECT).hex:i + +program-boot: xboot.hex + avrdude $(AVRDUDE_OPTS) -U flash:w:xboot.hex:i + +fuses: + avrdude $(AVRDUDE_OPTS) -U fuse0:w:$(FUSE0):m -U fuse1:w:$(FUSE1):m \ + -U fuse2:w:$(FUSE2):m -U fuse4:w:$(FUSE4):m -U fuse5:w:$(FUSE5):m + +read_fuses: + avrdude $(AVRDUDE_OPTS) -q -q -U fuse0:r:-:h -U fuse1:r:-:h -U fuse2:r:-:h \ + -U fuse4:r:-:h -U fuse5:r:-:h + +signature: + avrdude $(AVRDUDE_OPTS) -q -q -U signature:r:-:h + +prodsig: + avrdude $(AVRDUDE_OPTS) -q -q -U prodsig:r:-:h + +usersig: + avrdude $(AVRDUDE_OPTS) -q -q -U usersig:r:-:h + +# Clean +tidy: + rm -f $(shell find -name \*~ -o -name \#\*) + +clean: tidy + rm -rf $(PROJECT).elf $(PROJECT).hex $(PROJECT).eep $(PROJECT).lss \ + $(PROJECT).map build xboot.hex xboot.elf + +.PHONY: tidy clean size all reset erase program fuses read_fuses prodsig +.PHONY: signature usersig + +# Dependencies +-include $(shell mkdir -p build/dep) $(wildcard build/dep/*) diff --git a/avr/MoveLifecycle.md b/avr/MoveLifecycle.md new file mode 100644 index 0000000..f6d781b --- /dev/null +++ b/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 differencing is used to map +the trapezoid computed during the planning stage to a fifth-degree Bezier +polynomial S-curve. The curve is used to find the appropriate velocity at the +next target position. + +``_exec_aline_segment()`` is called for each non-zero section to convert the +computed target position to target steps by calling ``mp_kinematics()``. The +move fragment is then passed to the stepper driver by a call to +``st_prep_line()``. When a segment is complete ``_exec_aline_segment()`` +returns ``STAT_OK`` indicating the next segment should be loaded. When all +non-zero segments have been executed, the move is complete. + +## Pulse generation +Calls to ``st_prep_line()`` prepare short (~5ms) move fragements for pluse +generation by the stepper driver. Move time in clock ticks is computed from +travel in steps and move duration. Then ``motor_prep_move()`` is called for +each motor. ``motor_prep_move()`` may perform step correction and enable the +motor. It then computes the optimal step clock divisor, clock ticks and sets +the move direction, taking the motor's configuration in to account. + +The stepper timer ISR, after ending the previous move, calls +``motor_load_move()`` on each motor. This sets up and starts the motor clocks, +sets the motor direction lines and accumulates and resets the step encoders. +After (re)starting the motor clocks the ISR signals a lower level interrupt to +call ``mp_exec_move()`` and load the next move fragment. diff --git a/avr/README.md b/avr/README.md new file mode 100644 index 0000000..24ba38a --- /dev/null +++ b/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/avr/data_usage.py b/avr/data_usage.py new file mode 100755 index 0000000..8ba92f0 --- /dev/null +++ b/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/avr/src/axis.c b/avr/src/axis.c new file mode 100644 index 0000000..b01f99b --- /dev/null +++ b/avr/src/axis.c @@ -0,0 +1,214 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + + +int motor_map[AXES] = {-1, -1, -1, -1, -1, -1}; + + +typedef struct { + float velocity_max; // max velocity in mm/min or deg/min + float feedrate_max; // max velocity in mm/min or deg/min + float travel_max; // max work envelope for soft limits + float travel_min; // min work envelope for soft limits + float jerk_max; // max jerk (Jm) in mm/min^3 divided by 1 million + float recip_jerk; // reciprocal of current jerk value - with million + float junction_dev; // aka cornering delta + float radius; // radius in mm for rotary axes + float search_velocity; // homing search velocity + float latch_velocity; // homing latch velocity + float latch_backoff; // backoff from switches prior to homing latch movement + float zero_backoff; // backoff from switches for machine zero + homing_mode_t homing_mode; + bool homed; +} axis_t; + + +axis_t axes[MOTORS] = { + { + .velocity_max = X_VELOCITY_MAX, + .feedrate_max = X_FEEDRATE_MAX, + .travel_min = X_TRAVEL_MIN, + .travel_max = X_TRAVEL_MAX, + .jerk_max = X_JERK_MAX, + .junction_dev = X_JUNCTION_DEVIATION, + .search_velocity = X_SEARCH_VELOCITY, + .latch_velocity = X_LATCH_VELOCITY, + .latch_backoff = X_LATCH_BACKOFF, + .zero_backoff = X_ZERO_BACKOFF, + .homing_mode = X_HOMING_MODE, + }, { + .velocity_max = Y_VELOCITY_MAX, + .feedrate_max = Y_FEEDRATE_MAX, + .travel_min = Y_TRAVEL_MIN, + .travel_max = Y_TRAVEL_MAX, + .jerk_max = Y_JERK_MAX, + .junction_dev = Y_JUNCTION_DEVIATION, + .search_velocity = Y_SEARCH_VELOCITY, + .latch_velocity = Y_LATCH_VELOCITY, + .latch_backoff = Y_LATCH_BACKOFF, + .zero_backoff = Y_ZERO_BACKOFF, + .homing_mode = Y_HOMING_MODE, + }, { + .velocity_max = Z_VELOCITY_MAX, + .feedrate_max = Z_FEEDRATE_MAX, + .travel_min = Z_TRAVEL_MIN, + .travel_max = Z_TRAVEL_MAX, + .jerk_max = Z_JERK_MAX, + .junction_dev = Z_JUNCTION_DEVIATION, + .search_velocity = Z_SEARCH_VELOCITY, + .latch_velocity = Z_LATCH_VELOCITY, + .latch_backoff = Z_LATCH_BACKOFF, + .zero_backoff = Z_ZERO_BACKOFF, + .homing_mode = Z_HOMING_MODE, + }, { + .velocity_max = A_VELOCITY_MAX, + .feedrate_max = A_FEEDRATE_MAX, + .travel_min = A_TRAVEL_MIN, + .travel_max = A_TRAVEL_MAX, + .jerk_max = A_JERK_MAX, + .junction_dev = A_JUNCTION_DEVIATION, + .radius = A_RADIUS, + .search_velocity = A_SEARCH_VELOCITY, + .latch_velocity = A_LATCH_VELOCITY, + .latch_backoff = A_LATCH_BACKOFF, + .zero_backoff = A_ZERO_BACKOFF, + .homing_mode = A_HOMING_MODE, + } +}; + + +bool axis_is_enabled(int axis) { + int motor = axis_get_motor(axis); + return motor != -1 && motor_is_enabled(motor); +} + + +char axis_get_char(int axis) { + return (axis < 0 || AXES <= axis) ? '?' : "XYZABCUVW"[axis]; +} + + +int axis_get_id(char axis) { + const char *axes = "XYZABCUVW"; + char *ptr = strchr(axes, axis); + return ptr == 0 ? -1 : (ptr - axes); +} + + +int axis_get_motor(int axis) {return motor_map[axis];} + + +void axis_set_motor(int axis, int motor) { + motor_map[axis] = motor; + axis_set_jerk_max(axis, axes[motor].jerk_max); // Init 1/jerk +} + + +float axis_get_vector_length(const float a[], const float b[]) { + return sqrt(square(a[AXIS_X] - b[AXIS_X]) + square(a[AXIS_Y] - b[AXIS_Y]) + + square(a[AXIS_Z] - b[AXIS_Z]) + square(a[AXIS_A] - b[AXIS_A]) + + square(a[AXIS_B] - b[AXIS_B]) + square(a[AXIS_C] - b[AXIS_C])); +} + + +#define AXIS_VAR_GET(NAME, TYPE) \ + TYPE get_##NAME(int axis) {return axes[axis].NAME;} + + +#define AXIS_VAR_SET(NAME, TYPE) \ + void set_##NAME(int axis, TYPE value) {axes[axis].NAME = value;} + + +#define AXIS_GET(NAME, TYPE, DEFAULT) \ + TYPE axis_get_##NAME(int axis) { \ + int motor = axis_get_motor(axis); \ + return motor == -1 ? DEFAULT : axes[motor].NAME; \ + } \ + AXIS_VAR_GET(NAME, TYPE) + + +#define AXIS_SET(NAME, TYPE) \ + void axis_set_##NAME(int axis, TYPE value) { \ + int motor = axis_get_motor(axis); \ + if (motor != -1) axes[motor].NAME = value; \ + } \ + AXIS_VAR_SET(NAME, TYPE) + + +AXIS_GET(velocity_max, float, 0) +AXIS_GET(feedrate_max, float, 0) +AXIS_GET(homed, bool, false) +AXIS_SET(homed, bool) +AXIS_GET(homing_mode, homing_mode_t, HOMING_DISABLED) +AXIS_SET(homing_mode, homing_mode_t) +AXIS_GET(radius, float, 0) +AXIS_GET(travel_min, float, DISABLE_SOFT_LIMIT) +AXIS_GET(travel_max, float, DISABLE_SOFT_LIMIT) +AXIS_GET(search_velocity, float, 0) +AXIS_GET(latch_velocity, float, 0) +AXIS_GET(zero_backoff, float, 0) +AXIS_GET(latch_backoff, float, 0) +AXIS_GET(junction_dev, float, 0) +AXIS_GET(recip_jerk, float, 0) + + +/* Note on jerk functions + * + * Jerk values can be rather large. Jerk values are stored in the system in + * truncated format; values are divided by 1,000,000 then multiplied before use. + * + * The axis_jerk() functions expect the jerk in divided by 1,000,000 form. + */ +AXIS_GET(jerk_max, float, 0) + + +/// Sets jerk and its reciprocal for axis +void axis_set_jerk_max(int axis, float jerk) { + axes[axis].jerk_max = jerk; + axes[axis].recip_jerk = 1 / (jerk * JERK_MULTIPLIER); +} + + +AXIS_VAR_SET(velocity_max, float) +AXIS_VAR_SET(feedrate_max, float) +AXIS_VAR_SET(radius, float) +AXIS_VAR_SET(travel_min, float) +AXIS_VAR_SET(travel_max, float) +AXIS_VAR_SET(search_velocity, float) +AXIS_VAR_SET(latch_velocity, float) +AXIS_VAR_SET(zero_backoff, float) +AXIS_VAR_SET(latch_backoff, float) +AXIS_VAR_SET(junction_dev, float) +AXIS_VAR_SET(jerk_max, float) diff --git a/avr/src/axis.h b/avr/src/axis.h new file mode 100644 index 0000000..505cca1 --- /dev/null +++ b/avr/src/axis.h @@ -0,0 +1,76 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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_DISABLED, + HOMING_STALL_MIN, + HOMING_STALL_MAX, + HOMING_SWITCH_MIN, + HOMING_SWITCH_MAX, +} homing_mode_t; + + +bool axis_is_enabled(int axis); +char axis_get_char(int axis); +int axis_get_id(char axis); +int axis_get_motor(int axis); +void axis_set_motor(int axis, int motor); +float axis_get_vector_length(const float a[], const float b[]); + +float axis_get_velocity_max(int axis); +float axis_get_feedrate_max(int axis); +float axis_get_jerk_max(int axis); +void axis_set_jerk_max(int axis, float jerk); +bool axis_get_homed(int axis); +void axis_set_homed(int axis, bool homed); +homing_mode_t axis_get_homing_mode(int axis); +void axis_set_homing_mode(int axis, homing_mode_t mode); +float axis_get_radius(int axis); +float axis_get_travel_min(int axis); +float axis_get_travel_max(int axis); +float axis_get_search_velocity(int axis); +float axis_get_latch_velocity(int axis); +float axis_get_zero_backoff(int axis); +float axis_get_latch_backoff(int axis); +float axis_get_junction_dev(int axis); +float axis_get_recip_jerk(int axis); +float axis_get_jerk_max(int axis); diff --git a/avr/src/command.c b/avr/src/command.c new file mode 100644 index 0000000..936878a --- /dev/null +++ b/avr/src/command.c @@ -0,0 +1,335 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "homing.h" +#include "probing.h" +#include "i2c.h" +#include "plan/jog.h" +#include "plan/calibrate.h" +#include "plan/buffer.h" +#include "plan/arc.h" +#include "plan/state.h" +#include "config.h" + +#include +#include + +#include +#include +#include +#include + + +static char *_cmd = 0; + + +static void _reboot() {hw_request_hard_reset();} + + +static unsigned _parse_axis(uint8_t axis) { + switch (axis) { + case 'x': return 0; case 'y': return 1; case 'z': return 2; + case 'a': return 3; case 'b': return 4; case 'c': return 5; + case 'X': return 0; case 'Y': return 1; case 'Z': return 2; + case 'A': return 3; case 'B': return 4; case 'C': return 5; + default: return axis; + } +} + + +static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) { + switch (cmd) { + case I2C_NULL: break; + case I2C_ESTOP: estop_trigger(STAT_ESTOP_USER); break; + case I2C_CLEAR: estop_clear(); break; + case I2C_PAUSE: mp_request_hold(); break; + case I2C_OPTIONAL_PAUSE: mp_request_optional_pause(); break; + case I2C_RUN: mp_request_start(); break; + case I2C_STEP: mp_request_step(); break; + case I2C_FLUSH: mp_request_flush(); break; + case I2C_REPORT: report_request_full(); break; + case I2C_HOME: break; // TODO + case I2C_REBOOT: _reboot(); break; + case I2C_ZERO: + if (length == 0) mach_zero_all(); + else if (length == 1) mach_zero_axis(_parse_axis(*data)); + break; + } +} + + +void command_init() { + i2c_set_read_callback(command_i2c_cb); +} + + +// Command forward declarations +// (Don't be afraid, X-Macros rock!) +#define CMD(NAME, ...) \ + uint8_t command_##NAME(int, char *[]); + +#include "command.def" +#undef CMD + +// Command names & help +#define CMD(NAME, MINARGS, MAXARGS, HELP) \ + static const char pstr_##NAME[] PROGMEM = #NAME; \ + static const char NAME##_help[] PROGMEM = HELP; + +#include "command.def" +#undef CMD + +// Command table +#define CMD(NAME, MINARGS, MAXARGS, HELP) \ + {pstr_##NAME, command_##NAME, MINARGS, MAXARGS, NAME##_help}, + +static const command_t commands[] PROGMEM = { +#include "command.def" +#undef CMD + {}, // Sentinel +}; + + +int command_find(const char *match) { + for (int i = 0; ; i++) { + const char *name = pgm_read_word(&commands[i].name); + if (!name) break; + + if (strcmp_P(match, name) == 0) return i; + } + + return -1; +} + + +int command_exec(int argc, char *argv[]) { + putchar('\n'); + + int i = command_find(argv[0]); + if (i != -1) { + uint8_t min_args = pgm_read_byte(&commands[i].min_args); + uint8_t max_args = pgm_read_byte(&commands[i].max_args); + + if (argc <= min_args) return STAT_TOO_FEW_ARGUMENTS; + else if (max_args < argc - 1) return STAT_TOO_MANY_ARGUMENTS; + else { + command_cb_t cb = pgm_read_word(&commands[i].cb); + return cb(argc, argv); + } + + } else if (argc != 1) + return STAT_INVALID_OR_MALFORMED_COMMAND; + + // Get or set variable + char *value = strchr(argv[0], '='); + if (value) { + *value++ = 0; + if (vars_set(argv[0], value)) return STAT_OK; + + } else if (vars_print(argv[0])) { + putchar('\n'); + return STAT_OK; + } + + STATUS_ERROR(STAT_UNRECOGNIZED_NAME, "'%s'", argv[0]); + return STAT_UNRECOGNIZED_NAME; +} + + +int command_parser(char *cmd) { + // Parse line + char *p = cmd + 1; // Skip `$` + int argc = 0; + char *argv[MAX_ARGS] = {0}; + + if (cmd[1] == '$' && !cmd[2]) { + report_request_full(); // Full report + return STAT_OK; + } + + while (argc < MAX_ARGS && *p) { + // Skip space + while (*p && isspace(*p)) *p++ = 0; + + // Start of token + if (*p) argv[argc++] = p; + + // Find end + while (*p && !isspace(*p)) p++; + } + + // Exec command + if (argc) return command_exec(argc, argv); + + return STAT_OK; +} + + +static char *_command_next() { + if (_cmd) return _cmd; + + // Get next command + _cmd = usart_readline(); + if (!_cmd) return 0; + + // Remove leading whitespace + while (*_cmd && isspace(*_cmd)) _cmd++; + + // Remove trailing whitespace + for (size_t len = strlen(_cmd); len && isspace(_cmd[len - 1]); len--) + _cmd[len - 1] = 0; + + return _cmd; +} + + +void command_callback() { + if (!_command_next()) return; + + stat_t status = STAT_OK; + + switch (*_cmd) { + case 0: break; // Empty line + case '{': status = vars_parser(_cmd); break; + case '$': status = command_parser(_cmd); break; + case '%': break; // GCode program separator, ignore it + + default: + if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;} + else if (mp_is_flushing()) break; // Flush GCode command + else if (!mp_queue_get_room() || + mp_is_resuming() || + mach_arc_active() || + mach_is_homing() || + mach_is_probing() || + calibrate_busy() || + mp_jog_busy()) return; // Wait + + // Parse and execute GCode command + status = gc_gcode_parser(_cmd); + } + + _cmd = 0; // Command consumed + report_request(); + + if (status) status_error(status); +} + + +// Command functions +void static print_command_help(int i) { + static const char fmt[] PROGMEM = " $%-12S %S\n"; + const char *name = pgm_read_word(&commands[i].name); + const char *help = pgm_read_word(&commands[i].help); + + printf_P(fmt, name, help); +} + + +uint8_t command_help(int argc, char *argv[]) { + if (argc == 2) { + int i = command_find(argv[1]); + + if (i == -1) return STAT_UNRECOGNIZED_NAME; + else print_command_help(i); + + return STAT_OK; + } + + puts_P(PSTR("\nLine editing:\n" + " ENTER Submit current command line.\n" + " BS Backspace, delete last character.\n" + " CTRL-X Cancel current line entry.")); + + puts_P(PSTR("\nCommands:")); + for (int i = 0; ; i++) { + const char *name = pgm_read_word(&commands[i].name); + if (!name) break; + print_command_help(i); + wdt_reset(); + } + + puts_P(PSTR("\nVariables:")); + vars_print_help(); + + return STAT_OK; +} + + +uint8_t command_reboot(int argc, char *argv[]) { + _reboot(); + return 0; +} + + +uint8_t command_bootloader(int argc, char *argv[]) { + hw_request_bootloader(); + return 0; +} + + +uint8_t command_save(int argc, char *argv[]) { + vars_save(); + return 0; +} + + +uint8_t command_valid(int argc, char *argv[]) { + printf_P(vars_valid() ? PSTR("true\n") : PSTR("false\n")); + return 0; +} + + +uint8_t command_restore(int argc, char *argv[]) { + return vars_restore(); +} + + +uint8_t command_clear(int argc, char *argv[]) { + vars_clear(); + return 0; +} + + +uint8_t command_messages(int argc, char *argv[]) { + status_help(); + return 0; +} + + +uint8_t command_resume(int argc, char *argv[]) { + mp_request_resume(); + return 0; +} diff --git a/avr/src/command.def b/avr/src/command.def new file mode 100644 index 0000000..559e45d --- /dev/null +++ b/avr/src/command.def @@ -0,0 +1,41 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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(reboot, 0, 0, "Reboot the controller") +CMD(bootloader, 0, 0, "Load bootloader") +CMD(save, 0, 0, "Save settings") +CMD(valid, 0, 0, "Print 'true' if saved settings are valid") +CMD(restore, 0, 0, "Restore settings") +CMD(clear, 0, 0, "Clear saved settings") +CMD(jog, 1, 4, "Jog") +CMD(mreset, 0, 1, "Reset motor") +CMD(calibrate, 0, 0, "Calibrate motors") +CMD(messages, 0, 0, "Dump all possible status messages") +CMD(resume, 0, 0, "Resume processing after a flush") +CMD(home, 0, 0, "Home all axes") diff --git a/avr/src/command.h b/avr/src/command.h new file mode 100644 index 0000000..cecc572 --- /dev/null +++ b/avr/src/command.h @@ -0,0 +1,50 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..5255a0c --- /dev/null +++ b/avr/src/config.h @@ -0,0 +1,343 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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" + +#include + + +// Pins +enum { + STALL_X_PIN = PORT_A << 3, + STALL_Y_PIN, + STALL_Z_PIN, + STALL_A_PIN, + SPIN_DIR_PIN, + SPIN_ENABLE_PIN, + ANALOG_PIN, + PROBE_PIN, + + MIN_X_PIN = PORT_B << 3, + MAX_X_PIN, + MIN_A_PIN, + MAX_A_PIN, + MIN_Y_PIN, + MAX_Y_PIN, + MIN_Z_PIN, + MAX_Z_PIN, + + SDA_PIN = PORT_C << 3, + SCL_PIN, + SERIAL_RX_PIN, + SERIAL_TX_PIN, + SERIAL_CTS_PIN, + SPI_CLK_PIN, + SPI_MISO_PIN, + SPI_MOSI_PIN, + + STEP_X_PIN = PORT_D << 3, + SPI_CS_X_PIN, + SPI_CS_A_PIN, + SPI_CS_Z_PIN, + SPIN_PWM_PIN, + SWITCH_1_PIN, + RS485_RO_PIN, + RS485_DI_PIN, + + STEP_Y_PIN = PORT_E << 3, + SPI_CS_Y_PIN, + DIR_X_PIN, + DIR_Y_PIN, + STEP_A_PIN, + SWITCH_2_PIN, + DIR_Z_PIN, + DIR_A_PIN, + + STEP_Z_PIN = PORT_F << 3, + RS485_RW_PIN, + FAULT_PIN, + ESTOP_PIN, + MOTOR_FAULT_PIN, + MOTOR_ENABLE_PIN, + NC_0_PIN, + NC_1_PIN, +}; + +#define SPI_SS_PIN SERIAL_CTS_PIN // Needed for SPI configuration + + +// Compile-time settings +#define __CLOCK_EXTERNAL_16MHZ // uses PLL to provide 32 MHz system clock +//#define __CLOCK_INTERNAL_32MHZ + + +#define AXES 6 // number of axes +#define MOTORS 4 // number of motors on the board +#define COORDS 6 // number of supported coordinate systems +#define SWITCHES 9 // number of supported limit switches +#define PWMS 2 // number of supported PWM channels + +#define DISABLE_SOFT_LIMIT -1000000 + + +// Motor settings. See motor.c +#define MOTOR_MAX_CURRENT 1.0 // 1.0 is full power +#define MOTOR_MIN_CURRENT 0.25 // 1.0 is full power +#define MOTOR_IDLE_CURRENT 0.05 // 1.0 is full power +#define MOTOR_STALL_THRESHOLD 0 // 0 -> 1 is least -> most sensitive +#define MOTOR_MICROSTEPS 32 +#define MOTOR_POWER_MODE MOTOR_POWERED_ONLY_WHEN_MOVING +#define MOTOR_IDLE_TIMEOUT 0.25 // secs, motor off after this time + +#define M1_AXIS AXIS_X +#define M1_STEP_ANGLE 1.8 +#define M1_TRAVEL_PER_REV 6.35 +#define M1_MICROSTEPS MOTOR_MICROSTEPS +#define M1_REVERSE false +#define M1_POWER_MODE MOTOR_POWER_MODE + +#define M2_AXIS AXIS_Y +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 6.35 +#define M2_MICROSTEPS MOTOR_MICROSTEPS +#define M2_REVERSE false +#define M2_POWER_MODE MOTOR_POWER_MODE + +#define M3_AXIS AXIS_Z +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV (25.4 / 6.0) +#define M3_MICROSTEPS MOTOR_MICROSTEPS +#define M3_REVERSE false +#define M3_POWER_MODE MOTOR_POWER_MODE + +#define M4_AXIS AXIS_A +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 360 // degrees per motor rev +#define M4_MICROSTEPS MOTOR_MICROSTEPS +#define M4_REVERSE false +#define M4_POWER_MODE MOTOR_POWER_MODE + + +// Switch settings. See switch.c +#define SWITCH_INTLVL PORT_INT0LVL_MED_gc +#define SW_LOCKOUT_TICKS 250 // ms +#define SW_DEGLITCH_TICKS 30 // ms + + +// Machine settings +//#define STEP_CORRECTION // Enable step correction +#define MAX_STEP_CORRECTION 4 // In steps per segment +#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arcs +#define JERK_MAX 5 // yes, that's km/min^3 +#define JUNCTION_DEVIATION 0.05 // default value, in mm +#define JUNCTION_ACCELERATION 100000 // centripetal corner accel +#define JOG_JERK_MULT 4 // Jogging jerk multipler +#define JOG_MIN_VELOCITY 10 // mm/min +#define CAL_ACCELERATION 500000 // mm/min^2 + +// Axis settings +#define VELOCITY_MAX 13000 // mm/min +#define FEEDRATE_MAX VELOCITY_MAX + +#define X_VELOCITY_MAX VELOCITY_MAX // G0 max velocity in mm/min +#define X_FEEDRATE_MAX FEEDRATE_MAX // G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // minimum travel for soft limits +#define X_TRAVEL_MAX 350 // between switches or crashes +#define X_JERK_MAX JERK_MAX +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define X_SEARCH_VELOCITY 2400 // move in negative direction +#define X_LATCH_VELOCITY 100 // mm/min +#define X_LATCH_BACKOFF 5 // mm +#define X_ZERO_BACKOFF 1 // mm +#define X_HOMING_MODE HOMING_STALL_MAX + +#define Y_VELOCITY_MAX VELOCITY_MAX +#define Y_FEEDRATE_MAX FEEDRATE_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 350 +#define Y_JERK_MAX JERK_MAX +#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Y_SEARCH_VELOCITY 3000 +#define Y_LATCH_VELOCITY 100 +#define Y_LATCH_BACKOFF 5 +#define Y_ZERO_BACKOFF 1 +#define Y_HOMING_MODE HOMING_STALL_MAX + +#define Z_VELOCITY_MAX 2000 // VELOCITY_MAX +#define Z_FEEDRATE_MAX FEEDRATE_MAX +#define Z_TRAVEL_MIN 0 +#define Z_TRAVEL_MAX 75 +#define Z_JERK_MAX JERK_MAX +#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Z_SEARCH_VELOCITY 400 +#define Z_LATCH_VELOCITY 100 +#define Z_LATCH_BACKOFF 5 +#define Z_ZERO_BACKOFF 1 +#define Z_HOMING_MODE HOMING_STALL_MAX + +// A values are chosen to make the A motor react the same as X for testing +// set to the same speed as X axis +#define A_VELOCITY_MAX (X_VELOCITY_MAX / M1_TRAVEL_PER_REV * 360) +#define A_FEEDRATE_MAX A_VELOCITY_MAX +#define A_TRAVEL_MIN -1 +#define A_TRAVEL_MAX -1 // same value means infinite +#define A_JERK_MAX (X_JERK_MAX * 360 / M1_TRAVEL_PER_REV) +#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define A_RADIUS (M1_TRAVEL_PER_REV / 2 / M_PI) +#define A_SEARCH_VELOCITY 600 +#define A_LATCH_VELOCITY 100 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 +#define A_HOMING_MODE HOMING_DISABLED + + +// Spindle settings +#define SPINDLE_TYPE SPINDLE_TYPE_HUANYANG +#define SPINDLE_PWM_FREQUENCY 100 // in Hz +#define SPINDLE_MIN_RPM 1000 +#define SPINDLE_MAX_RPM 24000 +#define SPINDLE_MIN_DUTY 0.05 +#define SPINDLE_MAX_DUTY 0.99 +#define SPINDLE_REVERSE false + + +// Gcode defaults +#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES +#define GCODE_DEFAULT_PLANE PLANE_XY // See machine.h +#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58, G59 +#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS +#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE +#define GCODE_DEFAULT_ARC_DISTANCE_MODE INCREMENTAL_MODE + + +// Motor ISRs +#define STALL_ISR_vect PORTA_INT1_vect +#define FAULT_ISR_vect PORTF_INT1_vect + + +/* Interrupt usage: + * + * HI Stepper timers (set in stepper.h) + * LO Segment execution SW interrupt (set in stepper.h) + * MED GPIO1 switch port (set in gpio.h) + * MED Serial RX (set in usart.c) + * MED Serial TX (set in usart.c) (* see note) + * LO Real time clock interrupt (set in xmega_rtc.h) + * + * (*) The TX cannot run at LO level or exception reports and other prints + * called from a LO interrupt (as in prep_line()) will kill the system + * in a permanent loop call in usart_putc() (usart.c). + */ + +// Timer assignments - see specific modules for details +// TCC1 free +#define TIMER_STEP TCC0 // Step timer (see stepper.h) +#define TIMER_PWM TCD1 // PWM timer (see pwm_spindle.c) + +#define M1_TIMER TCD0 +#define M2_TIMER TCE0 +#define M3_TIMER TCF0 +#define M4_TIMER TCE1 + +#define M1_DMA_CH DMA.CH0 +#define M2_DMA_CH DMA.CH1 +#define M3_DMA_CH DMA.CH2 +#define M4_DMA_CH DMA.CH3 + +#define M1_DMA_TRIGGER DMA_CH_TRIGSRC_TCD0_CCA_gc +#define M2_DMA_TRIGGER DMA_CH_TRIGSRC_TCE0_CCA_gc +#define M3_DMA_TRIGGER DMA_CH_TRIGSRC_TCF0_CCA_gc +#define M4_DMA_TRIGGER DMA_CH_TRIGSRC_TCE1_CCA_gc + + +// Timer setup for stepper and dwells +#define STEP_TIMER_DISABLE 0 +#define STEP_TIMER_ENABLE TC_CLKSEL_DIV4_gc +#define STEP_TIMER_DIV 4 +#define STEP_TIMER_FREQ (F_CPU / STEP_TIMER_DIV) +#define STEP_TIMER_POLL (STEP_TIMER_FREQ * 0.001) +#define STEP_TIMER_WGMODE TC_WGMODE_NORMAL_gc // count to TOP & rollover +#define STEP_TIMER_ISR TCC0_OVF_vect +#define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc + +#define MAX_SEGMENT_TIME ((float)0xffff / 60.0 / STEP_TIMER_FREQ) +#define NOM_SEGMENT_USEC 5000.0 // nominal segment time +#define MIN_SEGMENT_USEC 2500.0 // minimum segment time + + +// Huanyang settings +#define HUANYANG_PORT USARTD1 +#define HUANYANG_DRE_vect USARTD1_DRE_vect +#define HUANYANG_TXC_vect USARTD1_TXC_vect +#define HUANYANG_RXC_vect USARTD1_RXC_vect +#define HUANYANG_TIMEOUT 50 // ms. response timeout +#define HUANYANG_RETRIES 4 // Number of retries before failure +#define HUANYANG_ID 1 // Default ID + + +// Serial settings +#define SERIAL_BAUD USART_BAUD_115200 +#define SERIAL_PORT USARTC0 +#define SERIAL_DRE_vect USARTC0_DRE_vect +#define SERIAL_RXC_vect USARTC0_RXC_vect + + +// Input +#define INPUT_BUFFER_LEN 255 // text buffer size (255 max) + + +// Arc +#define ARC_RADIUS_ERROR_MAX 1.0 // max mm diff between start and end radius +#define ARC_RADIUS_ERROR_MIN 0.005 // min mm where 1% rule applies +#define ARC_RADIUS_TOLERANCE 0.001 // 0.1% radius variance test + + +// Planner +/// Should be at least the number of buffers required to support optimal +/// planning in the case of very short lines or arc segments. Suggest no less +/// than 12. Maximum is 255 with out also changing the type of mb.space. Must +/// leave headroom for stack. +#define PLANNER_BUFFER_POOL_SIZE 32 + +/// Buffers to reserve in planner before processing new input line +#define PLANNER_BUFFER_HEADROOM 4 + +/// Minimum number of filled buffers before timeout until execution starts +#define PLANNER_EXEC_MIN_FILL 4 + +/// Delay before executing new buffers unless PLANNER_EXEC_MIN_FILL is met +/// This gives the planner a chance to make a good plan before execution starts +#define PLANNER_EXEC_DELAY 250 // In ms + + +// I2C +#define I2C_DEV TWIC +#define I2C_ISR TWIC_TWIS_vect +#define I2C_ADDR 0x2b +#define I2C_MAX_DATA 8 diff --git a/avr/src/coolant.c b/avr/src/coolant.c new file mode 100644 index 0000000..29cac81 --- /dev/null +++ b/avr/src/coolant.c @@ -0,0 +1,55 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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) { + if (x) OUTCLR_PIN(SWITCH_1_PIN); + else OUTSET_PIN(SWITCH_1_PIN); +} + + +void coolant_set_flood(bool x) { + if (x) OUTCLR_PIN(SWITCH_2_PIN); + else OUTSET_PIN(SWITCH_2_PIN); +} + + +bool coolant_get_mist() {return OUT_PIN(SWITCH_1_PIN);} +bool coolant_get_flood() {return OUT_PIN(SWITCH_2_PIN);} diff --git a/avr/src/coolant.h b/avr/src/coolant.h new file mode 100644 index 0000000..f29166f --- /dev/null +++ b/avr/src/coolant.h @@ -0,0 +1,37 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..c7abc59 --- /dev/null +++ b/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/avr/src/drv8711.c b/avr/src/drv8711.c new file mode 100644 index 0000000..f55d811 --- /dev/null +++ b/avr/src/drv8711.c @@ -0,0 +1,401 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "motor.h" + +#include +#include + +#include +#include +#include + + +#define DRIVERS MOTORS +#define COMMANDS 10 + + +#define DRV8711_WORD_BYTE_PTR(WORD, LOW) \ + (((uint8_t *)&(WORD)) + ((LOW) ? 0 : 1)) + + +bool motor_fault = false; + + +typedef struct { + uint8_t status; + + bool active; + float idle_current; + float drive_current; + float stall_threshold; + float power; + + uint8_t mode; // microstepping mode + stall_callback_t stall_cb; + + uint8_t cs_pin; + uint8_t stall_pin; +} drv8711_driver_t; + + +static drv8711_driver_t drivers[DRIVERS] = { + {.cs_pin = SPI_CS_X_PIN, .stall_pin = STALL_X_PIN}, + {.cs_pin = SPI_CS_Y_PIN, .stall_pin = STALL_Y_PIN}, + {.cs_pin = SPI_CS_Z_PIN, .stall_pin = STALL_Z_PIN}, + {.cs_pin = SPI_CS_A_PIN, .stall_pin = STALL_A_PIN}, +}; + + +typedef struct { + uint8_t *read; + bool callback; + uint8_t disable_cs_pin; + + uint8_t cmd; + uint8_t driver; + bool low_byte; + + uint8_t ncmds; + uint16_t commands[DRIVERS][COMMANDS]; + uint16_t responses[DRIVERS]; +} spi_t; + +static spi_t spi = {0}; + + +static void _driver_check_status(int driver) { + uint8_t status = drivers[driver].status; + uint8_t mflags = 0; + + if (status & DRV8711_STATUS_OTS_bm) mflags |= MOTOR_FLAG_OVER_TEMP_bm; + if (status & DRV8711_STATUS_AOCP_bm) mflags |= MOTOR_FLAG_OVER_CURRENT_bm; + if (status & DRV8711_STATUS_BOCP_bm) mflags |= MOTOR_FLAG_OVER_CURRENT_bm; + if (status & DRV8711_STATUS_APDF_bm) mflags |= MOTOR_FLAG_DRIVER_FAULT_bm; + if (status & DRV8711_STATUS_BPDF_bm) mflags |= MOTOR_FLAG_DRIVER_FAULT_bm; + if (status & DRV8711_STATUS_UVLO_bm) mflags |= MOTOR_FLAG_UNDER_VOLTAGE_bm; + if (status & DRV8711_STATUS_STD_bm) mflags |= MOTOR_FLAG_STALLED_bm; + if (status & DRV8711_STATUS_STDLAT_bm) mflags |= MOTOR_FLAG_STALLED_bm; + + //if (mflags) motor_error_callback(driver, mflags); TODO +} + + +static float _driver_get_current(int driver) { + drv8711_driver_t *drv = &drivers[driver]; + +#if 1 + if (!drv->active) return drv->idle_current; + return + MOTOR_MIN_CURRENT + (drv->drive_current - MOTOR_MIN_CURRENT) * drv->power; + +#else + return drv->active ? drv->drive_current : drv->idle_current; +#endif +} + + +static uint8_t _spi_next_command(uint8_t cmd) { + // Process status responses + for (int driver = 0; driver < DRIVERS; driver++) { + uint16_t command = spi.commands[driver][cmd]; + + if (DRV8711_CMD_IS_READ(command) && + DRV8711_CMD_ADDR(command) == DRV8711_STATUS_REG) { + drivers[driver].status = spi.responses[driver]; + _driver_check_status(driver); + } + } + + // Next command + if (++cmd == spi.ncmds) { + cmd = 0; // Wrap around + + for (int driver = 0; driver < DRIVERS; driver++) + motor_driver_callback(driver); + } + + // Prep next command + for (int driver = 0; driver < DRIVERS; driver++) { + uint16_t *command = &spi.commands[driver][cmd]; + + switch (DRV8711_CMD_ADDR(*command)) { + case DRV8711_STATUS_REG: + if (!DRV8711_CMD_IS_READ(*command)) + *command = (*command & 0xf000) | (0x0fff & ~(drivers[driver].status)); + break; + + case DRV8711_TORQUE_REG: // Update motor current setting + *command = (*command & 0xff00) | + (uint8_t)round(0xff * _driver_get_current(driver)); + break; + + case DRV8711_CTRL_REG: // Set microsteps + *command = (*command & 0xff87) | (drivers[driver].mode << 3); + break; + + default: break; + } + } + + return cmd; +} + + +static void _spi_send() { + // Flush any status errors (TODO check for errors) + uint8_t x = SPIC.STATUS; + x = x; + + // Disable CS + if (spi.disable_cs_pin) { + OUTCLR_PIN(spi.disable_cs_pin); // Set low (inactive) + _delay_us(1); + spi.disable_cs_pin = 0; + } + + // Schedule next CS disable or enable next CS now + if (spi.low_byte) spi.disable_cs_pin = drivers[spi.driver].cs_pin; + else { + OUTSET_PIN(drivers[spi.driver].cs_pin); // Set high (active) + _delay_us(1); + } + + // Read + if (spi.read) { + *spi.read = SPIC.DATA; + spi.read = 0; + } + + // Callback, passing current command index, and get next command index + if (spi.callback) { + spi.cmd = _spi_next_command(spi.cmd); + spi.callback = false; + } + + // Write byte and prep next read + SPIC.DATA = + *DRV8711_WORD_BYTE_PTR(spi.commands[spi.driver][spi.cmd], spi.low_byte); + spi.read = DRV8711_WORD_BYTE_PTR(spi.responses[spi.driver], spi.low_byte); + + // Check if WORD complete, go to next driver & check if command finished + if (spi.low_byte && ++spi.driver == DRIVERS) { + spi.driver = 0; // Wrap around + spi.callback = true; // Call back after last byte is read + } + + // Next byte + spi.low_byte = !spi.low_byte; +} + + +static void _init_spi_commands() { + // Setup SPI command sequence + for (int driver = 0; driver < DRIVERS; driver++) { + uint16_t *commands = spi.commands[driver]; + spi.ncmds = 0; + + // Set OFF + commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_OFF_REG, 12); + + // Set BLANK + commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_BLANK_REG, 0x80); + + // Set DECAY + commands[spi.ncmds++] = + DRV8711_WRITE(DRV8711_DECAY_REG, DRV8711_DECAY_DECMOD_AUTO_OPT | 6); + + // Set STALL + commands[spi.ncmds++] = + DRV8711_WRITE(DRV8711_STALL_REG, + DRV8711_STALL_SDCNT_2 | DRV8711_STALL_VDIV_8 | 200); + + // Set DRIVE + commands[spi.ncmds++] = + DRV8711_WRITE(DRV8711_DRIVE_REG, DRV8711_DRIVE_IDRIVEP_50 | + DRV8711_DRIVE_IDRIVEN_100 | DRV8711_DRIVE_TDRIVEP_500 | + DRV8711_DRIVE_TDRIVEN_500 | DRV8711_DRIVE_OCPDEG_2 | + DRV8711_DRIVE_OCPTH_500); + + // Set TORQUE + commands[spi.ncmds++] = + DRV8711_WRITE(DRV8711_TORQUE_REG, DRV8711_TORQUE_SMPLTH_50); + + // Set CTRL enable motor & set ISENSE gain + commands[spi.ncmds++] = + DRV8711_WRITE(DRV8711_CTRL_REG, DRV8711_CTRL_ENBL_bm | + DRV8711_CTRL_ISGAIN_10 | DRV8711_CTRL_DTIME_850); + + // Read STATUS + commands[spi.ncmds++] = DRV8711_READ(DRV8711_STATUS_REG); + + // Clear STATUS + commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_STATUS_REG, 0); + } + + if (COMMANDS < spi.ncmds) + STATUS_ERROR(STAT_INTERNAL_ERROR, + "SPI command buffer overflow increase COMMANDS in %s", + __FILE__); + + _spi_send(); // Kick it off +} + + +ISR(SPIC_INT_vect) {_spi_send();} + + +ISR(STALL_ISR_vect) { + for (int i = 0; i < DRIVERS; i++) { + drv8711_driver_t *driver = &drivers[i]; + if (driver->stall_cb) driver->stall_cb(i); + } +} + + +ISR(FAULT_ISR_vect) {motor_fault = !IN_PIN(MOTOR_FAULT_PIN);} // TODO + + +void drv8711_init() { + // Configure drivers + for (int i = 0; i < DRIVERS; i++) { + drivers[i].idle_current = MOTOR_IDLE_CURRENT; + drivers[i].drive_current = MOTOR_MAX_CURRENT; + drivers[i].stall_threshold = MOTOR_STALL_THRESHOLD; + + drv8711_disable(i); + } + + // Setup pins + // Must set the SS pin either in/high or any/output for master mode to work + // Note, this pin is also used by the USART as the CTS line + DIRSET_PIN(SPI_SS_PIN); // Output + OUTSET_PIN(SPI_CLK_PIN); // High + DIRSET_PIN(SPI_CLK_PIN); // Output + DIRCLR_PIN(SPI_MISO_PIN); // Input + OUTSET_PIN(SPI_MOSI_PIN); // High + DIRSET_PIN(SPI_MOSI_PIN); // Output + + for (int i = 0; i < DRIVERS; i++) { + uint8_t cs_pin = drivers[i].cs_pin; + uint8_t stall_pin = drivers[i].stall_pin; + + OUTSET_PIN(cs_pin); // High + DIRSET_PIN(cs_pin); // Output + DIRCLR_PIN(stall_pin); // Input + + // Stall interrupt + PINCTRL_PIN(stall_pin) = PORT_ISC_FALLING_gc; + PORT(stall_pin)->INT1MASK |= BM(stall_pin); + PORT(stall_pin)->INTCTRL |= PORT_INT1LVL_HI_gc; + } + + // Fault interrupt + DIRCLR_PIN(MOTOR_FAULT_PIN); + PINCTRL_PIN(MOTOR_FAULT_PIN) = PORT_ISC_RISING_gc; + PORT(MOTOR_FAULT_PIN)->INT1MASK |= BM(MOTOR_FAULT_PIN); + PORT(MOTOR_FAULT_PIN)->INTCTRL |= PORT_INT1LVL_HI_gc; + + // Configure SPI + PR.PRPC &= ~PR_SPI_bm; // Disable power reduction + SPIC.CTRL = SPI_ENABLE_bm | SPI_MASTER_bm | SPI_MODE_0_gc | + SPI_PRESCALER_DIV16_gc; // enable, big endian, master, mode, clock div + PORT(SPI_CLK_PIN)->REMAP = PORT_SPI_bm; // Swap SCK and MOSI + SPIC.INTCTRL = SPI_INTLVL_LO_gc; // interupt level + + _init_spi_commands(); +} + + +void drv8711_enable(int driver) { + if (driver < 0 || DRIVERS <= driver) return; + drivers[driver].active = true; +} + + +void drv8711_disable(int driver) { + if (driver < 0 || DRIVERS <= driver) return; + drivers[driver].active = false; +} + + +void drv8711_set_power(int driver, float power) { + if (driver < 0 || DRIVERS <= driver) return; + drivers[driver].power = power < 0 ? 0 : (1 < power ? 1 : power); +} + + +void drv8711_set_microsteps(int driver, uint16_t msteps) { + if (driver < 0 || DRIVERS <= driver) return; + switch (msteps) { + case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256: + break; + default: return; // Invalid + } + + drivers[driver].mode = round(logf(msteps) / logf(2)); +} + + +void drv8711_set_stall_callback(int driver, stall_callback_t cb) { + drivers[driver].stall_cb = cb; +} + + +float get_drive_power(int driver) { + if (driver < 0 || DRIVERS <= driver) return 0; + return drivers[driver].drive_current; +} + + +void set_drive_power(int driver, float value) { + if (driver < 0 || DRIVERS <= driver || value < 0 || 1 < value) return; + drivers[driver].drive_current = value; +} + + +float get_idle_power(int driver) { + if (driver < 0 || DRIVERS <= driver) return 0; + return drivers[driver].idle_current; +} + + +void set_idle_power(int driver, float value) { + if (driver < 0 || DRIVERS <= driver || value < 0 || 1 < value) return; + drivers[driver].idle_current = value; +} + + +float get_current_power(int driver) { + if (driver < 0 || DRIVERS <= driver) return 0; + return _driver_get_current(driver); +} + + +bool get_motor_fault() {return motor_fault;} diff --git a/avr/src/drv8711.h b/avr/src/drv8711.h new file mode 100644 index 0000000..187c88a --- /dev/null +++ b/avr/src/drv8711.h @@ -0,0 +1,169 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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, +}; + + +#define DRV8711_READ(ADDR) ((1 << 15) | ((ADDR) << 12)) +#define DRV8711_WRITE(ADDR, DATA) (((ADDR) << 12) | ((DATA) & 0xfff)) +#define DRV8711_CMD_ADDR(CMD) (((CMD) >> 12) & 7) +#define DRV8711_CMD_IS_READ(CMD) ((1 << 15) & (CMD)) + + +void drv8711_init(); +void drv8711_enable(int driver); +void drv8711_disable(int driver); +void drv8711_set_power(int driver, float power); +void drv8711_set_microsteps(int driver, uint16_t msteps); +void drv8711_set_stall_callback(int driver, stall_callback_t cb); diff --git a/avr/src/estop.c b/avr/src/estop.c new file mode 100644 index 0000000..5f533ae --- /dev/null +++ b/avr/src/estop.c @@ -0,0 +1,147 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "homing.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 + if (estop.triggered) OUTSET_PIN(FAULT_PIN); // High + else OUTCLR_PIN(FAULT_PIN); // Low + DIRSET_PIN(FAULT_PIN); // Output +} + + +bool estop_triggered() { + return estop.triggered || switch_is_active(SW_ESTOP); +} + + +void estop_trigger(stat_t reason) { + if (estop.triggered) return; + estop.triggered = true; + + // Hard stop the motors and the spindle + st_shutdown(); + spindle_estop(); + + // Set machine state + mp_state_estop(); + + // Set axes not homed + mach_set_not_homed(); + + // Save reason + _set_reason(reason); + + report_request(); +} + + +void estop_clear() { + // Check if estop switch is set + if (switch_is_active(SW_ESTOP)) { + if (_get_reason() != STAT_ESTOP_SWITCH) _set_reason(STAT_ESTOP_SWITCH); + return; // Can't clear while estop switch is still active + } + + // Clear fault signal + OUTCLR_PIN(FAULT_PIN); // Low + + estop.triggered = false; + + // Clear reason + _set_reason(STAT_OK); + + // Reboot + // Note, hardware.c waits until any spindle stop command has been delivered + hw_request_hard_reset(); +} + + +bool get_estop() { + return estop_triggered(); +} + + +void set_estop(bool value) { + if (value == estop_triggered()) return; + if (value) estop_trigger(STAT_ESTOP_USER); + else estop_clear(); +} + + +PGM_P get_estop_reason() { + return status_to_pgmstr(_get_reason()); +} diff --git a/avr/src/estop.h b/avr/src/estop.h new file mode 100644 index 0000000..55fdec4 --- /dev/null +++ b/avr/src/estop.h @@ -0,0 +1,38 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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_parser.c b/avr/src/gcode_parser.c new file mode 100644 index 0000000..0803507 --- /dev/null +++ b/avr/src/gcode_parser.c @@ -0,0 +1,569 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "machine.h" +#include "plan/arc.h" +#include "probing.h" +#include "homing.h" +#include "axis.h" +#include "util.h" + +#include +#include +#include +#include +#include + + +parser_t parser = {{0}}; + + +#define SET_MODAL(m, parm, val) \ + {parser.gn.parm = val; parser.gf.parm = true; modals[m] += 1; break;} +#define SET_NON_MODAL(parm, val) \ + {parser.gn.parm = val; parser.gf.parm = true; break;} +#define EXEC_FUNC(f, parm) if (parser.gf.parm) f(parser.gn.parm) + + +static uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block + + +/* Normalize a block (line) of gcode in place + * + * Normalization functions: + * - convert all letters to upper case + * - remove white space, control and other invalid characters + * - remove (erroneous) leading zeros that might be taken to mean Octal + * - identify and return start of comments and messages + * - signal if a block-delete character (/) was encountered in the first space + * + * So this: " g1 x100 Y100 f400" becomes this: "G1X100Y100F400" + * + * Comment and message handling: + * - Comments field start with a '(' char or alternately a semicolon ';' + * - Comments and messages are not normalized - they are left alone + * - The 'MSG' specifier in comment can have mixed case but cannot cannot + * have embedded white spaces + * - Normalization returns true if there was a message to display, false + * otherwise + * - Comments always terminate the block - i.e. leading or embedded comments + * are not supported + * - Valid cases (examples) Notes: + * G0X10 - command only - no comment + * (comment text) - There is no command on this line + * G0X10 (comment text) + * G0X10 (comment text - It's OK to drop the trailing paren + * G0X10 ;comment text - It's OK to drop the trailing paren + * + * - Invalid cases (examples) Notes: + * G0X10 comment text - Comment with no separator + * N10 (comment) G0X10 - embedded comment. G0X10 will be ignored + * (comment) G0X10 - leading comment. G0X10 will be ignored + * G0X10 # comment - invalid separator + * + * Returns: + * - com points to comment string or to 0 if no comment + * - msg points to message string or to 0 if no comment + * - block_delete_flag is set true if block delete encountered, false otherwise + */ + +static void _normalize_gcode_block(char *str, char **com, char **msg, + uint8_t *block_delete_flag) { + char *rd = str; // read pointer + char *wr = str; // write pointer + + // mark block deletes + *block_delete_flag = *rd == '/'; + + // normalize the command block & find the comment (if any) + for (; *wr; rd++) + if (!*rd) *wr = 0; + else if (*rd == '(' || *rd == ';') {*wr = 0; *com = rd + 1;} + else if (isalnum(*rd) || strchr("-.", *rd)) // all valid characters + *wr++ = toupper(*rd); + + // Perform Octal stripping - remove invalid leading zeros in number strings + rd = str; + while (*rd) { + if (*rd == '.') break; // don't strip past a decimal point + if (!isdigit(*rd) && *(rd + 1) == '0' && isdigit(*(rd + 2))) { + wr = rd + 1; + while (*wr) {*wr = *(wr + 1); wr++;} // copy forward w/overwrite + continue; + } + + rd++; + } + + // process comments and messages + if (**com) { + rd = *com; + while (isspace(*rd)) rd++; // skip any leading spaces before "msg" + + if (tolower(*rd) == 'm' && tolower(*(rd + 1)) == 's' && + tolower(*(rd + 2)) == 'g') + *msg = rd + 3; + + for (; *rd; rd++) + // 0 terminate on trailing parenthesis, if any + if (*rd == ')') *rd = 0; + } +} + + +/* Get gcode word consisting of a letter and a value + * + * This function requires the Gcode string to be normalized. + * Normalization must remove any leading zeros or they will be converted to + * octal. G0X... is not interpreted as hexadecimal. This is trapped. + */ +static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) { + if (!**pstr) return STAT_COMPLETE; // no more words to process + + // get letter part + if (!isupper(**pstr)) return STAT_INVALID_OR_MALFORMED_COMMAND; + *letter = **pstr; + (*pstr)++; + + // X-axis-becomes-a-hexadecimal-number get-value case, e.g. G0X100 --> G255 + if (**pstr == '0' && *(*pstr + 1) == 'X') { + *value = 0; + (*pstr)++; + return STAT_OK; // pointer points to X + } + + // get-value general case + char *end; + *value = strtod(*pstr, &end); + // more robust test then checking for value == 0 + if (end == *pstr) return STAT_GCODE_COMMAND_UNSUPPORTED; + *pstr = end; // pointer points to next character after the word + + return STAT_OK; +} + + +/// Isolate the decimal point value as an integer +static uint8_t _point(float value) {return value * 10 - trunc(value) * 10;} + + +#if 0 +static bool _axis_changed() { + for (int axis = 0; axis < AXES; axis++) + if (parser.gf.target[axis]) return true; + return false; +} +#endif + + +/// Check for some gross Gcode block semantic violations +static stat_t _validate_gcode_block() { + // Check for modal group violations. From NIST, section 3.4 "It + // is an error to put a G-code from group 1 and a G-code from + // group 0 on the same line if both of them use axis words. If an + // axis word-using G-code from group 1 is implicitly in effect on + // a line (by having been activated on an earlier line), and a + // group 0 G-code that uses axis words appears on the line, the + // activity of the group 1 G-code is suspended for that line. The + // axis word-using G-codes from group 0 are G10, G28, G30, and G92" + + if (modals[MODAL_GROUP_G0] && modals[MODAL_GROUP_G1]) + return STAT_MODAL_GROUP_VIOLATION; + +#if 0 // This check fails for arcs which may have offsets but no axis word + // look for commands that require an axis word to be present + if (modals[MODAL_GROUP_G0] || modals[MODAL_GROUP_G1]) + if (!_axis_changed()) return STAT_GCODE_AXIS_IS_MISSING; +#endif + + return STAT_OK; +} + + +/* Execute parsed block + * + * Conditionally (based on whether a flag is set in gf) call the + * machining functions in order of execution as per RS274NGC_3 table 8 + * (below, with modifications): + * + * 0. record the line number + * 1. comment (includes message) [handled during block normalization] + * 2. set feed rate mode (G93, G94 - inverse time or per minute) + * 3. set feed rate (F) + * 3a. set feed override rate (M50) + * 4. set spindle speed (S) + * 4a. set spindle override rate (M51) + * 5. select tool (T) + * 6. change tool (M6) + * 7. spindle on or off (M3, M4, M5) + * 8. coolant on or off (M7, M8, M9) + * 9. enable or disable overrides (M48, M49) + * 10. dwell (G4) + * 11. set active plane (G17, G18, G19) + * 12. set length units (G20, G21) + * 13. cutter radius compensation on or off (G40, G41, G42) + * 14. cutter length compensation on or off (G43, G49) + * 15. coordinate system selection (G54, G55, G56, G57, G58, G59) + * 16. set path control mode (G61, G61.1, G64) + * 17. set distance mode (G90, G91, G90.1, G91.1) + * 18. set retract mode (G98, G99) + * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30) + * 19b. update system data (G10) + * 19c. set axis offsets (G92, G92.1, G92.2, G92.3) + * 20. perform motion (G0 to G3, G80-G89) as modified (possibly) by G53 + * 21. stop and end (M0, M1, M2, M30, M60) + * + * Values in gn are in original units and should not be unit converted prior + * to calling the machine functions (which does the unit conversions) + */ +static stat_t _execute_gcode_block() { + stat_t status = STAT_OK; + + mach_set_line(parser.gn.line); + EXEC_FUNC(mach_set_feed_mode, feed_mode); + EXEC_FUNC(mach_set_feed_rate, feed_rate); + EXEC_FUNC(mach_feed_override_enable, feed_override_enable); + EXEC_FUNC(mach_set_spindle_speed, spindle_speed); + EXEC_FUNC(mach_spindle_override_enable, spindle_override_enable); + EXEC_FUNC(mach_select_tool, tool); + EXEC_FUNC(mach_change_tool, tool_change); + EXEC_FUNC(mach_set_spindle_mode, spindle_mode); + EXEC_FUNC(mach_mist_coolant_control, mist_coolant); + EXEC_FUNC(mach_flood_coolant_control, flood_coolant); + EXEC_FUNC(mach_override_enables, override_enables); + + if (parser.gn.next_action == NEXT_ACTION_DWELL) // G4 - dwell + RITORNO(mach_dwell(parser.gn.parameter)); + + EXEC_FUNC(mach_set_plane, plane); + EXEC_FUNC(mach_set_units, units); + //--> cutter radius compensation goes here + //--> cutter length compensation goes here + EXEC_FUNC(mach_set_coord_system, coord_system); + EXEC_FUNC(mach_set_path_mode, path_mode); + EXEC_FUNC(mach_set_distance_mode, distance_mode); + EXEC_FUNC(mach_set_arc_distance_mode, arc_distance_mode); + //--> set retract mode goes here + + switch (parser.gn.next_action) { + case NEXT_ACTION_SET_G28_POSITION: // G28.1 + mach_set_g28_position(); + break; + case NEXT_ACTION_GOTO_G28_POSITION: // G28 + status = mach_goto_g28_position(parser.gn.target, parser.gf.target); + break; + case NEXT_ACTION_SET_G30_POSITION: // G30.1 + mach_set_g30_position(); + break; + case NEXT_ACTION_GOTO_G30_POSITION: // G30 + status = mach_goto_g30_position(parser.gn.target, parser.gf.target); + break; + case NEXT_ACTION_SEARCH_HOME: // G28.2 + mach_homing_cycle_start(); + break; + case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: // G28.3 + mach_set_absolute_origin(parser.gn.target, parser.gf.target); + break; + case NEXT_ACTION_HOMING_NO_SET: // G28.4 + mach_homing_cycle_start_no_set(); + break; + case NEXT_ACTION_STRAIGHT_PROBE: // G38.2 + status = mach_probe(parser.gn.target, parser.gf.target); + break; + case NEXT_ACTION_SET_COORD_DATA: + mach_set_coord_offsets(parser.gn.parameter, parser.gn.target, + parser.gf.target); + break; + case NEXT_ACTION_SET_ORIGIN_OFFSETS: + mach_set_origin_offsets(parser.gn.target, parser.gf.target); + break; + case NEXT_ACTION_RESET_ORIGIN_OFFSETS: + mach_reset_origin_offsets(); + break; + case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: + mach_suspend_origin_offsets(); + break; + case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: + mach_resume_origin_offsets(); + break; + case NEXT_ACTION_DWELL: break; // Handled above + + case NEXT_ACTION_DEFAULT: + // apply override setting to gm struct + mach_set_absolute_mode(parser.gn.absolute_mode); + + switch (parser.gn.motion_mode) { + case MOTION_MODE_CANCEL_MOTION_MODE: + mach_set_motion_mode(parser.gn.motion_mode); + break; + case MOTION_MODE_RAPID: + status = mach_rapid(parser.gn.target, parser.gf.target); + break; + case MOTION_MODE_FEED: + status = mach_feed(parser.gn.target, parser.gf.target); + break; + case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: + // gf.radius sets radius mode if radius was collected in gn + status = mach_arc_feed(parser.gn.target, parser.gf.target, + parser.gn.arc_offset, parser.gf.arc_offset, + parser.gn.arc_radius, parser.gf.arc_radius, + parser.gn.parameter, parser.gf.parameter, + modals[MODAL_GROUP_G1], parser.gn.motion_mode); + break; + default: break; // Should not get here + } + } + // un-set absolute override once the move is planned + mach_set_absolute_mode(false); + + // do the program stops and ends : M0, M1, M2, M30, M60 + if (parser.gf.program_flow) + switch (parser.gn.program_flow) { + case PROGRAM_STOP: mach_program_stop(); break; + case PROGRAM_OPTIONAL_STOP: mach_optional_program_stop(); break; + case PROGRAM_PALLET_CHANGE_STOP: mach_pallet_change_stop(); break; + case PROGRAM_END: mach_program_end(); break; + } + + return status; +} + + +/* Parses one line of 0 terminated G-Code. + * + * All the parser does is load the state values in gn (next model + * state) and set flags in gf (model state flags). The execute + * routine applies them. The buffer is assumed to contain only + * uppercase characters and signed floats (no whitespace). + * + * A number of implicit things happen when the gn struct is zeroed: + * - inverse feed rate mode is canceled - set back to units_per_minute mode + */ +static stat_t _parse_gcode_block(char *buf) { + char *pstr = buf; // persistent pointer for parsing words + char letter; // parsed letter, eg.g. G or X or Y + float value = 0; // value parsed from letter (e.g. 2 for G2) + stat_t status = STAT_OK; + + // set initial state for new move + memset(modals, 0, sizeof(modals)); // clear all parser values + memset(&parser.gf, 0, sizeof(gcode_flags_t)); // clear all next-state flags + memset(&parser.gn, 0, sizeof(gcode_state_t)); // clear all next-state values + + // get motion mode from previous block + parser.gn.motion_mode = mach_get_motion_mode(); + + // extract commands and parameters + while ((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) { + switch (letter) { + case 'G': + switch ((uint8_t)value) { + case 0: + SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_RAPID); + case 1: + SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_FEED); + case 2: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC); + case 3: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC); + case 4: SET_NON_MODAL(next_action, NEXT_ACTION_DWELL); + case 10: + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA); + case 17: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XY); + case 18: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XZ); + case 19: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_YZ); + case 20: SET_MODAL(MODAL_GROUP_G6, units, INCHES); + case 21: SET_MODAL(MODAL_GROUP_G6, units, MILLIMETERS); + case 28: + switch (_point(value)) { + case 0: + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G28_POSITION); + case 1: + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G28_POSITION); + case 2: SET_NON_MODAL(next_action, NEXT_ACTION_SEARCH_HOME); + case 3: SET_NON_MODAL(next_action, NEXT_ACTION_SET_ABSOLUTE_ORIGIN); + case 4: SET_NON_MODAL(next_action, NEXT_ACTION_HOMING_NO_SET); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 30: + switch (_point(value)) { + case 0: + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G30_POSITION); + case 1: + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G30_POSITION); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 38: + switch (_point(value)) { + case 2: SET_NON_MODAL(next_action, NEXT_ACTION_STRAIGHT_PROBE); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 40: break; // ignore cancel cutter radius compensation + case 49: break; // ignore cancel tool length offset comp. + case 53: SET_NON_MODAL(absolute_mode, true); + case 54: SET_MODAL(MODAL_GROUP_G12, coord_system, G54); + case 55: SET_MODAL(MODAL_GROUP_G12, coord_system, G55); + case 56: SET_MODAL(MODAL_GROUP_G12, coord_system, G56); + case 57: SET_MODAL(MODAL_GROUP_G12, coord_system, G57); + case 58: SET_MODAL(MODAL_GROUP_G12, coord_system, G58); + case 59: SET_MODAL(MODAL_GROUP_G12, coord_system, G59); + case 61: + switch (_point(value)) { + case 0: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_PATH); + case 1: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_STOP); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 64: SET_MODAL(MODAL_GROUP_G13,path_mode, PATH_CONTINUOUS); + case 80: SET_MODAL(MODAL_GROUP_G1, motion_mode, + MOTION_MODE_CANCEL_MOTION_MODE); + // case 90: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); + // case 91: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); + case 90: + switch (_point(value)) { + case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); + case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, ABSOLUTE_MODE); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 91: + switch (_point(value)) { + case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); + case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, INCREMENTAL_MODE); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 92: + switch (_point(value)) { + case 0: SET_MODAL(MODAL_GROUP_G0, next_action, + NEXT_ACTION_SET_ORIGIN_OFFSETS); + case 1: SET_NON_MODAL(next_action, NEXT_ACTION_RESET_ORIGIN_OFFSETS); + case 2: SET_NON_MODAL(next_action, NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS); + case 3: SET_NON_MODAL(next_action, NEXT_ACTION_RESUME_ORIGIN_OFFSETS); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 93: SET_MODAL(MODAL_GROUP_G5, feed_mode, INVERSE_TIME_MODE); + case 94: SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_MINUTE_MODE); + // case 95: + // SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_REVOLUTION_MODE); + // case 96: // Spindle Constant Surface Speed (not currently supported) + case 97: break; // Spindle RPM mode (only mode curently supported) + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 'M': + switch ((uint8_t)value) { + case 0: + SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_STOP); + case 1: + SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_OPTIONAL_STOP); + case 60: + SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_PALLET_CHANGE_STOP); + case 2: case 30: + SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_END); + case 3: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_CW); + case 4: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_CCW); + case 5: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_OFF); + case 6: SET_NON_MODAL(tool_change, true); + case 7: SET_MODAL(MODAL_GROUP_M8, mist_coolant, true); + case 8: SET_MODAL(MODAL_GROUP_M8, flood_coolant, true); + case 9: SET_MODAL(MODAL_GROUP_M8, flood_coolant, false); // Also mist + case 48: SET_MODAL(MODAL_GROUP_M9, override_enables, true); + case 49: SET_MODAL(MODAL_GROUP_M9, override_enables, false); + case 50: SET_MODAL(MODAL_GROUP_M9, feed_override_enable, true); + case 51: SET_MODAL(MODAL_GROUP_M9, spindle_override_enable, true); + default: status = STAT_MCODE_COMMAND_UNSUPPORTED; + } + break; + + case 'T': SET_NON_MODAL(tool, (uint8_t)trunc(value)); + case 'F': SET_NON_MODAL(feed_rate, value); + // used for dwell time, G10 coord select, rotations + case 'P': SET_NON_MODAL(parameter, value); + case 'S': SET_NON_MODAL(spindle_speed, value); + case 'X': SET_NON_MODAL(target[AXIS_X], value); + case 'Y': SET_NON_MODAL(target[AXIS_Y], value); + case 'Z': SET_NON_MODAL(target[AXIS_Z], value); + case 'A': SET_NON_MODAL(target[AXIS_A], value); + case 'B': SET_NON_MODAL(target[AXIS_B], value); + case 'C': SET_NON_MODAL(target[AXIS_C], value); + // case 'U': SET_NON_MODAL(target[AXIS_U], value); // reserved + // case 'V': SET_NON_MODAL(target[AXIS_V], value); // reserved + // case 'W': SET_NON_MODAL(target[AXIS_W], value); // reserved + case 'I': SET_NON_MODAL(arc_offset[0], value); + case 'J': SET_NON_MODAL(arc_offset[1], value); + case 'K': SET_NON_MODAL(arc_offset[2], value); + case 'R': SET_NON_MODAL(arc_radius, value); + case 'N': SET_NON_MODAL(line, (uint32_t)value); // line number + case 'L': break; // not used for anything + case 0: break; + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + + if (status != STAT_OK) break; + } + + if (status != STAT_OK && status != STAT_COMPLETE) return status; + RITORNO(_validate_gcode_block()); + + return _execute_gcode_block(); // if successful execute the block +} + + +/// Parse a block (line) of gcode +/// Top level of gcode parser. Normalizes block and looks for special cases +stat_t gc_gcode_parser(char *block) { + char *str = block; // gcode command or 0 string + char none = 0; + char *com = &none; // gcode comment or 0 string + char *msg = &none; // gcode message or 0 string + uint8_t block_delete_flag; + + _normalize_gcode_block(str, &com, &msg, &block_delete_flag); + + // Block delete omits the line if a / char is present in the first space + // For now this is unconditional and will always delete + if (block_delete_flag) return STAT_NOOP; + + // queue a "(MSG" response + if (*msg) mach_message(msg); // queue the message + + return _parse_gcode_block(block); +} diff --git a/avr/src/gcode_parser.h b/avr/src/gcode_parser.h new file mode 100644 index 0000000..4d729a0 --- /dev/null +++ b/avr/src/gcode_parser.h @@ -0,0 +1,67 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 struct { + gcode_state_t gn; // gcode input values + gcode_flags_t gf; // gcode input flags +} parser_t; + + +extern parser_t parser; + + +stat_t gc_gcode_parser(char *block); diff --git a/avr/src/gcode_state.c b/avr/src/gcode_state.c new file mode 100644 index 0000000..c368b29 --- /dev/null +++ b/avr/src/gcode_state.c @@ -0,0 +1,97 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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" + + +PGM_P gs_get_units_pgmstr(units_t mode) { + switch (mode) { + case INCHES: return PSTR("IN"); + case MILLIMETERS: return PSTR("MM"); + case DEGREES: return PSTR("DEG"); + } + + return PSTR("INVALID"); +} + + +PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode) { + switch (mode) { + case INVERSE_TIME_MODE: return PSTR("INVERSE TIME"); + case UNITS_PER_MINUTE_MODE: return PSTR("PER MIN"); + case UNITS_PER_REVOLUTION_MODE: return PSTR("PER REV"); + } + + return PSTR("INVALID"); +} + + +PGM_P gs_get_plane_pgmstr(plane_t plane) { + switch (plane) { + case PLANE_XY: return PSTR("XY"); + case PLANE_XZ: return PSTR("XZ"); + case PLANE_YZ: return PSTR("YZ"); + } + + return PSTR("INVALID"); +} + + +PGM_P gs_get_coord_system_pgmstr(coord_system_t cs) { + switch (cs) { + case ABSOLUTE_COORDS: return PSTR("ABS"); + case G54: return PSTR("G54"); + case G55: return PSTR("G55"); + case G56: return PSTR("G56"); + case G57: return PSTR("G57"); + case G58: return PSTR("G58"); + case G59: return PSTR("G59"); + } + + return PSTR("INVALID"); +} + + +PGM_P gs_get_path_mode_pgmstr(path_mode_t mode) { + switch (mode) { + case PATH_EXACT_PATH: return PSTR("EXACT PATH"); + case PATH_EXACT_STOP: return PSTR("EXACT STOP"); + case PATH_CONTINUOUS: return PSTR("CONTINUOUS"); + } + + return PSTR("INVALID"); +} + + +PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode) { + switch (mode) { + case ABSOLUTE_MODE: return PSTR("ABSOLUTE"); + case INCREMENTAL_MODE: return PSTR("INCREMENTAL"); + } + + return PSTR("INVALID"); +} diff --git a/avr/src/gcode_state.def b/avr/src/gcode_state.def new file mode 100644 index 0000000..aa4d042 --- /dev/null +++ b/avr/src/gcode_state.def @@ -0,0 +1,69 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..ff3525b --- /dev/null +++ b/avr/src/gcode_state.h @@ -0,0 +1,204 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + + +/* The difference between next_action_t and motion_mode_t is that + * next_action_t is used by the current block, and may carry non-modal + * commands, whereas motion_mode_t persists across blocks as G modal group 1 + */ + +/// these are in order to optimized CASE statement +typedef enum { + NEXT_ACTION_DEFAULT, // Must be zero (invokes motion modes) + NEXT_ACTION_SEARCH_HOME, // G28.2 homing cycle + NEXT_ACTION_SET_ABSOLUTE_ORIGIN, // G28.3 origin set + NEXT_ACTION_HOMING_NO_SET, // G28.4 homing cycle no coord setting + NEXT_ACTION_SET_G28_POSITION, // G28.1 set position in abs coordinates + NEXT_ACTION_GOTO_G28_POSITION, // G28 go to machine position + NEXT_ACTION_SET_G30_POSITION, // G30.1 + NEXT_ACTION_GOTO_G30_POSITION, // G30 + NEXT_ACTION_SET_COORD_DATA, // G10 + NEXT_ACTION_SET_ORIGIN_OFFSETS, // G92 + NEXT_ACTION_RESET_ORIGIN_OFFSETS, // G92.1 + NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS, // G92.2 + NEXT_ACTION_RESUME_ORIGIN_OFFSETS, // G92.3 + NEXT_ACTION_DWELL, // G4 + NEXT_ACTION_STRAIGHT_PROBE, // G38.2 +} next_action_t; + + +typedef enum { // G Modal Group 1 + MOTION_MODE_RAPID, // G0 - rapid + MOTION_MODE_FEED, // G1 - straight feed + MOTION_MODE_CW_ARC, // G2 - clockwise arc feed + MOTION_MODE_CCW_ARC, // G3 - counter-clockwise arc feed + MOTION_MODE_CANCEL_MOTION_MODE, // G80 + MOTION_MODE_STRAIGHT_PROBE, // G38.2 + MOTION_MODE_CANNED_CYCLE_81, // G81 - drilling + MOTION_MODE_CANNED_CYCLE_82, // G82 - drilling with dwell + MOTION_MODE_CANNED_CYCLE_83, // G83 - peck drilling + MOTION_MODE_CANNED_CYCLE_84, // G84 - right hand tapping + MOTION_MODE_CANNED_CYCLE_85, // G85 - boring, no dwell, feed out + MOTION_MODE_CANNED_CYCLE_86, // G86 - boring, spindle stop, rapid out + MOTION_MODE_CANNED_CYCLE_87, // G87 - back boring + MOTION_MODE_CANNED_CYCLE_88, // G88 - boring, spindle stop, manual out + MOTION_MODE_CANNED_CYCLE_89, // G89 - boring, dwell, feed out +} motion_mode_t; + + +typedef enum { // plane - translates to: + // axis_0 axis_1 axis_2 + PLANE_XY, // G17 X Y Z + PLANE_XZ, // G18 X Z Y + PLANE_YZ, // G19 Y Z X +} plane_t; + + +typedef enum { + INCHES, // G20 + MILLIMETERS, // G21 + DEGREES, // ABC axes (this value used for displays only) +} units_t; + + +typedef enum { + ABSOLUTE_COORDS, // machine coordinate system + G54, G55, G56, G57, G58, G59, +} coord_system_t; + + +/// G Modal Group 13 +typedef enum { + PATH_EXACT_PATH, // G61 hits corners but stops only if needed + PATH_EXACT_STOP, // G61.1 stops at all corners + PATH_CONTINUOUS, // G64 and typically the default mode +} path_mode_t; + + +typedef enum { + ABSOLUTE_MODE, // G90 + INCREMENTAL_MODE, // G91 +} distance_mode_t; + + +typedef enum { + INVERSE_TIME_MODE, // G93 + UNITS_PER_MINUTE_MODE, // G94 + UNITS_PER_REVOLUTION_MODE, // G95 (unimplemented) +} feed_mode_t; + + +typedef enum { + ORIGIN_OFFSET_SET, // G92 - set origin offsets + ORIGIN_OFFSET_CANCEL, // G92.1 - zero out origin offsets + ORIGIN_OFFSET_SUSPEND, // G92.2 - do not apply offsets, but preserve values + ORIGIN_OFFSET_RESUME, // G92.3 - resume application of the suspended offsets +} origin_offset_t; + + +typedef enum { + PROGRAM_STOP, + PROGRAM_OPTIONAL_STOP, + PROGRAM_PALLET_CHANGE_STOP, + PROGRAM_END, +} program_flow_t; + + +/// spindle state settings +typedef enum { + SPINDLE_OFF, + SPINDLE_CW, + SPINDLE_CCW, +} spindle_mode_t; + + +/// mist and flood coolant states +typedef enum { + COOLANT_OFF, // all coolant off + COOLANT_ON, // request coolant on or indicate both coolants are on + COOLANT_MIST, // indicates mist coolant on + COOLANT_FLOOD, // indicates flood coolant on +} coolant_state_t; + + +/// used for spindle and arc dir +typedef enum { + DIRECTION_CW, + DIRECTION_CCW, +} direction_t; + + +/* Gcode model + * + * - mach.gm is the core Gcode model state. It keeps the internal gcode + * state model in normalized, canonical form. All values are unit + * converted (to mm) and in the machine coordinate system + * (absolute coordinate system). Gm is owned by the machine layer and + * should be accessed only through mach_*() routines. + * + * - parser.gn is used by the gcode parser and is re-initialized for + * each gcode block. It accepts data in the new gcode block in the + * formats present in the block (pre-normalized forms). During + * initialization some state elements are necessarily restored + * from gm. + * + * - parser.gf is used by the gcode parser to hold flags for any data that has + * changed in gn during the parse. + */ + + +/// Gcode model state +typedef struct { +#define MEMBER(TYPE, VAR) TYPE VAR; +#include "gcode_state.def" +#undef MEMBER +} gcode_state_t; + + +typedef struct { +#define MEMBER(TYPE, VAR) bool VAR; +#include "gcode_state.def" +#undef MEMBER +} gcode_flags_t; + + +PGM_P gs_get_units_pgmstr(units_t mode); +PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode); +PGM_P gs_get_plane_pgmstr(plane_t plane); +PGM_P gs_get_coord_system_pgmstr(coord_system_t cs); +PGM_P gs_get_path_mode_pgmstr(path_mode_t mode); +PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode); diff --git a/avr/src/hardware.c b/avr/src/hardware.c new file mode 100644 index 0000000..65c5c8f --- /dev/null +++ b/avr/src/hardware.c @@ -0,0 +1,196 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 +#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] + + +/// This routine is lifted and modified from Boston Android and from +/// http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659 +static void _init_clock() { +#if defined(__CLOCK_EXTERNAL_8MHZ) // external 8 Mhx Xtal w/ 4x PLL = 32 Mhz + // 2-9 MHz crystal; 0.4-16 MHz XTAL w/ 16K CLK startup + OSC.XOSCCTRL = OSC_FRQRANGE_2TO9_gc | OSC_XOSCSEL_XTAL_16KCLK_gc; + OSC.CTRL = OSC_XOSCEN_bm; // enable external crystal oscillator + while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready + + OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 4; // PLL source, 4x (32 MHz sys clock) + OSC.CTRL = OSC_PLLEN_bm | OSC_XOSCEN_bm; // Enable PLL & External Oscillator + while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready + + CCP = CCP_IOREG_gc; + CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock + + OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock + +#elif defined(__CLOCK_EXTERNAL_16MHZ) // external 16Mhz Xtal w/ 2x PLL = 32 Mhz + // 12-16 MHz crystal; 0.4-16 MHz XTAL w/ 16K CLK startup + OSC.XOSCCTRL = OSC_FRQRANGE_12TO16_gc | OSC_XOSCSEL_XTAL_16KCLK_gc; + OSC.CTRL = OSC_XOSCEN_bm; // enable external crystal oscillator + while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready + + OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 2; // PLL source, 2x (32 MHz sys clock) + OSC.CTRL = OSC_PLLEN_bm | OSC_XOSCEN_bm; // Enable PLL & External Oscillator + while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready + + CCP = CCP_IOREG_gc; + CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock + + OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock + +#elif defined(__CLOCK_INTERNAL_32MHZ) // 32 MHz internal clock + OSC.CTRL = OSC_RC32MEN_bm; // enable internal 32MHz oscillator + while (!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for oscillator ready + + CCP = CCP_IOREG_gc; // Security Signature to modify clk + CLK.CTRL = CLK_SCLKSEL_RC32M_gc; // select sysclock 32MHz osc + +#else +#error No clock defined +#endif +} + + +static void _load_hw_id_byte(int i, register8_t *reg) { + NVM.CMD = NVM_CMD_READ_CALIB_ROW_gc; + uint8_t byte = pgm_read_byte(reg); + NVM.CMD = NVM_CMD_NO_OPERATION_gc; + + hw.id[i] = HEXNIB(byte >> 4); + hw.id[i + 1] = HEXNIB(byte); +} + + +static void _read_hw_id() { + int i = 0; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM5); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM4); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM3); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM2); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM1); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.LOTNUM0); i += 2; + hw.id[i++] = '-'; + _load_hw_id_byte(i, &PROD_SIGS.WAFNUM); i += 2; + hw.id[i++] = '-'; + _load_hw_id_byte(i, &PROD_SIGS.COORDX1); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.COORDX0); i += 2; + hw.id[i++] = '-'; + _load_hw_id_byte(i, &PROD_SIGS.COORDY1); i += 2; + _load_hw_id_byte(i, &PROD_SIGS.COORDY0); i += 2; + hw.id[i] = 0; +} + + +/// Lowest level hardware init +void hardware_init() { + _init_clock(); // set system clock + rtc_init(); // real time counter + _read_hw_id(); + + // Round-robin, interrupts in application section, all interupts levels + CCP = CCP_IOREG_gc; + PMIC.CTRL = + PMIC_RREN_bm | PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm; +} + + +void hw_request_hard_reset() {hw.hard_reset = true;} + + +/// Hard reset using watchdog timer +/// software hard reset using the watchdog timer +void hw_hard_reset() { + usart_flush(); + cli(); + CCP = CCP_IOREG_gc; + RST.CTRL = RST_SWRST_bm; +} + + +/// Controller's rest handler +void hw_reset_handler() { + if (hw.hard_reset) { + while (huanyang_stopping() || !usart_tx_empty() || !eeprom_is_ready()) + continue; + + hw_hard_reset(); + } + + if (hw.bootloader) { + // TODO enable bootloader interrupt vectors and jump to BOOT_SECTION_START + hw.bootloader = false; + } +} + + +/// Executes a software reset using CCPWrite +void hw_request_bootloader() {hw.bootloader = true;} + + +uint8_t hw_disable_watchdog() { + uint8_t state = WDT.CTRL; + wdt_disable(); + return state; +} + + +void hw_restore_watchdog(uint8_t state) { + cli(); + CCP = CCP_IOREG_gc; + WDT.CTRL = state | WDT_CEN_bm; + sei(); +} + + +const char *get_hw_id() { + return hw.id; +} diff --git a/avr/src/hardware.h b/avr/src/hardware.h new file mode 100644 index 0000000..4287779 --- /dev/null +++ b/avr/src/hardware.h @@ -0,0 +1,45 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2012 - 2015 Rob Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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/home.c b/avr/src/home.c new file mode 100644 index 0000000..a8b8986 --- /dev/null +++ b/avr/src/home.c @@ -0,0 +1,176 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "home.h" + +#include "plan/runtime.h" +#include "plan/line.h" +#include "plan/state.h" +#include "plan/exec.h" +#include "axis.h" +#include "motor.h" +#include "util.h" +#include "config.h" + +#include +#include + + +typedef enum { + STATE_INIT, + STATE_HOMING, + STATE_STALLED, + STATE_BACKOFF, + STATE_DONE, +} home_state_t; + + +typedef struct { + bool homed[AXES]; + unsigned axis; + home_state_t state; + float velocity; + uint16_t microsteps; +} home_t; + +static home_t home; + + +void _stall_callback(int motor) { + if (home.velocity == mp_runtime_get_velocity()) { + mp_exec_abort(); + home.state = STATE_STALLED; + } +} + + +void _move_axis(float travel) { + float target[AXES]; + float *position = mp_runtime_get_position(); + copy_vector(target, position); + target[home.axis] += travel; + mp_aline(target, false, false, false, home.velocity, 1, -1); +} + + +void home_callback() { + if (mp_get_cycle() != CYCLE_HOMING || !mp_is_quiescent() || + !mp_queue_is_empty()) return; + + while (true) { + int motor = axis_get_motor(home.axis); + homing_mode_t mode = axis_get_homing_mode(home.axis); + int direction = + (mode == HOMING_STALL_MIN || mode == HOMING_SWITCH_MIN) ? -1 : 1; + float min = axis_get_travel_min(home.axis); + float max = axis_get_travel_max(home.axis); + + switch (home.state) { + case STATE_INIT: { + if (motor == -1 || mode == HOMING_DISABLED) { + home.state = STATE_DONE; + break; + } + + STATUS_INFO("Homing %c axis", axis_get_char(home.axis)); + + // Set axis not homed + home.homed[home.axis] = false; + + // Determine homing type: switch or stall + + // Configure driver, set stall callback and compute homing velocity + home.microsteps = motor_get_microsteps(motor); + motor_set_microsteps(motor, 8); + motor_set_stall_callback(motor, _stall_callback); + //home.velocity = axis_get_stall_homing_velocity(home.axis); + home.velocity = axis_get_search_velocity(home.axis); + + // Move in home direction + float travel = max - min; // TODO consider ramping distance + _move_axis(travel * direction); + + home.state = STATE_HOMING; + return; + } + + case STATE_HOMING: + case STATE_STALLED: + // Restore motor driver config + motor_set_microsteps(motor, home.microsteps); + motor_set_stall_callback(motor, 0); + + if (home.state == STATE_HOMING) { + STATUS_ERROR(STAT_HOMING_CYCLE_FAILED, "Failed to find %c axis home", + axis_get_char(home.axis)); + mp_set_cycle(CYCLE_MACHINING); + + } else { + STATUS_INFO("Backing off %c axis", axis_get_char(home.axis)); + mach_set_axis_position(home.axis, direction < 0 ? min : max); + _move_axis(axis_get_zero_backoff(home.axis) * direction * -1); + home.state = STATE_BACKOFF; + } + return; + + case STATE_BACKOFF: + STATUS_INFO("Homed %c axis", axis_get_char(home.axis)); + + // Set axis position & homed + mach_set_axis_position(home.axis, min); + home.homed[home.axis] = true; + home.state = STATE_DONE; + break; + + case STATE_DONE: + if (home.axis == AXIS_X) { + // Done + mp_set_cycle(CYCLE_MACHINING); + return; + } + + // Next axis + home.axis--; + home.state = STATE_INIT; + break; + } + } +} + + +uint8_t command_home(int argc, char *argv[]) { + if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY) + return 0; + + // Init + memset(&home, 0, sizeof(home)); + home.axis = AXIS_Z; + + mp_set_cycle(CYCLE_HOMING); + + return 0; +} diff --git a/avr/src/home.h b/avr/src/home.h new file mode 100644 index 0000000..dd6f286 --- /dev/null +++ b/avr/src/home.h @@ -0,0 +1,31 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + +void home_init(); +void home_callback(); diff --git a/avr/src/homing.c b/avr/src/homing.c new file mode 100644 index 0000000..fda66c9 --- /dev/null +++ b/avr/src/homing.c @@ -0,0 +1,408 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "homing.h" + +#include "axis.h" +#include "machine.h" +#include "switch.h" +#include "gcode_parser.h" +#include "report.h" + +#include "plan/planner.h" +#include "plan/runtime.h" +#include "plan/state.h" + + +typedef void (*homing_func_t)(int8_t axis); + +static void _homing_axis_start(int8_t axis); + + +typedef enum { + HOMING_NOT_HOMED, // machine is not homed + HOMING_HOMED, // machine is homed + HOMING_WAITING, // machine waiting to be homed +} homing_state_t; + + +/// persistent homing runtime variables +typedef struct { + homing_state_t state; // homing cycle sub-state machine + bool homed[AXES]; // individual axis homing flags + + // controls for homing cycle + int8_t axis; // axis currently being homed + + /// homing switch for current axis (index into switch flag table) + int8_t homing_switch; + int8_t limit_switch; // limit switch for current axis or -1 if none + + uint8_t homing_closed; // 0=open, 1=closed + uint8_t limit_closed; // 0=open, 1=closed + /// G28.4 flag. true = set coords to zero at the end of homing cycle + uint8_t set_coordinates; + homing_func_t func; // binding for callback function state machine + + // per-axis parameters + /// set to 1 for positive (max), -1 for negative (to min); + float direction; + float search_travel; // signed distance to travel in search + float search_velocity; // search speed as positive number + float latch_velocity; // latch speed as positive number + /// max distance to back off switch during latch phase + float latch_backoff; + /// distance to back off switch before setting zero + float zero_backoff; + /// maximum distance of switch clearing backoffs before erring out + float max_clear_backoff; + + // state saved from gcode model + uint8_t saved_units; // G20,G21 global setting + uint8_t saved_coord_system; // G54 - G59 setting + uint8_t saved_distance_mode; // G90,G91 global setting + uint8_t saved_feed_mode; // G93,G94 global setting + float saved_feed_rate; // F setting + float saved_jerk; // saved and restored for each axis homed +} homing_t; + + +static homing_t hm = {0}; + + +// G28.2 homing cycle + +/* Homing works from a G28.2 according to the following writeup: + * + * https://github.com/synthetos + * /TinyG/wiki/Homing-and-Limits-Description-and-Operation + * + * How does this work? + * + * Homing is invoked using a G28.2 command with 1 or more axes specified in the + * command: e.g. g28.2 x0 y0 z0 (FYI: the number after each axis is irrelevant) + * + * Homing is always run in the following order - for each enabled axis: + * Z,X,Y,A Note: B and C cannot be homed + * + * At the start of a homing cycle those switches configured for homing + * (or for homing and limits) are treated as homing switches (they are modal). + * + * After initialization the following sequence is run for each axis to be homed: + * + * 0. If a homing or limit switch is closed on invocation, clear the switch + * 1. Drive towards the homing switch at search velocity until switch is hit + * 2. Drive away from the homing switch at latch velocity until switch opens + * 3. Back off switch by the zero backoff distance and set zero for that axis + * + * Homing works as a state machine that is driven by registering a callback + * function at hm.func() for the next state to be run. Once the axis is + * initialized each callback basically does two things (1) start the move + * for the current function, and (2) register the next state with hm.func(). + * When a move is started it will either be interrupted if the homing switch + * changes state, This will cause the move to stop with a feedhold. The other + * thing that can happen is the move will run to its full length if no switch + * change is detected (hit or open), + * + * Once all moves for an axis are complete the next axis in the sequence is + * homed. + * + * When a homing cycle is initiated the homing state is set to HOMING_NOT_HOMED + * When homing completes successfully this is set to HOMING_HOMED, otherwise it + * remains HOMING_NOT_HOMED. + * + * Notes: + * + * 1. When coding a cycle (like this one) you get to perform one queued + * move per entry into the continuation, then you must exit. + * + * 2. When coding a cycle (like this one) you must wait until + * the last move has actually been queued (or has finished) before + * declaring the cycle to be done. Otherwise there is a nasty race + * condition that will accept the next command before the position of + * the final move has been recorded in the Gcode model. + */ + + +/*** Return next axis in sequence based on axis in arg + * + * Accepts "axis" arg as the current axis; or -1 to retrieve the first axis + * Returns next axis based on "axis" argument and if that axis is flagged for + * homing in the gf struct + * Returns -1 when all axes have been processed + * Returns -2 if no axes are specified (Gcode calling error) + * Homes Z first, then the rest in sequence + * + * Isolating this function facilitates implementing more complex and + * user-specified axis homing orders + */ +static int8_t _get_next_axis(int8_t axis) { + switch (axis) { + case -1: if (parser.gf.target[AXIS_Z]) return AXIS_Z; + case AXIS_Z: if (parser.gf.target[AXIS_X]) return AXIS_X; + case AXIS_X: if (parser.gf.target[AXIS_Y]) return AXIS_Y; + case AXIS_Y: if (parser.gf.target[AXIS_A]) return AXIS_A; + case AXIS_A: if (parser.gf.target[AXIS_B]) return AXIS_B; + case AXIS_B: if (parser.gf.target[AXIS_C]) return AXIS_C; + } + + return axis == -1 ? -2 : -1; // error or done +} + + +/// Helper to finalize homing, third part of return to home +static void _homing_finalize_exit() { + mp_flush_planner(); // should be stopped, but in case of switch closure + + // Restore saved machine state + mach_set_coord_system(hm.saved_coord_system); + mach_set_units(hm.saved_units); + mach_set_distance_mode(hm.saved_distance_mode); + mach_set_feed_mode(hm.saved_feed_mode); + mach_set_feed_rate(hm.saved_feed_rate); + mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); + + mp_set_cycle(CYCLE_MACHINING); // Default cycle +} + + +static void _homing_error_exit(stat_t status) { + _homing_finalize_exit(); + status_error(status); +} + + +/// Execute moves +static void _homing_axis_move(int8_t axis, float target, float velocity) { + float vect[AXES] = {0}; + bool flags[AXES] = {0}; + + vect[axis] = target; + flags[axis] = true; + mach_set_feed_rate(velocity); + mp_flush_planner(); // don't use mp_request_flush() here + + stat_t status = mach_feed(vect, flags); + if (status) _homing_error_exit(status); +} + + +/// End homing cycle in progress +static void _homing_abort(int8_t axis) { + axis_set_jerk_max(axis, hm.saved_jerk); // restore the max jerk value + + // homing state remains HOMING_NOT_HOMED + _homing_error_exit(STAT_HOMING_CYCLE_FAILED); + + report_request(); +} + + +/// set zero and finish up +static void _homing_axis_set_zero(int8_t axis) { + if (hm.set_coordinates) { + mach_set_axis_position(axis, 0); + hm.homed[axis] = true; + + } else // do not set axis if in G28.4 cycle + mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); + + axis_set_jerk_max(axis, hm.saved_jerk); // restore the max jerk value + + hm.func = _homing_axis_start; +} + + +/// backoff to zero position +static void _homing_axis_zero_backoff(int8_t axis) { + _homing_axis_move(axis, hm.zero_backoff, hm.search_velocity); + hm.func = _homing_axis_set_zero; +} + + +/// Slow reverse until switch opens again +static void _homing_axis_latch(int8_t axis) { + // verify assumption that we arrived here because of homing switch closure + // rather than user-initiated feedhold or other disruption + if (!switch_is_active(hm.homing_switch)) hm.func = _homing_abort; + + else { + _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity); + hm.func = _homing_axis_zero_backoff; + } +} + + +/// Fast search for switch, closes switch +static void _homing_axis_search(int8_t axis) { + // use the homing jerk for search onward + _homing_axis_move(axis, hm.search_travel, hm.search_velocity); + hm.func = _homing_axis_latch; +} + + +/// Initiate a clear to move off a switch that is thrown at the start +static void _homing_axis_clear(int8_t axis) { + // Handle an initial switch closure by backing off the closed switch + // NOTE: Relies on independent switches per axis (not shared) + + if (switch_is_active(hm.homing_switch)) + _homing_axis_move(axis, hm.latch_backoff, hm.search_velocity); + + else if (switch_is_active(hm.limit_switch)) + _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity); + + hm.func = _homing_axis_search; +} + + +/// Get next axis, initialize variables, call the clear +static void _homing_axis_start(int8_t axis) { + // get the first or next axis + if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error + if (axis == -1) { // -1 is done + hm.state = HOMING_HOMED; + _homing_finalize_exit(); + return; + + } else if (axis == -2) // -2 is error + return _homing_error_exit(STAT_HOMING_ERROR_BAD_OR_NO_AXIS); + } + + // clear the homed flag for axis to move w/o triggering soft limits + hm.homed[axis] = false; + + // trap axis mis-configurations + if (fp_ZERO(axis_get_search_velocity(axis))) + return _homing_error_exit(STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY); + if (fp_ZERO(axis_get_latch_velocity(axis))) + return _homing_error_exit(STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY); + if (axis_get_latch_backoff(axis) < 0) + return _homing_error_exit(STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF); + + // calculate and test travel distance + float travel_distance = + fabs(axis_get_travel_max(axis) - axis_get_travel_min(axis)) + + axis_get_latch_backoff(axis); + if (fp_ZERO(travel_distance)) + return _homing_error_exit(STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL); + + hm.axis = axis; // persist the axis + // search velocity is always positive + hm.search_velocity = fabs(axis_get_search_velocity(axis)); + // latch velocity is always positive + hm.latch_velocity = fabs(axis_get_latch_velocity(axis)); + + // determine which switch to use + bool min_enabled = switch_is_enabled(MIN_SWITCH(axis)); + bool max_enabled = switch_is_enabled(MAX_SWITCH(axis)); + + if (min_enabled) { + // setup parameters homing to the minimum switch + hm.homing_switch = MIN_SWITCH(axis); // min is the homing switch + hm.limit_switch = MAX_SWITCH(axis); // max would be limit switch + hm.search_travel = -travel_distance; // in negative direction + hm.latch_backoff = axis_get_latch_backoff(axis); // in positive direction + hm.zero_backoff = axis_get_zero_backoff(axis); + + } else if (max_enabled) { + // setup parameters for positive travel (homing to the maximum switch) + hm.homing_switch = MAX_SWITCH(axis); // max is homing switch + hm.limit_switch = MIN_SWITCH(axis); // min would be limit switch + hm.search_travel = travel_distance; // in positive direction + hm.latch_backoff = -axis_get_latch_backoff(axis); // in negative direction + hm.zero_backoff = -axis_get_zero_backoff(axis); + + } else { + // if homing is disabled for the axis then skip to the next axis + hm.limit_switch = -1; // disable the limit switch parameter + hm.func = _homing_axis_start; + return; + } + + hm.saved_jerk = axis_get_jerk_max(axis); // save the max jerk value + hm.func = _homing_axis_clear; // start the clear +} + + +bool mach_is_homing() { + return mp_get_cycle() == CYCLE_HOMING; +} + + +void mach_set_not_homed() { + for (int axis = 0; axis < AXES; axis++) + mach_set_homed(axis, false); +} + + +bool mach_get_homed(int axis) { + return hm.homed[axis]; +} + + +void mach_set_homed(int axis, bool homed) { + // TODO save homed to EEPROM + hm.homed[axis] = homed; +} + + +/// G28.2 homing cycle using limit switches +void mach_homing_cycle_start() { + // save relevant non-axis parameters from Gcode model + hm.saved_units = mach_get_units(); + hm.saved_coord_system = mach_get_coord_system(); + hm.saved_distance_mode = mach_get_distance_mode(); + hm.saved_feed_mode = mach_get_feed_mode(); + hm.saved_feed_rate = mach_get_feed_rate(); + + // set working values + mach_set_units(MILLIMETERS); + mach_set_distance_mode(INCREMENTAL_MODE); + mach_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates + mach_set_feed_mode(UNITS_PER_MINUTE_MODE); + hm.set_coordinates = true; + + hm.axis = -1; // set to retrieve initial axis + hm.func = _homing_axis_start; // bind initial processing function + mp_set_cycle(CYCLE_HOMING); + hm.state = HOMING_NOT_HOMED; +} + + +void mach_homing_cycle_start_no_set() { + mach_homing_cycle_start(); + hm.set_coordinates = false; // don't update position variables at end of cycle +} + + +/// Main loop callback for running the homing cycle +void mach_homing_callback() { + if (mp_get_cycle() != CYCLE_HOMING || mp_get_state() != STATE_READY) return; + hm.func(hm.axis); // execute the current homing move +} diff --git a/avr/src/homing.h b/avr/src/homing.h new file mode 100644 index 0000000..f4798f1 --- /dev/null +++ b/avr/src/homing.h @@ -0,0 +1,39 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + + +bool mach_is_homing(); +void mach_set_not_homed(); +bool mach_get_homed(int axis); +void mach_set_homed(int axis, bool homed); +void mach_homing_cycle_start(); +void mach_homing_cycle_start_no_set(); +void mach_homing_callback(); diff --git a/avr/src/huanyang.c b/avr/src/huanyang.c new file mode 100644 index 0000000..1abe98b --- /dev/null +++ b/avr/src/huanyang.c @@ -0,0 +1,543 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 +#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_DC_VOLTAGE, + HUANYANG_AC_VOLTAGE, + HUANYANG_CONT, + HUANYANG_TEMPERATURE, +}; + + +enum { + HUANYANG_FORWARD = 1, + HUANYANG_STOP = 8, + HUANYANG_REVERSE = 17, +}; + + +enum { + HUANYANG_RUN = 1 << 0, + HUANYANG_JOG = 1 << 1, + HUANYANG_COMMAND_RF = 1 << 2, + HUANYANG_RUNNING = 1 << 3, + HUANYANG_JOGGING = 1 << 4, + HUANYANG_RUNNING_RF = 1 << 5, + HUANYANG_BRACKING = 1 << 6, + HUANYANG_TRACK_START = 1 << 7, +}; + + +typedef bool (*next_command_cb_t)(int index); + + +typedef struct { + uint8_t id; + bool debug; + + next_command_cb_t next_command_cb; + uint8_t command_index; + uint8_t current_offset; + uint8_t command[8]; + uint8_t command_length; + uint8_t response_length; + uint8_t response[10]; + uint32_t last; + uint8_t retry; + + bool connected; + bool changed; + bool estop; + spindle_mode_t mode; + float speed; + + float actual_freq; + float actual_current; + uint16_t actual_rpm; + uint16_t dc_voltage; + uint16_t ac_voltage; + uint16_t temperature; + + float max_freq; + float min_freq; + uint16_t rated_rpm; + + uint8_t status; +} huanyang_t; + + +static huanyang_t ha = {0}; + + +#define CTRL_STATUS_RESPONSE(R) ((uint16_t)R[4] << 8 | R[5]) +#define FUNC_RESPONSE(R) (R[2] == 2 ? R[4] : ((uint16_t)R[4] << 8 | R[5])) + + +static uint16_t _crc16(const uint8_t *buffer, unsigned length) { + uint16_t crc = 0xffff; + + for (unsigned i = 0; i < length; i++) + crc = _crc16_update(crc, buffer[i]); + + return crc; +} + + +static void _set_baud(uint16_t bsel, uint8_t bscale) { + HUANYANG_PORT.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8)); + HUANYANG_PORT.BAUDCTRLA = bsel; +} + + +static void _set_write(bool x) { + if (x) OUTSET_PIN(RS485_RW_PIN); // High + else OUTCLR_PIN(RS485_RW_PIN); // Low +} + + +static void _set_dre_interrupt(bool enable) { + if (enable) HUANYANG_PORT.CTRLA |= USART_DREINTLVL_MED_gc; + else HUANYANG_PORT.CTRLA &= ~USART_DREINTLVL_MED_gc; +} + + +static void _set_txc_interrupt(bool enable) { + if (enable) HUANYANG_PORT.CTRLA |= USART_TXCINTLVL_MED_gc; + else HUANYANG_PORT.CTRLA &= ~USART_TXCINTLVL_MED_gc; +} + + +static void _set_rxc_interrupt(bool enable) { + if (enable) HUANYANG_PORT.CTRLA |= USART_RXCINTLVL_MED_gc; + else HUANYANG_PORT.CTRLA &= ~USART_RXCINTLVL_MED_gc; +} + + +static void _set_command1(int code, uint8_t arg0) { + ha.command_length = 4; + ha.command[1] = code; + ha.command[2] = 1; + ha.command[3] = arg0; +} + + +static void _set_command2(int code, uint8_t arg0, uint8_t arg1) { + ha.command_length = 5; + ha.command[1] = code; + ha.command[2] = 2; + ha.command[3] = arg0; + ha.command[4] = arg1; +} + + +static void _set_command3(int code, uint8_t arg0, uint8_t arg1, uint8_t arg2) { + ha.command_length = 6; + ha.command[1] = code; + ha.command[2] = 3; + ha.command[3] = arg0; + ha.command[4] = arg1; + ha.command[5] = arg2; +} + + +static int _response_length(int code) { + switch (code) { + case HUANYANG_FUNC_READ: return 8; + case HUANYANG_FUNC_WRITE: return 8; + case HUANYANG_CTRL_WRITE: return 6; + case HUANYANG_CTRL_READ: return 8; + case HUANYANG_FREQ_WRITE: return 7; + default: return -1; + } +} + + +static bool _update(int index) { + // Read response + switch (index) { + case 1: ha.status = ha.response[5]; break; + case 2: ha.max_freq = FUNC_RESPONSE(ha.response) * 0.01; break; + case 3: ha.min_freq = FUNC_RESPONSE(ha.response) * 0.01; break; + case 4: ha.rated_rpm = FUNC_RESPONSE(ha.response); break; + default: break; + } + + // Setup next command + uint8_t var; + switch (index) { + case 0: { // Update mode + uint8_t state; + switch (ha.mode) { + case SPINDLE_CW: state = HUANYANG_FORWARD; break; + case SPINDLE_CCW: state = HUANYANG_REVERSE; break; + default: state = HUANYANG_STOP; break; + } + + _set_command1(HUANYANG_CTRL_WRITE, state); + + return true; + } + + case 1: var = HUANYANG_MAX_FREQ; break; + case 2: var = HUANYANG_MIN_FREQ; break; + case 3: var = HUANYANG_RATED_RPM; break; + + case 4: { // Update freqency + // Compute frequency in Hz + float freq = fabs(ha.speed * 50 / ha.rated_rpm); + + // Clamp frequency + if (ha.max_freq < freq) freq = ha.max_freq; + if (freq < ha.min_freq) freq = ha.min_freq; + + // Frequency write command + uint16_t f = freq * 100; + _set_command2(HUANYANG_FREQ_WRITE, f >> 8, f); + + return true; + } + + default: + report_request(); + return false; + } + + _set_command3(HUANYANG_FUNC_READ, var, 0, 0); + + return true; +} + + +static bool _query_status(int index) { + // Read response + switch (index) { + case 1: ha.actual_freq = CTRL_STATUS_RESPONSE(ha.response) * 0.01; break; + case 2: ha.actual_current = CTRL_STATUS_RESPONSE(ha.response) * 0.1; break; + case 3: ha.actual_rpm = CTRL_STATUS_RESPONSE(ha.response); break; + case 4: ha.dc_voltage = CTRL_STATUS_RESPONSE(ha.response); break; + case 5: ha.ac_voltage = CTRL_STATUS_RESPONSE(ha.response); break; + case 6: ha.temperature = CTRL_STATUS_RESPONSE(ha.response); break; + default: break; + } + + // Setup next command + uint8_t var; + switch (index) { + case 0: var = HUANYANG_ACTUAL_FREQ; break; + case 1: var = HUANYANG_ACTUAL_CURRENT; break; + case 2: var = HUANYANG_ACTUAL_RPM; break; + case 3: var = HUANYANG_DC_VOLTAGE; break; + case 4: var = HUANYANG_AC_VOLTAGE; break; + case 5: var = HUANYANG_TEMPERATURE; break; + default: + report_request(); + return false; + } + + _set_command1(HUANYANG_CTRL_READ, var); + + return true; +} + + +static void _next_command(); + + +static void _next_state() { + if (ha.changed) { + ha.next_command_cb = _update; + ha.changed = false; + + } else ha.next_command_cb = _query_status; + + _next_command(); +} + + +static bool _check_response() { + // Check CRC + uint16_t computed = _crc16(ha.response, ha.response_length - 2); + uint16_t expected = (ha.response[ha.response_length - 1] << 8) | + ha.response[ha.response_length - 2]; + + if (computed != expected) { + STATUS_WARNING("huanyang: invalid CRC, expected=0x%04u got=0x%04u", + expected, computed); + return false; + } + + // Check return function code matches sent + if (ha.command[1] != ha.response[1]) { + STATUS_WARNING("huanyang: invalid function code, expected=%u got=%u", + ha.command[2], ha.response[2]); + return false; + } + + return true; +} + + +static void _next_command() { + if (ha.next_command_cb && ha.next_command_cb(ha.command_index++)) { + ha.response_length = _response_length(ha.command[1]); + + ha.command[0] = ha.id; + + uint16_t crc = _crc16(ha.command, ha.command_length); + ha.command[ha.command_length++] = crc; + ha.command[ha.command_length++] = crc >> 8; + + _set_dre_interrupt(true); + + return; + } + + ha.command_index = 0; + _next_state(); +} + + +static void _retry_command() { + ha.last = 0; + ha.current_offset = 0; + ha.retry++; + + _set_write(true); // RS485 write mode + + _set_txc_interrupt(false); + _set_rxc_interrupt(false); + _set_dre_interrupt(true); + + if (ha.debug) STATUS_DEBUG("huanyang: retry %d", ha.retry); +} + + +// Data register empty interrupt +ISR(HUANYANG_DRE_vect) { + HUANYANG_PORT.DATA = ha.command[ha.current_offset++]; + + if (ha.current_offset == ha.command_length) { + _set_dre_interrupt(false); + _set_txc_interrupt(true); + ha.current_offset = 0; + } +} + + +/// Transmit complete interrupt +ISR(HUANYANG_TXC_vect) { + _set_txc_interrupt(false); + _set_rxc_interrupt(true); + _set_write(false); // RS485 read mode + ha.last = rtc_get_time(); // Set timeout timer +} + + +// Data received interrupt +ISR(HUANYANG_RXC_vect) { + ha.response[ha.current_offset++] = USARTD1.DATA; + + if (ha.current_offset == ha.response_length) { + _set_rxc_interrupt(false); + ha.current_offset = 0; + _set_write(true); // RS485 write mode + + if (_check_response()) { + ha.last = 0; // Clear timeout timer + ha.retry = 0; // Reset retry counter + ha.connected = true; + + _next_command(); + } + } +} + + +void huanyang_init() { + PR.PRPD &= ~PR_USART1_bm; // Disable power reduction + + DIRCLR_PIN(RS485_RO_PIN); // Input + OUTSET_PIN(RS485_DI_PIN); // High + DIRSET_PIN(RS485_DI_PIN); // Output + OUTSET_PIN(RS485_RW_PIN); // High + DIRSET_PIN(RS485_RW_PIN); // Output + + _set_baud(3325, 0b1101); // 9600 @ 32MHz with 2x USART + + // No parity, 8 data bits, 1 stop bit + USARTD1.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | + USART_CHSIZE_8BIT_gc; + + // Configure receiver and transmitter + USARTD1.CTRLB = USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm; + + ha.id = HUANYANG_ID; + huanyang_reset(); +} + + +void huanyang_set(spindle_mode_t mode, float speed) { + if ((ha.mode != mode || ha.speed != speed) && !ha.estop) { + if (ha.debug) STATUS_DEBUG("huanyang: mode=%d, speed=%0.2f", mode, speed); + + ha.mode = mode; + ha.speed = speed; + ha.changed = true; + } +} + + +void huanyang_reset() { + _set_dre_interrupt(false); + _set_txc_interrupt(false); + _set_rxc_interrupt(false); + _set_write(true); // RS485 write mode + + // Flush USART + uint8_t x = USARTD1.DATA; + x = USARTD1.STATUS; + x = x; + + // Save settings + uint8_t id = ha.id; + spindle_mode_t mode = ha.mode; + float speed = ha.speed; + bool debug = ha.debug; + + // Clear state + memset(&ha, 0, sizeof(ha)); + + // Restore settings + ha.id = id; + ha.mode = mode; + ha.speed = speed; + ha.debug = debug; + ha.changed = true; + + _next_state(); +} + + +void huanyang_rtc_callback() { + if (ha.last && rtc_expired(ha.last + HUANYANG_TIMEOUT)) { + if (ha.retry < HUANYANG_RETRIES) _retry_command(); + else { + if (ha.debug) STATUS_DEBUG("huanyang: timedout"); + + if (ha.debug && ha.current_offset) { + const uint8_t buf_len = 8 * 2 + 1; + char sent[buf_len]; + char received[buf_len]; + + uint8_t i; + for (i = 0; i < ha.command_length; i++) + sprintf(sent + i * 2, "%02x", ha.command[i]); + sent[i * 2] = 0; + + for (i = 0; i < ha.current_offset; i++) + sprintf(received + i * 2, "%02x", ha.response[i]); + received[i * 2] = 0; + + STATUS_DEBUG("huanyang: sent 0x%s received 0x%s expected %u bytes", + sent, received, ha.response_length); + } + + huanyang_reset(); + } + } +} + + +void huanyang_estop() { + huanyang_set(SPINDLE_OFF, 0); + huanyang_reset(); + ha.estop = true; +} + + +bool huanyang_stopping() { + return ha.estop && (ha.changed || ha.next_command_cb == _update); +} + + +uint8_t get_huanyang_id() {return ha.id;} +void set_huanyang_id(uint8_t value) {ha.id = value;} +bool get_huanyang_debug() {return ha.debug;} +void set_huanyang_debug(bool value) {ha.debug = value;} +bool get_huanyang_connected() {return ha.connected;} +float get_huanyang_freq() {return ha.actual_freq;} +float get_huanyang_current() {return ha.actual_current;} +uint16_t get_huanyang_rpm() {return ha.actual_rpm;} +uint16_t get_huanyang_dcv() {return ha.dc_voltage;} +uint16_t get_huanyang_acv() {return ha.ac_voltage;} +uint16_t get_huanyang_temp() {return ha.temperature;} +float get_huanyang_max_freq() {return ha.max_freq;} +float get_huanyang_min_freq() {return ha.min_freq;} +uint16_t get_huanyang_rated_rpm() {return ha.rated_rpm;} +float get_huanyang_status() {return ha.status;} diff --git a/avr/src/huanyang.h b/avr/src/huanyang.h new file mode 100644 index 0000000..46efeac --- /dev/null +++ b/avr/src/huanyang.h @@ -0,0 +1,38 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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_estop(); +bool huanyang_stopping(); diff --git a/avr/src/i2c.c b/avr/src/i2c.c new file mode 100644 index 0000000..0ec26b9 --- /dev/null +++ b/avr/src/i2c.c @@ -0,0 +1,128 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2013 Alden S. Hart Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..7ded350 --- /dev/null +++ b/avr/src/i2c.h @@ -0,0 +1,58 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2013 Alden S. Hart Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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_HOME, + I2C_REBOOT, + I2C_ZERO, +} i2c_cmd_t; + + +typedef void (*i2c_read_cb_t)(i2c_cmd_t cmd, uint8_t *data, uint8_t length); +typedef uint8_t (*i2c_write_cb_t)(uint8_t offset, bool *done); + + +void i2c_init(); +void i2c_set_read_callback(i2c_read_cb_t cb); +void i2c_set_write_callback(i2c_write_cb_t cb); diff --git a/avr/src/machine.c b/avr/src/machine.c new file mode 100644 index 0000000..49c6349 --- /dev/null +++ b/avr/src/machine.c @@ -0,0 +1,855 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2012 - 2015 Rob Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "homing.h" + +#include "plan/planner.h" +#include "plan/runtime.h" +#include "plan/dwell.h" +#include "plan/arc.h" +#include "plan/line.h" +#include "plan/state.h" + + +typedef struct { // struct to manage mach globals and cycles + float offset[COORDS + 1][AXES]; // coordinate systems & offsets G53-G59 + float origin_offset[AXES]; // G92 offsets + bool origin_offset_enable; // G92 offsets enabled / disabled + + float position[AXES]; // model position + float g28_position[AXES]; // stored machine position for G28 + float g30_position[AXES]; // stored machine position for G30 + + gcode_state_t gm; // core gcode model state +} machine_t; + + +static machine_t mach = { + // Offsets + .offset = { + {}, // ABSOLUTE_COORDS + + {0, 0, 0, 0, 0, 0}, // G54 + {X_TRAVEL_MAX / 2, Y_TRAVEL_MAX / 2, 0, 0, 0, 0}, // G55 + {0, 0, 0, 0, 0, 0}, // G56 + {0, 0, 0, 0, 0, 0}, // G57 + {0, 0, 0, 0, 0, 0}, // G58 + {0, 0, 0, 0, 0, 0}, // G59 + }, + + // State + .gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE}, +}; + + +// Machine State functions +uint32_t mach_get_line() {return mach.gm.line;} +float mach_get_feed_rate() {return mach.gm.feed_rate;} +feed_mode_t mach_get_feed_mode() {return mach.gm.feed_mode;} + + +bool mach_is_inverse_time_mode() { + return mach.gm.feed_mode == INVERSE_TIME_MODE; +} + + +float mach_get_feed_override() { + return mach.gm.feed_override_enable ? mach.gm.feed_override : 1; +} + + +float mach_get_spindle_override() { + return mach.gm.spindle_override_enable ? mach.gm.spindle_override : 1; +} + + +motion_mode_t mach_get_motion_mode() {return mach.gm.motion_mode;} +bool mach_is_rapid() {return mach.gm.motion_mode == MOTION_MODE_RAPID;} +plane_t mach_get_plane() {return mach.gm.plane;} +units_t mach_get_units() {return mach.gm.units;} +coord_system_t mach_get_coord_system() {return mach.gm.coord_system;} +bool mach_get_absolute_mode() {return mach.gm.absolute_mode;} +path_mode_t mach_get_path_mode() {return mach.gm.path_mode;} +bool mach_is_exact_stop() {return mach.gm.path_mode == PATH_EXACT_STOP;} +distance_mode_t mach_get_distance_mode() {return mach.gm.distance_mode;} +distance_mode_t mach_get_arc_distance_mode() {return mach.gm.arc_distance_mode;} + + +void mach_set_motion_mode(motion_mode_t motion_mode) { + mach.gm.motion_mode = motion_mode; +} + + +/// Spindle speed callback from planner queue +static stat_t _exec_spindle_speed(mp_buffer_t *bf) { + spindle_set_speed(bf->value); + return STAT_NOOP; // No move queued +} + + +/// Queue the S parameter to the planner buffer +void mach_set_spindle_speed(float speed) { + mp_buffer_t *bf = mp_queue_get_tail(); + bf->value = speed * mach_get_spindle_override(); + mp_queue_push_nonstop(_exec_spindle_speed, mach_get_line()); +} + + +/// execute the spindle command (called from planner) +static stat_t _exec_spindle_mode(mp_buffer_t *bf) { + spindle_set_mode(bf->value); + return STAT_NOOP; // No move queued +} + + +/// Queue the spindle command to the planner buffer +void mach_set_spindle_mode(spindle_mode_t mode) { + mp_buffer_t *bf = mp_queue_get_tail(); + bf->value = mode; + mp_queue_push_nonstop(_exec_spindle_mode, mach_get_line()); +} + + +void mach_set_absolute_mode(bool absolute_mode) { + mach.gm.absolute_mode = absolute_mode; +} + + +void mach_set_line(uint32_t line) {mach.gm.line = line;} + + +/* Coordinate systems and offsets + * + * Functions to get, set and report coordinate systems and work offsets + * These functions are not part of the NIST defined functions + * + * Notes on Coordinate System and Offset functions + * + * All positional information in the machine is kept as + * absolute coords and in canonical units (mm). The offsets are only + * used to translate in and out of canonical form during + * interpretation and response. + * + * Managing the coordinate systems & offsets is somewhat complicated. + * The following affect offsets: + * - coordinate system selected. 1-9 correspond to G54-G59 + * - absolute override: forces current move to be interpreted in machine + * coordinates: G53 (system 0) + * - G92 offsets are added "on top of" the coord system offsets -- + * if origin_offset_enable + * - G28 and G30 moves; these are run in absolute coordinates + * + * The offsets themselves are considered static, are kept in mach, and are + * supposed to be persistent. + * + * To reduce complexity and data load the following is done: + * - Full data for coordinates/offsets is only accessible by the + * machine, not the downstream + * - Resolved set of coord and G92 offsets, with per-move exceptions can + * be captured as "work_offsets" + * - The core gcode context (gm) only knows about the active coord system + * and the work offsets + */ + +/* Return the currently active coordinate offset for an axis + * + * Takes G5x, G92 and absolute override into account to return the + * active offset for this move + * + * This function is typically used to evaluate and set offsets. + */ +float mach_get_active_coord_offset(uint8_t axis) { + // no offset in absolute override mode + if (mach.gm.absolute_mode) return 0; + float offset = mach.offset[mach.gm.coord_system][axis]; + + if (mach.origin_offset_enable) + offset += mach.origin_offset[axis]; // includes G5x and G92 components + + return offset; +} + + +static stat_t _exec_update_work_offsets(mp_buffer_t *bf) { + mp_runtime_set_work_offsets(bf->target); + return STAT_NOOP; // No move queued +} + + +// Capture coord offsets from the model into absolute values +void mach_update_work_offsets() { + static float work_offset[AXES] = {0}; + bool same = true; + + for (int axis = 0; axis < AXES; axis++) { + float offset = mach_get_active_coord_offset(axis); + + if (offset != work_offset[axis]) { + work_offset[axis] = offset; + same = false; + } + } + + if (!same) { + mp_buffer_t *bf = mp_queue_get_tail(); + copy_vector(bf->target, work_offset); + mp_queue_push_nonstop(_exec_update_work_offsets, mach_get_line()); + } +} + + +const float *mach_get_position() {return mach.position;} + + +void mach_set_position(const float position[]) { + copy_vector(mach.position, position); +} + + +/*** Get position of axis in absolute coordinates + * + * NOTE: Machine position is always returned in mm mode. No units conversion + * is performed. + */ +float mach_get_axis_position(uint8_t axis) {return mach.position[axis];} + + +/* Critical helpers + * + * Core functions supporting the machining functions + * These functions are not part of the NIST defined functions + */ + + + +/*** Calculate target vector + * + * This is a core routine. It handles: + * - conversion of linear units to internal canonical form (mm) + * - conversion of relative mode to absolute (internal canonical form) + * - translation of work coordinates to machine coordinates (internal + * canonical form) + * - application of axis radius mode + * + * Target coordinates are provided in @param values. + * Axes that need processing are signaled in @param flags. + */ +void mach_calc_target(float target[], const float values[], + const bool flags[]) { + for (int axis = 0; axis < AXES; axis++) { + target[axis] = mach.position[axis]; + if (!flags[axis] || !axis_is_enabled(axis)) continue; + + target[axis] = mach.gm.distance_mode == ABSOLUTE_MODE ? + mach_get_active_coord_offset(axis) : mach.position[axis]; + + float radius = axis_get_radius(axis); + if (radius) // Handle radius mode if radius is non-zero + target[axis] += TO_MM(values[axis]) * 360 / (2 * M_PI * radius); + // For ABC axes no mm conversion - it's already in degrees + else if (AXIS_Z < axis) target[axis] += values[axis]; + else target[axis] += TO_MM(values[axis]); + } +} + + +/*** Return error code if soft limit is exceeded + * + * Must be called with target properly set in GM struct. Best done + * after mach_calc_target(). + * + * Tests for soft limit for any homed axis if min and max are + * different values. You can set min and max to 0,0 to disable soft + * limits for an axis. Also will not test a min or a max if the value + * is < -1000000 (negative one million). This allows a single end to + * be tested w/the other disabled, should that requirement ever arise. + */ +stat_t mach_test_soft_limits(float target[]) { + for (int axis = 0; axis < AXES; axis++) { + if (!axis_get_homed(axis)) continue; // don't test axes that arent homed + + float min = axis_get_travel_min(axis); + float max = axis_get_travel_max(axis); + + // min == max means no soft limits + if (fp_EQ(min, max)) continue; + + if ((min > DISABLE_SOFT_LIMIT && target[axis] < min) || + (max > DISABLE_SOFT_LIMIT && target[axis] > max)) + return STAT_SOFT_LIMIT_EXCEEDED; + } + + return STAT_OK; +} + + +/* machining functions + * Values are passed in pre-unit_converted state (from gn structure) + * All operations occur on gm (current model state) + * + * These are organized by section number (x.x.x) in the order they are + * found in NIST RS274 NGCv3 + */ + +// Initialization and Termination (4.3.2) + +void machine_init() { + // Set gcode defaults + mach_set_units(GCODE_DEFAULT_UNITS); + mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM); + mach_set_plane(GCODE_DEFAULT_PLANE); + mach_set_path_mode(GCODE_DEFAULT_PATH_CONTROL); + mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE); + mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE); + mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // always the default + + // Sub-system inits + spindle_init(); + coolant_init(); +} + + +// Representation (4.3.3) +// +// Affect the Gcode model only (asynchronous) +// These functions assume input validation occurred upstream. + +/// G17, G18, G19 select axis plane +void mach_set_plane(plane_t plane) {mach.gm.plane = plane;} + + +/// G20, G21 +void mach_set_units(units_t mode) {mach.gm.units = mode;} + + +/// G90, G91 +void mach_set_distance_mode(distance_mode_t mode) { + mach.gm.distance_mode = mode; +} + + +/// G90.1, G91.1 +void mach_set_arc_distance_mode(distance_mode_t mode) { + mach.gm.arc_distance_mode = mode; +} + + +/* G10 L2 Pn, delayed persistence + * + * This function applies the offset to the GM model. + */ +void mach_set_coord_offsets(coord_system_t coord_system, float offset[], + bool flags[]) { + if (coord_system < G54 || G59 < coord_system) return; + + for (int axis = 0; axis < AXES; axis++) + if (flags[axis]) + mach.offset[coord_system][axis] = TO_MM(offset[axis]); +} + + +/// G54-G59 +void mach_set_coord_system(coord_system_t coord_system) { + mach.gm.coord_system = coord_system; +} + + +/* Set the position of a single axis in the model, planner and runtime + * + * This command sets an axis/axes to a position provided as an argument. + * This is useful for setting origins for homing, probing, and other operations. + * + * !!!!! DO NOT CALL THIS FUNCTION WHILE IN A MACHINING CYCLE !!!!! + * + * More specifically, do not call this function if there are any moves + * in the planner or if the runtime is moving. The system must be + * quiescent or you will introduce positional errors. This is true + * because the planned / running moves have a different reference + * frame than the one you are now going to set. These functions should + * only be called during initialization sequences and during cycles + * (such as homing cycles) when you know there are no more moves in + * the planner and that all motion has stopped. + */ +void mach_set_axis_position(unsigned axis, float position) { + //if (!mp_is_quiescent()) ALARM(STAT_MACH_NOT_QUIESCENT); + if (AXES <= axis) return; + + mach.position[axis] = position; + mp_set_axis_position(axis, position); + mp_runtime_set_axis_position(axis, position); + mp_runtime_set_steps_from_position(); +} + + +stat_t mach_zero_all() { + for (unsigned axis = 0; axis < AXES; axis++) { + stat_t status = mach_zero_axis(axis); + if (status != STAT_OK) return status; + } + + return STAT_OK; +} + + +stat_t mach_zero_axis(unsigned axis) { + if (!mp_is_quiescent()) return STAT_MACH_NOT_QUIESCENT; + if (AXES <= axis) return STAT_INVALID_AXIS; + + mach_set_axis_position(axis, 0); + + return STAT_OK; +} + + +// G28.3 functions and support +static stat_t _exec_absolute_origin(mp_buffer_t *bf) { + const float *origin = bf->target; + const float *flags = bf->unit; + + for (int axis = 0; axis < AXES; axis++) + if (flags[axis]) { + mp_runtime_set_axis_position(axis, origin[axis]); + mach_set_homed(axis, true); // G28.3 is not considered homed until here + } + + mp_runtime_set_steps_from_position(); + + return STAT_NOOP; // No move queued +} + + +/* G28.3 - model, planner and queue to runtime + * + * Takes a vector of origins (presumably 0's, but not necessarily) and + * applies them to all axes where the corresponding position in the + * flags vector is true (1). + * + * This is a 2 step process. The model and planner contexts are set + * immediately, the runtime command is queued and synchronized with + * the planner queue. This includes the runtime position and the step + * recording done by the encoders. At that point any axis that is set + * is also marked as homed. + */ +void mach_set_absolute_origin(float origin[], bool flags[]) { + float value[AXES]; + + for (int axis = 0; axis < AXES; axis++) + if (flags[axis]) { + value[axis] = TO_MM(origin[axis]); + mach.position[axis] = value[axis]; // set model position + mp_set_axis_position(axis, value[axis]); // set mm position + } + + mp_buffer_t *bf = mp_queue_get_tail(); + copy_vector(bf->target, origin); + copy_vector(bf->unit, flags); + mp_queue_push_nonstop(_exec_absolute_origin, mach_get_line()); +} + + +/* G92's behave according to NIST 3.5.18 & LinuxCNC G92 + * http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G92-G92.1-G92.2-G92.3 + */ + +/// G92 +void mach_set_origin_offsets(float offset[], bool flags[]) { + // set offsets in the Gcode model extended context + mach.origin_offset_enable = true; + for (int axis = 0; axis < AXES; axis++) + if (flags[axis]) + mach.origin_offset[axis] = mach.position[axis] - + mach.offset[mach.gm.coord_system][axis] - TO_MM(offset[axis]); +} + + +/// G92.1 +void mach_reset_origin_offsets() { + mach.origin_offset_enable = false; + for (int axis = 0; axis < AXES; axis++) + mach.origin_offset[axis] = 0; +} + + +/// G92.2 +void mach_suspend_origin_offsets() { + mach.origin_offset_enable = false; +} + + +/// G92.3 +void mach_resume_origin_offsets() { + mach.origin_offset_enable = true; +} + + +stat_t mach_plan_line(float target[]) { + return mp_aline(target, mach_is_rapid(), mach_is_inverse_time_mode(), + mach_is_exact_stop(), mach.gm.feed_rate, + mach_get_feed_override(), mach_get_line()); +} + + +// Free Space Motion (4.3.4) +static stat_t _feed(float values[], bool flags[]) { + float target[AXES]; + mach_calc_target(target, values, flags); + + // test soft limits + stat_t status = mach_test_soft_limits(target); + if (status != STAT_OK) return ALARM(status); + + // prep and plan the move + mach_update_work_offsets(); // update resolved offsets + mach_plan_line(target); + copy_vector(mach.position, target); // update model position + + return status; +} + + +/// G0 linear rapid +stat_t mach_rapid(float values[], bool flags[]) { + mach.gm.motion_mode = MOTION_MODE_RAPID; + return _feed(values, flags); +} + + +/// G28.1 +void mach_set_g28_position() {copy_vector(mach.g28_position, mach.position);} + + +/// G28 +stat_t mach_goto_g28_position(float target[], bool flags[]) { + mach_set_absolute_mode(true); + + // move through intermediate point, or skip + mach_rapid(target, flags); + + // execute actual stored move + bool f[] = {true, true, true, true, true, true}; + return mach_rapid(mach.g28_position, f); +} + + +/// G30.1 +void mach_set_g30_position() {copy_vector(mach.g30_position, mach.position);} + + +/// G30 +stat_t mach_goto_g30_position(float target[], bool flags[]) { + mach_set_absolute_mode(true); + + // move through intermediate point, or skip + mach_rapid(target, flags); + + // execute actual stored move + bool f[] = {true, true, true, true, true, true}; + return mach_rapid(mach.g30_position, f); +} + + +// Machining Attributes (4.3.5) + +/// F parameter +/// Normalize feed rate to mm/min or to minutes if in inverse time mode +void mach_set_feed_rate(float feed_rate) { + if (mach.gm.feed_mode == INVERSE_TIME_MODE) + // normalize to minutes (active for this gcode block only) + mach.gm.feed_rate = feed_rate ? 1 / feed_rate : 0; // Avoid div by zero + + else mach.gm.feed_rate = TO_MM(feed_rate); +} + + +/// G93, G94 +void mach_set_feed_mode(feed_mode_t mode) { + if (mach.gm.feed_mode == mode) return; + mach.gm.feed_rate = 0; // Force setting feed rate after changing modes + mach.gm.feed_mode = mode; +} + + +/// G61, G61.1, G64 +void mach_set_path_mode(path_mode_t mode) { + mach.gm.path_mode = mode; +} + + +// Machining Functions (4.3.6) See arc.c + +/// G4, P parameter (seconds) +stat_t mach_dwell(float seconds) { + return mp_dwell(seconds, mach_get_line()); +} + + +/// G1 +stat_t mach_feed(float values[], bool flags[]) { + // trap zero feed rate condition + if (fp_ZERO(mach.gm.feed_rate) || + (mach.gm.feed_mode == INVERSE_TIME_MODE && !parser.gf.feed_rate)) + return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; + + mach.gm.motion_mode = MOTION_MODE_FEED; + return _feed(values, flags); +} + + +// Spindle Functions (4.3.7) see spindle.c, spindle.h + +// Tool Functions (4.3.8) + +/// T parameter +void mach_select_tool(uint8_t tool) {mach.gm.tool = tool;} + + +static stat_t _exec_change_tool(mp_buffer_t *bf) { + mp_runtime_set_tool(bf->value); + mp_set_hold_reason(HOLD_REASON_TOOL_CHANGE); + mp_state_holding(); + return STAT_NOOP; // No move queued +} + + +/// M6 +void mach_change_tool(bool x) { + mp_buffer_t *bf = mp_queue_get_tail(); + bf->value = mach.gm.tool; + mp_queue_push(_exec_change_tool, mach_get_line()); +} + + +// Miscellaneous Functions (4.3.9) +static stat_t _exec_mist_coolant(mp_buffer_t *bf) { + coolant_set_mist(bf->value); + return STAT_NOOP; // No move queued +} + + +/// M7 +void mach_mist_coolant_control(bool mist_coolant) { + mp_buffer_t *bf = mp_queue_get_tail(); + bf->value = mist_coolant; + mp_queue_push_nonstop(_exec_mist_coolant, mach_get_line()); +} + + +static stat_t _exec_flood_coolant(mp_buffer_t *bf) { + coolant_set_flood(bf->value); + if (!bf->value) coolant_set_mist(false); // M9 special function + return STAT_NOOP; // No move queued +} + + +/// M8, M9 +void mach_flood_coolant_control(bool flood_coolant) { + mp_buffer_t *bf = mp_queue_get_tail(); + bf->value = flood_coolant; + mp_queue_push_nonstop(_exec_flood_coolant, mach_get_line()); +} + + +/* Override enables are kind of a mess in Gcode. This is an attempt to sort + * them out. See + * http://www.linuxcnc.org/docs/2.4/html/gcode_main.html#sec:M50:-Feed-Override + */ + +/// M48, M49 +void mach_override_enables(bool flag) { + mach.gm.feed_override_enable = flag; + mach.gm.spindle_override_enable = flag; +} + + +/// M50 +void mach_feed_override_enable(bool flag) { + if (parser.gf.parameter && fp_ZERO(parser.gn.parameter)) + mach.gm.feed_override_enable = false; + else { + if (parser.gf.parameter) mach.gm.feed_override = parser.gf.parameter; + mach.gm.feed_override_enable = true; + } +} + + +/// M51 +void mach_spindle_override_enable(bool flag) { + if (parser.gf.parameter && fp_ZERO(parser.gn.parameter)) + mach.gm.spindle_override_enable = false; + else { + if (parser.gf.parameter) mach.gm.spindle_override = parser.gf.parameter; + mach.gm.spindle_override_enable = true; + } +} + + +void mach_message(const char *message) { + status_message_P(0, STAT_LEVEL_INFO, STAT_OK, PSTR("%s"), message); +} + + +/* Program Functions (4.3.10) + * + * This group implements stop, start, end, and hold. + * It is extended beyond the NIST spec to handle various situations. + * + * mach_program_stop and mach_optional_program_stop are synchronous Gcode + * commands that are received through the interpreter. They cause all motion + * to stop at the end of the current command, including spindle motion. + * + * Note that the stop occurs at the end of the immediately preceding command + * (i.e. the stop is queued behind the last command). + * + * mach_program_end is a stop that also resets the machine to initial state + */ + + +static stat_t _exec_program_stop(mp_buffer_t *bf) { + // Machine should be stopped at this point. Go into hold so that a start is + // needed before executing further instructions. + mp_set_hold_reason(HOLD_REASON_PROGRAM_PAUSE); + mp_state_holding(); + return STAT_NOOP; // No move queued +} + + +/// M0 Queue a program stop +void mach_program_stop() { + mp_queue_push(_exec_program_stop, mach_get_line()); +} + + +static stat_t _exec_optional_program_stop(mp_buffer_t *bf) { + mp_state_optional_pause(); // Pause here if it was requested by the user + return STAT_NOOP; // No move queued +} + + +/// M1 +void mach_optional_program_stop() { + mp_queue_push(_exec_optional_program_stop, mach_get_line()); +} + + +static stat_t _exec_pallet_change_stop(mp_buffer_t *bf) { + // Emit pallet change signal + mp_set_hold_reason(HOLD_REASON_PALLET_CHANGE); + mp_state_holding(); + return STAT_NOOP; // No move queued +} + + +/// M60 +void mach_pallet_change_stop() { + mp_queue_push(_exec_pallet_change_stop, mach_get_line()); +} + + +/*** mach_program_end() implements M2 and M30. End behaviors are defined by + * NIST 3.6.1 are: + * + * 1. Axis offsets are set to zero (like G92.2) and origin offsets are set + * to the default (like G54) + * 2. Selected plane is set to PLANE_XY (like G17) + * 3. Distance mode is set to MODE_ABSOLUTE (like G90) + * 4. Feed rate mode is set to UNITS_PER_MINUTE (like G94) + * 5. Feed and speed overrides are set to ON (like M48) + * 6. Cutter compensation is turned off (like G40) + * 7. The spindle is stopped (like M5) + * 8. The current motion mode is set to G_1 (like G1) + * 9. Coolant is turned off (like M9) + * + * mach_program_end() implements things slightly differently: + * + * 1. Axis offsets are set to G92.1 CANCEL offsets + * (instead of using G92.2 SUSPEND Offsets) + * Set default coordinate system + * 2. Selected plane is set to default plane + * 3. Distance mode is set to MODE_ABSOLUTE (like G90) + * 4. Feed rate mode is set to UNITS_PER_MINUTE (like G94) + * 5. Not implemented + * 6. Not implemented + * 7. The spindle is stopped (like M5) + * 8. Motion mode is canceled like G80 (not set to G1) + * 9. Coolant is turned off (like M9) + * + Default INCHES or MM units mode is restored + */ + + +/// M2, M30 +void mach_program_end() { + mach_reset_origin_offsets(); // G92.1 - we do G91.1 instead of G92.2 + mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM); + mach_set_plane(GCODE_DEFAULT_PLANE); + mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE); + mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE); + mach_set_spindle_mode(SPINDLE_OFF); // M5 + mach_flood_coolant_control(false); // M9 + mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // G94 + mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); +} diff --git a/avr/src/machine.h b/avr/src/machine.h new file mode 100644 index 0000000..c6f5497 --- /dev/null +++ b/avr/src/machine.h @@ -0,0 +1,136 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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" + + +#define TO_MM(a) (mach_get_units() == INCHES ? (a) * MM_PER_INCH : a) + +// Model state getters and setters +uint32_t mach_get_line(); +float mach_get_feed_rate(); +bool mach_is_inverse_time_mode(); +feed_mode_t mach_get_feed_mode(); +float mach_get_feed_override(); +float mach_get_spindle_override(); +motion_mode_t mach_get_motion_mode(); +bool mach_is_rapid(); +plane_t mach_get_plane(); +units_t mach_get_units(); +coord_system_t mach_get_coord_system(); +bool mach_get_absolute_mode(); +path_mode_t mach_get_path_mode(); +bool mach_is_exact_stop(); +distance_mode_t mach_get_distance_mode(); +distance_mode_t mach_get_arc_distance_mode(); + +void mach_set_motion_mode(motion_mode_t motion_mode); +void mach_set_spindle_mode(spindle_mode_t spindle_mode); +void mach_set_spindle_speed(float speed); +void mach_set_absolute_mode(bool absolute_mode); +void mach_set_line(uint32_t line); + +// Coordinate systems and offsets +float mach_get_active_coord_offset(uint8_t axis); +void mach_update_work_offsets(); +const float *mach_get_position(); +void mach_set_position(const float position[]); +float mach_get_axis_position(uint8_t axis); + +// Critical helpers +void mach_calc_target(float target[], const float values[], const bool flags[]); +stat_t mach_test_soft_limits(float target[]); + +// machining functions defined by NIST [organized by NIST Gcode doc] + +// Initialization and termination (4.3.2) +void machine_init(); + +// Representation (4.3.3) +void mach_set_plane(plane_t plane); +void mach_set_units(units_t mode); +void mach_set_distance_mode(distance_mode_t mode); +void mach_set_arc_distance_mode(distance_mode_t mode); +void mach_set_coord_offsets(coord_system_t coord_system, float offset[], + bool flags[]); + +void mach_set_axis_position(unsigned axis, float position); +void mach_set_absolute_origin(float origin[], bool flags[]); + +stat_t mach_zero_all(); +stat_t mach_zero_axis(unsigned axis); + +void mach_set_coord_system(coord_system_t coord_system); +void mach_set_origin_offsets(float offset[], bool flags[]); +void mach_reset_origin_offsets(); +void mach_suspend_origin_offsets(); +void mach_resume_origin_offsets(); + +// Free Space Motion (4.3.4) +stat_t mach_plan_line(float target[]); +stat_t mach_rapid(float target[], bool flags[]); +void mach_set_g28_position(); +stat_t mach_goto_g28_position(float target[], bool flags[]); +void mach_set_g30_position(); +stat_t mach_goto_g30_position(float target[], bool flags[]); + +// Machining Attributes (4.3.5) +void mach_set_feed_rate(float feed_rate); +void mach_set_feed_mode(feed_mode_t mode); +void mach_set_path_mode(path_mode_t mode); + +// Machining Functions (4.3.6) see arc.h +stat_t mach_feed(float target[], bool flags[]); +stat_t mach_dwell(float seconds); + +// Spindle Functions (4.3.7) see spindle.h + +// Tool Functions (4.3.8) +void mach_select_tool(uint8_t tool); +void mach_change_tool(bool x); + +// Miscellaneous Functions (4.3.9) +void mach_mist_coolant_control(bool mist_coolant); +void mach_flood_coolant_control(bool flood_coolant); + +void mach_override_enables(bool flag); +void mach_feed_override_enable(bool flag); +void mach_spindle_override_enable(bool flag); + +void mach_message(const char *message); + +// Program Functions (4.3.10) +void mach_program_stop(); +void mach_optional_program_stop(); +void mach_pallet_change_stop(); +void mach_program_end(); diff --git a/avr/src/main.c b/avr/src/main.c new file mode 100644 index 0000000..973ebbd --- /dev/null +++ b/avr/src/main.c @@ -0,0 +1,102 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2013 - 2015 Robert Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "probing.h" +#include "homing.h" +#include "home.h" +#include "i2c.h" + +#include "plan/planner.h" +#include "plan/arc.h" +#include "plan/state.h" + +#include +#include + +#include +#include + + +int main() { + //wdt_enable(WDTO_250MS); TODO + + // Init + cli(); // disable interrupts + + hardware_init(); // hardware setup - must be first + usart_init(); // serial port + i2c_init(); // i2c port + drv8711_init(); // motor drivers + stepper_init(); // steppers + motor_init(); // motors + switch_init(); // switches + mp_init(); // motion planning + machine_init(); // gcode machine + vars_init(); // configuration variables + estop_init(); // emergency stop handler + command_init(); + + sei(); // enable interrupts + + // Splash + fprintf_P(stdout, PSTR("\n{\"firmware\": \"Buildbotics AVR\", " + "\"version\": \"" VERSION "\"}\n")); + + // Nominal segment time cannot be longer than maximum + if (MAX_SEGMENT_TIME < NOM_SEGMENT_TIME) ALARM(STAT_INTERNAL_ERROR); + + // Main loop + while (true) { + hw_reset_handler(); // handle hard reset requests + if (!estop_triggered()) { + mp_state_callback(); + mach_arc_callback(); // arc generation runs + home_callback(); + //mach_homing_callback(); // G28.2 continuation + mach_probe_callback(); // G38.2 continuation + } + command_callback(); // process next command + report_callback(); // report changes + wdt_reset(); + } + + return 0; +} diff --git a/avr/src/messages.def b/avr/src/messages.def new file mode 100644 index 0000000..c3f8ed6 --- /dev/null +++ b/avr/src/messages.def @@ -0,0 +1,114 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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") + +// Errors and warnings +STAT_MSG(MINIMUM_LENGTH_MOVE, "Move less than minimum length") +STAT_MSG(MINIMUM_TIME_MOVE, "Move less than minimum time") +STAT_MSG(MAXIMUM_TIME_MOVE, "Move greater than maximum time") +STAT_MSG(MACHINE_ALARMED, "Machine alarmed - Command not processed") +STAT_MSG(LIMIT_SWITCH_HIT, "Limit switch hit - Shutdown occurred") +STAT_MSG(SOFT_LIMIT_EXCEEDED, "Soft limit exceeded") +STAT_MSG(INVALID_AXIS, "Invalid axis") +STAT_MSG(EXPECTED_MOVE, "A move was expected but none was queued") + +// Homing +STAT_MSG(HOMING_CYCLE_FAILED, "Homing cycle failed") +STAT_MSG(HOMING_ERROR_BAD_OR_NO_AXIS, "Homing Error - Bad or no axis specified") +STAT_MSG(HOMING_ERROR_AXIS_MISCONFIGURED, "Homing Error - Axis misconfigured") +STAT_MSG(HOMING_ERROR_ZERO_SEARCH_VELOCITY, + "Homing Error - Search velocity is zero") +STAT_MSG(HOMING_ERROR_ZERO_LATCH_VELOCITY, + "Homing Error - Latch velocity is zero") +STAT_MSG(HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL, + "Homing Error - Travel min & max are the same") +STAT_MSG(HOMING_ERROR_NEGATIVE_LATCH_BACKOFF, + "Homing Error - Negative latch backoff") +STAT_MSG(HOMING_ERROR_SWITCH_MISCONFIGURATION, + "Homing Error - Homing switches misconfigured") + +// Probing +STAT_MSG(PROBE_INVALID_DEST, "Probing error - invalid destination") +STAT_MSG(MOVE_DURING_PROBE, "Probing error - move during probe") + +// End of stats marker +STAT_MSG(MAX, "") diff --git a/avr/src/motor.c b/avr/src/motor.c new file mode 100644 index 0000000..43221ae --- /dev/null +++ b/avr/src/motor.c @@ -0,0 +1,624 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "plan/runtime.h" +#include "plan/calibrate.h" + +#include +#include + +#include +#include +#include +#include + + +typedef enum { + MOTOR_IDLE, // motor stopped and may be partially energized + MOTOR_ENERGIZING, + MOTOR_ACTIVE +} motor_power_state_t; + + +typedef struct { + // Config + uint8_t axis; // map motor to axis + uint16_t microsteps; // microsteps per full step + bool reverse; + motor_power_mode_t power_mode; + float step_angle; // degrees per whole step + float travel_rev; // mm or deg of travel per motor revolution + uint8_t step_pin; + uint8_t dir_pin; + TC0_t *timer; + DMA_CH_t *dma; + uint8_t dma_trigger; + + // Runtime state + motor_power_state_t power_state; // state machine for managing motor power + uint32_t timeout; + motor_flags_t flags; + int32_t encoder; + int32_t commanded; + int32_t steps; // Currently used by the x-axis only + uint8_t last_clock; + bool wasEnabled; + int32_t error; + bool last_negative; // Last step sign + bool reading; + + // Move prep + uint8_t timer_clock; // clock divisor setting or zero for off + uint16_t timer_period; // clock period counter + bool negative; // step sign + direction_t direction; // travel direction corrected for polarity + int32_t position; + bool round_up; // toggle rounding direction + float power; +} motor_t; + + +static motor_t motors[MOTORS] = { + { + .axis = M1_AXIS, + .step_angle = M1_STEP_ANGLE, + .travel_rev = M1_TRAVEL_PER_REV, + .microsteps = M1_MICROSTEPS, + .reverse = M1_REVERSE, + .power_mode = M1_POWER_MODE, + .step_pin = STEP_X_PIN, + .dir_pin = DIR_X_PIN, + .timer = &M1_TIMER, + .dma = &M1_DMA_CH, + .dma_trigger = M1_DMA_TRIGGER, + }, { + .axis = M2_AXIS, + .step_angle = M2_STEP_ANGLE, + .travel_rev = M2_TRAVEL_PER_REV, + .microsteps = M2_MICROSTEPS, + .reverse = M2_REVERSE, + .power_mode = M2_POWER_MODE, + .step_pin = STEP_Y_PIN, + .dir_pin = DIR_Y_PIN, + .timer = &M2_TIMER, + .dma = &M2_DMA_CH, + .dma_trigger = M2_DMA_TRIGGER, + }, { + .axis = M3_AXIS, + .step_angle = M3_STEP_ANGLE, + .travel_rev = M3_TRAVEL_PER_REV, + .microsteps = M3_MICROSTEPS, + .reverse = M3_REVERSE, + .power_mode = M3_POWER_MODE, + .step_pin = STEP_Z_PIN, + .dir_pin = DIR_Z_PIN, + .timer = &M3_TIMER, + .dma = &M3_DMA_CH, + .dma_trigger = M3_DMA_TRIGGER, + }, { + .axis = M4_AXIS, + .step_angle = M4_STEP_ANGLE, + .travel_rev = M4_TRAVEL_PER_REV, + .microsteps = M4_MICROSTEPS, + .reverse = M4_REVERSE, + .power_mode = M4_POWER_MODE, + .step_pin = STEP_A_PIN, + .dir_pin = DIR_A_PIN, + .timer = (TC0_t *)&M4_TIMER, + .dma = &M4_DMA_CH, + .dma_trigger = M4_DMA_TRIGGER, + } +}; + + +static uint8_t _dummy; + + +void motor_init() { + // Enable DMA + DMA.CTRL = DMA_RESET_bm; + DMA.CTRL = DMA_ENABLE_bm; + DMA.INTFLAGS = 0xff; // clear all interrupts + + // Motor enable + OUTSET_PIN(MOTOR_ENABLE_PIN); // Low (disabled) + DIRSET_PIN(MOTOR_ENABLE_PIN); // Output + + for (int motor = 0; motor < MOTORS; motor++) { + motor_t *m = &motors[motor]; + + axis_set_motor(m->axis, motor); + + // IO pins + DIRSET_PIN(m->step_pin); // Output + DIRSET_PIN(m->dir_pin); // Output + + // Setup motor timer + m->timer->CTRLB = TC_WGMODE_FRQ_gc | TC1_CCAEN_bm; + + // Setup DMA channel as timer event counter + m->dma->ADDRCTRL = DMA_CH_SRCDIR_FIXED_gc | DMA_CH_DESTDIR_FIXED_gc; + m->dma->TRIGSRC = m->dma_trigger; + m->dma->REPCNT = 0; + + // Note, the DMA transfer must read CCA to clear the trigger + m->dma->SRCADDR0 = (((uintptr_t)&m->timer->CCA) >> 0) & 0xff; + m->dma->SRCADDR1 = (((uintptr_t)&m->timer->CCA) >> 8) & 0xff; + m->dma->SRCADDR2 = 0; + + m->dma->DESTADDR0 = (((uintptr_t)&_dummy) >> 0) & 0xff; + m->dma->DESTADDR1 = (((uintptr_t)&_dummy) >> 8) & 0xff; + m->dma->DESTADDR2 = 0; + + m->dma->CTRLB = 0; + m->dma->CTRLA = + DMA_CH_REPEAT_bm | DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc; + m->dma->CTRLA |= DMA_CH_ENABLE_bm; + + drv8711_set_microsteps(motor, m->microsteps); + } +} + + +void motor_enable(int motor, bool enable) { + if (enable) OUTSET_PIN(MOTOR_ENABLE_PIN); // Active high + else { + OUTCLR_PIN(MOTOR_ENABLE_PIN); + motors[motor].power_state = MOTOR_IDLE; + } +} + + +bool motor_is_enabled(int motor) { + return motors[motor].power_mode != MOTOR_DISABLED; +} + + +int motor_get_axis(int motor) {return motors[motor].axis;} + + +void motor_set_stall_callback(int motor, stall_callback_t cb) { + drv8711_set_stall_callback(motor, cb); +} + + +/// @return computed homing velocity +float motor_get_stall_homing_velocity(int motor) { + // Compute velocity: + // velocity = travel_rev * step_angle * 60 / (SMPLTH * mstep * 360 * 2) + // SMPLTH = 50us = 0.00005s + // mstep = 8 + return motors[motor].travel_rev * motors[motor].step_angle * 1667 / + motors[motor].microsteps; +} + + +float motor_get_steps_per_unit(int motor) { + return 360 * motors[motor].microsteps / motors[motor].travel_rev / + motors[motor].step_angle; +} + + +float motor_get_units_per_step(int motor) { + return motors[motor].travel_rev * motors[motor].step_angle / + motors[motor].microsteps / 360; +} + + +float _get_max_velocity(int motor) { + return + axis_get_velocity_max(motors[motor].axis) * motor_get_steps_per_unit(motor); +} + + +uint16_t motor_get_microsteps(int motor) {return motors[motor].microsteps;} + + +void motor_set_microsteps(int motor, uint16_t microsteps) { + switch (microsteps) { + case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256: + break; + default: return; + } + + motors[motor].microsteps = microsteps; + drv8711_set_microsteps(motor, microsteps); +} + + +int32_t motor_get_encoder(int motor) {return motors[motor].encoder;} + + +void motor_set_encoder(int motor, float encoder) { + //if (st_is_busy()) ALARM(STAT_INTERNAL_ERROR); TODO + + motor_t *m = &motors[motor]; + m->encoder = m->position = m->commanded = round(encoder); + m->error = 0; +} + + +int32_t motor_get_error(int motor) { + motor_t *m = &motors[motor]; + + m->reading = true; + int32_t error = motors[motor].error; + m->reading = false; + + return error; +} + + +int32_t motor_get_position(int motor) {return motors[motor].position;} + + +/// returns true if motor is in an error state +bool motor_error(int m) { + uint8_t flags = motors[m].flags; + if (calibrate_busy()) flags &= ~MOTOR_FLAG_STALLED_bm; + return flags & MOTOR_FLAG_ERROR_bm; +} + + +bool motor_stalled(int m) { + return motors[m].flags & MOTOR_FLAG_STALLED_bm; +} + + +void motor_reset(int m) { + motors[m].flags = 0; +} + + +/// Remove power from a motor +static void _deenergize(int m) { + if (motors[m].power_state == MOTOR_ACTIVE) { + motors[m].power_state = MOTOR_IDLE; + drv8711_disable(m); + } +} + + +/// Apply power to a motor +static void _energize(int m) { + if (motors[m].power_state == MOTOR_IDLE && !motor_error(m)) { + motors[m].power_state = MOTOR_ENERGIZING; + drv8711_enable(m); + + motor_driver_callback(m); // TODO Shouldn't call this directly + } + + // Reset timeout, regardless + motors[m].timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000; +} + + +bool motor_energizing() { + for (int m = 0; m < MOTORS; m++) + if (motors[m].power_state == MOTOR_ENERGIZING) + return true; + + return false; +} + + +void motor_driver_callback(int motor) { + motor_t *m = &motors[motor]; + + if (m->power_state == MOTOR_IDLE) m->flags &= ~MOTOR_FLAG_ENABLED_bm; + else if (!estop_triggered()) { + m->power_state = MOTOR_ACTIVE; + m->flags |= MOTOR_FLAG_ENABLED_bm; + motor_enable(motor, true); + } + + report_request(); +} + + +/// Callback to manage motor power sequencing and power-down timing. +stat_t motor_rtc_callback() { // called by controller + for (int motor = 0; motor < MOTORS; motor++) + // Deenergize motor if disabled or timedout + if (motors[motor].power_mode == MOTOR_DISABLED || + rtc_expired(motors[motor].timeout)) _deenergize(motor); + + return STAT_OK; +} + + +void motor_error_callback(int motor, motor_flags_t errors) { + motor_t *m = &motors[motor]; + + if (m->power_state != MOTOR_ACTIVE) return; + + m->flags |= errors & MOTOR_FLAG_ERROR_bm; + report_request(); + + if (motor_error(motor)) { + if (m->flags & MOTOR_FLAG_STALLED_bm) ALARM(STAT_MOTOR_STALLED); + if (m->flags & MOTOR_FLAG_OVER_TEMP_bm) ALARM(STAT_MOTOR_OVER_TEMP); + if (m->flags & MOTOR_FLAG_OVER_CURRENT_bm) ALARM(STAT_MOTOR_OVER_CURRENT); + if (m->flags & MOTOR_FLAG_DRIVER_FAULT_bm) ALARM(STAT_MOTOR_DRIVER_FAULT); + if (m->flags & MOTOR_FLAG_UNDER_VOLTAGE_bm) ALARM(STAT_MOTOR_UNDER_VOLTAGE); + } +} + + +void motor_load_move(int motor) { + motor_t *m = &motors[motor]; + + // Get actual step count from DMA channel + uint16_t steps = 0xffff - m->dma->TRFCNT; + m->dma->TRFCNT = 0xffff; // Reset DMA channel counter + m->dma->CTRLB = DMA_CH_CHBUSY_bm | DMA_CH_CHPEND_bm; + m->dma->CTRLA |= DMA_CH_ENABLE_bm; + + // Adjust clock count + if (m->last_clock) { + uint32_t count = m->timer->CNT; + int8_t freq_change = m->last_clock - m->timer_clock; + + count <<= freq_change; // Adjust count + + if (m->timer_period < count) count -= m->timer_period; + if (m->timer_period < count) count -= m->timer_period; + if (m->timer_period < count) count = m->timer_period / 2; + + m->timer->CNT = count; + + } else m->timer->CNT = m->timer_period / 2; + + // Set or zero runtime clock and period + m->timer->CCA = m->timer_period; // Set frequency + m->timer->CTRLA = m->timer_clock; // Start or stop + m->last_clock = m->timer_clock; + m->timer_clock = 0; // Clear clock + + // Set direction + if (m->direction == DIRECTION_CW) OUTCLR_PIN(m->dir_pin); + else OUTSET_PIN(m->dir_pin); + + // Accumulate encoder + if (!m->wasEnabled) steps = 0; + + m->encoder += m->last_negative ? -(int32_t)steps : steps; + m->last_negative = m->negative; + + // Compute error + if (!m->reading) m->error = m->commanded - m->encoder; + m->commanded = m->position; + + // Set power + drv8711_set_power(motor, m->power); +} + + +void motor_end_move(int motor) { + motor_t *m = &motors[motor]; + m->wasEnabled = m->dma->CTRLA & DMA_CH_ENABLE_bm; + m->dma->CTRLA &= ~DMA_CH_ENABLE_bm; + m->timer->CTRLA = 0; // Stop clock +} + + +stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error, + float time) { + motor_t *m = &motors[motor]; + + // Validate input + if (motor < 0 || MOTORS < motor) return ALARM(STAT_INTERNAL_ERROR); + if (clocks < 0) return ALARM(STAT_INTERNAL_ERROR); + if (isinf(target)) return ALARM(STAT_MOVE_TARGET_INFINITE); + if (isnan(target)) return ALARM(STAT_MOVE_TARGET_NAN); + + // Compute motor timer clock and period. Rounding is performed to eliminate + // a negative bias in the uint32_t conversion that results in long-term + // negative drift. + int32_t travel = round(target) - m->position + error; + uint32_t ticks_per_step = travel ? labs(clocks / 2 / travel) : 0; + m->position = round(target); + + // Setup the direction, compensating for polarity. + m->negative = travel < 0; + if (m->negative) m->direction = DIRECTION_CCW ^ m->reverse; + else m->direction = DIRECTION_CW ^ m->reverse; + + // Find the clock rate that will fit the required number of steps + if (ticks_per_step <= 0xffff) m->timer_clock = TC_CLKSEL_DIV1_gc; + else if (ticks_per_step <= 0x1ffff) m->timer_clock = TC_CLKSEL_DIV2_gc; + else if (ticks_per_step <= 0x3ffff) m->timer_clock = TC_CLKSEL_DIV4_gc; + else if (ticks_per_step <= 0x7ffff) m->timer_clock = TC_CLKSEL_DIV8_gc; + else m->timer_clock = 0; // Clock off, too slow + + // Note, we rely on the fact that TC_CLKSEL_DIV1_gc through TC_CLKSEL_DIV8_gc + // equal 1, 2, 3 & 4 respectively. + m->timer_period = ticks_per_step >> (m->timer_clock - 1); + + // Round up if DIV4 or DIV8 and the error is high enough + if (0xffff < ticks_per_step && m->timer_period < 0xffff) { + int8_t step_error = ticks_per_step & ((1 << (m->timer_clock - 1)) - 1); + int8_t half_error = 1 << (m->timer_clock - 2); + + if (step_error == half_error) { + if (m->round_up) m->timer_period++; + m->round_up = !m->round_up; + + } else if (half_error < step_error) m->timer_period++; + } + + if (!m->timer_period) m->timer_clock = 0; + if (!m->timer_clock) m->timer_period = 0; + + // Power motor + switch (m->power_mode) { + case MOTOR_DISABLED: break; + + case MOTOR_POWERED_ONLY_WHEN_MOVING: + if (!m->timer_clock) break; // Not moving + // Fall through + + case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: + _energize(motor); // TODO is ~5ms enough time to enable the motor? + break; + + default: break; // Shouldn't get here + } + + // Compute power from axis velocity + m->power = travel / (_get_max_velocity(motor) * time); + + return STAT_OK; +} + + +// Var callbacks +char get_axis_name(int motor) {return axis_get_char(motors[motor].axis);} + + +void set_axis_name(int motor, char axis) { + int id = axis_get_id(axis); + if (id != -1) motors[motor].axis = id; +} + + +float get_step_angle(int motor) {return motors[motor].step_angle;} +void set_step_angle(int motor, float value) {motors[motor].step_angle = value;} +float get_travel(int motor) {return motors[motor].travel_rev;} +void set_travel(int motor, float value) {motors[motor].travel_rev = value;} +uint16_t get_microstep(int motor) {return motors[motor].microsteps;} + + +void set_microstep(int motor, uint16_t value) { + if (motor < 0 || MOTORS <= motor) return; + motor_set_microsteps(motor, value); +} + + +bool get_reverse(int motor) { + if (motor < 0 || MOTORS <= motor) return 0; + return motors[motor].reverse; +} + + +void set_reverse(int motor, bool value) {motors[motor].reverse = value;} +uint8_t get_motor_axis(int motor) {return motors[motor].axis;} + + +void set_motor_axis(int motor, uint8_t value) { + if (MOTORS <= motor || AXES <= value || value == motors[motor].axis) return; + axis_set_motor(motors[motor].axis, -1); + motors[motor].axis = value; + axis_set_motor(value, motor); +} + + +uint8_t get_power_mode(int motor) {return motors[motor].power_mode;} + + +void set_power_mode(int motor, uint8_t value) { + if (value < MOTOR_POWER_MODE_MAX_VALUE) + motors[motor].power_mode = value; +} + + +uint8_t get_status_flags(int motor) {return motors[motor].flags;} + + +void print_status_flags(uint8_t flags) { + bool first = true; + + putchar('"'); + + if (MOTOR_FLAG_ENABLED_bm & flags) { + printf_P(PSTR("enable")); + first = false; + } + + if (MOTOR_FLAG_STALLED_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("stall")); + first = false; + } + + if (MOTOR_FLAG_OVER_TEMP_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("over temp")); + first = false; + } + + if (MOTOR_FLAG_OVER_CURRENT_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("over current")); + first = false; + } + + if (MOTOR_FLAG_DRIVER_FAULT_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("fault")); + first = false; + } + + if (MOTOR_FLAG_UNDER_VOLTAGE_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("uvlo")); + first = false; + } + + putchar('"'); +} + + +uint8_t get_status_strings(int m) {return get_status_flags(m);} + + +// Command callback +void command_mreset(int argc, char *argv[]) { + if (argc == 1) + for (int m = 0; m < MOTORS; m++) + motor_reset(m); + + else { + int m = atoi(argv[1]); + if (m < MOTORS) motor_reset(m); + } + + report_request(); +} diff --git a/avr/src/motor.h b/avr/src/motor.h new file mode 100644 index 0000000..3575afc --- /dev/null +++ b/avr/src/motor.h @@ -0,0 +1,93 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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_FLAG_ENABLED_bm = 1 << 0, + MOTOR_FLAG_STALLED_bm = 1 << 1, + MOTOR_FLAG_OVER_TEMP_bm = 1 << 2, + MOTOR_FLAG_OVER_CURRENT_bm = 1 << 3, + MOTOR_FLAG_DRIVER_FAULT_bm = 1 << 4, + MOTOR_FLAG_UNDER_VOLTAGE_bm = 1 << 5, + MOTOR_FLAG_ERROR_bm = (//MOTOR_FLAG_STALLED_bm | + MOTOR_FLAG_OVER_TEMP_bm | + MOTOR_FLAG_OVER_CURRENT_bm | + MOTOR_FLAG_DRIVER_FAULT_bm | + MOTOR_FLAG_UNDER_VOLTAGE_bm) +} motor_flags_t; + + +typedef enum { + MOTOR_DISABLED, // motor enable is deactivated + MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON + MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, + // de-powered out of cycle + MOTOR_POWERED_ONLY_WHEN_MOVING, // idles shortly after stopped, even in cycle + MOTOR_POWER_MODE_MAX_VALUE // for input range checking +} motor_power_mode_t; + + +typedef void (*stall_callback_t)(int motor); + + +void motor_init(); +void motor_enable(int motor, bool enable); + +bool motor_is_enabled(int motor); +int motor_get_axis(int motor); +void motor_set_stall_callback(int motor, stall_callback_t cb); +float motor_get_stall_homing_velocity(int motor); +float motor_get_steps_per_unit(int motor); +float motor_get_units_per_step(int motor); +uint16_t motor_get_microsteps(int motor); +void motor_set_microsteps(int motor, uint16_t microsteps); +int32_t motor_get_encoder(int motor); +void motor_set_encoder(int motor, float encoder); +int32_t motor_get_error(int motor); +int32_t motor_get_position(int motor); + +bool motor_error(int motor); +bool motor_stalled(int motor); +void motor_reset(int motor); + +bool motor_energizing(); + +void motor_driver_callback(int motor); +stat_t motor_rtc_callback(); +void motor_error_callback(int motor, motor_flags_t errors); + +void motor_load_move(int motor); +void motor_end_move(int motor); +stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error, + float time); diff --git a/avr/src/pins.c b/avr/src/pins.c new file mode 100644 index 0000000..99f8a55 --- /dev/null +++ b/avr/src/pins.c @@ -0,0 +1,31 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..c972333 --- /dev/null +++ b/avr/src/pins.h @@ -0,0 +1,47 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + + +enum {PORT_A = 1, PORT_B, PORT_C, PORT_D, PORT_E, PORT_F}; + +extern PORT_t *pin_ports[]; + +#define PORT(PIN) pin_ports[(PIN >> 3) - 1] +#define BM(PIN) (1 << (PIN & 7)) + +#define DIRSET_PIN(PIN) PORT(PIN)->DIRSET = BM(PIN) +#define DIRCLR_PIN(PIN) PORT(PIN)->DIRCLR = BM(PIN) +#define OUTCLR_PIN(PIN) PORT(PIN)->OUTCLR = BM(PIN) +#define OUTSET_PIN(PIN) PORT(PIN)->OUTSET = BM(PIN) +#define OUTTGL_PIN(PIN) PORT(PIN)->OUTTGL = BM(PIN) +#define OUT_PIN(PIN) (!!(PORT(PIN)->OUT & BM(PIN))) +#define IN_PIN(PIN) (!!(PORT(PIN)->IN & BM(PIN))) +#define PINCTRL_PIN(PIN) ((&PORT(PIN)->PIN0CTRL)[PIN & 7]) diff --git a/avr/src/plan/arc.c b/avr/src/plan/arc.c new file mode 100644 index 0000000..957a617 --- /dev/null +++ b/avr/src/plan/arc.c @@ -0,0 +1,507 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + + +typedef struct { + bool running; + + float target[AXES]; // XYZABC where the move should go + float position[AXES]; // end point of the current segment + + float theta; // total angle specified by arc + float radius; // Raw R value, or computed via offsets + + uint8_t plane_axis_0; // arc plane axis 0 - e.g. X for G17 + uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17 + uint8_t linear_axis; // linear axis (normal to plane) + + uint32_t segments; // count of running segments + float segment_theta; // angular motion per segment + float segment_linear_travel; // linear motion per segment + float center_0; // center at axis 0 (e.g. X for G17) + float center_1; // center at axis 1 (e.g. Y for G17) +} arc_t; + +arc_t arc = {0}; + + +/*** Returns a naive estimate of arc execution time to inform segment + * calculation. The arc time is computed not to exceed the time taken + * in the slowest dimension in the arc plane or in linear + * travel. Maximum feed rates are compared in each dimension, but the + * comparison assumes that the arc will have at least one segment + * where the unit vector is 1 in that dimension. This is not true for + * any arbitrary arc, with the result that the time returned may be + * less than optimal. + */ +static float _estimate_arc_time(float length, float linear_travel, + float planar_travel) { + // Determine move time at requested feed rate + // Inverse feed rate is normalized to minutes + float time = mach_is_inverse_time_mode() ? + mach_get_feed_rate() : length / mach_get_feed_rate(); + + + // Apply feed override + time /= mach_get_feed_override(); + + // Downgrade the time if there is a rate-limiting axis + return max4(time, planar_travel / axis_get_feedrate_max(arc.plane_axis_0), + planar_travel / axis_get_feedrate_max(arc.plane_axis_1), + fabs(linear_travel) / axis_get_feedrate_max(arc.linear_axis)); +} + + +/*** Compute arc center (offset) from radius. + * + * Needs to calculate the center of the circle that has the designated radius + * and passes through both the current position and the target position + * + * This method calculates the following set of equations where: + * + * [x,y] is the vector from current to target position, + * d == magnitude of that vector, + * h == hypotenuse of the triangle formed by the radius of the circle, + * the distance to the center of the travel vector. + * + * A vector perpendicular to the travel vector [-y,x] is scaled to the length + * of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] + * to form the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the + * center of the arc. + * + * d^2 == x^2 + y^2 + * h^2 == r^2 - (d/2)^2 + * i == x/2 - y/d*h + * j == y/2 + x/d*h + * O <- [i,j] + * - | + * r - | + * - | + * - | h + * - | + * [0,0] -> C -----------------+--------------- T <- [x,y] + * | <------ d/2 ---->| + * + * C - Current position + * T - Target position + * O - center of circle that pass through both C and T + * d - distance from C to T + * r - designated radius + * h - distance from center of CT to O + * + * Expanding the equations: + * + * d -> sqrt(x^2 + y^2) + * h -> sqrt(4 * r^2 - x^2 - y^2)/2 + * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + * + * Which can be written: + * + * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + * + * Which we for size and speed reasons optimize to: + * + * h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) + * i = (x - (y * h_x2_div_d))/2 + * j = (y + (x * h_x2_div_d))/2 + * + * Computing clockwise vs counter-clockwise motion + * + * The counter clockwise circle lies to the left of the target direction. + * When offset is positive the left hand circle will be generated - + * when it is negative the right hand circle is generated. + * + * T <-- Target position + * ^ + * Clockwise circles with | Clockwise circles with + * this center will have | this center will have + * > 180 deg of angular travel | < 180 deg of angular travel, + * \ | which is a good thing! + * \ | / + * center of arc when -> x <----- | -----> x <- center of arc when + * h_x2_div_d is positive | h_x2_div_d is negative + * | + * C <-- Current position + * + * + * Assumes arc singleton has been pre-loaded with target and position. + * Parts of this routine were originally sourced from the grbl project. + */ +static stat_t _compute_arc_offsets_from_radius(float offset[], bool clockwise) { + // Calculate the change in position along each selected axis + float x = + arc.target[arc.plane_axis_0] - mach_get_axis_position(arc.plane_axis_0); + float y = + arc.target[arc.plane_axis_1] - mach_get_axis_position(arc.plane_axis_1); + + // *** From Forrest Green - Other Machine Co, 3/27/14 + // If the distance between endpoints is greater than the arc diameter, disc + // will be negative indicating that the arc is offset into the complex plane + // beyond the reach of any real CNC. However, numerical errors can flip the + // sign of disc as it approaches zero (which happens as the arc angle + // approaches 180 degrees). To avoid mishandling these arcs we use the + // closest real solution (which will be 0 when disc <= 0). This risks + // obscuring g-code errors where the radius is actually too small (they will + // be treated as half circles), but ensures that all valid arcs end up + // reasonably close to their intended paths regardless of any numerical + // issues. + float disc = 4 * square(arc.radius) - (square(x) + square(y)); + + float h_x2_div_d = disc > 0 ? -sqrt(disc) / hypotf(x, y) : 0; + + // Invert sign of h_x2_div_d if circle is counter clockwise (see header notes) + if (!clockwise) h_x2_div_d = -h_x2_div_d; + + // Negative R is g-code-alese for "I want a circle with more than 180 degrees + // of travel" (go figure!), even though it is advised against ever generating + // such circles in a single line of g-code. By inverting the sign of + // h_x2_div_d the center of the circles is placed on the opposite side of + // the line of travel and thus we get the unadvisably long arcs as prescribed. + if (arc.radius < 0) h_x2_div_d = -h_x2_div_d; + + // Complete the operation by calculating the actual center of the arc + offset[arc.plane_axis_0] = (x - y * h_x2_div_d) / 2; + offset[arc.plane_axis_1] = (y + x * h_x2_div_d) / 2; + offset[arc.linear_axis] = 0; + + return STAT_OK; +} + + +/* Compute arc from I and J (arc center point) + * + * The theta calculation sets up an clockwise or counterclockwise arc + * from the current position to the target position around the center + * designated by the offset vector. All theta-values measured in + * radians of deviance from the positive y-axis. + * + * | <- theta == 0 + * * * * + * * * + * * * + * * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2) + * * / + * C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4)) + * + * Parts of this routine were originally sourced from the grbl project. + */ +static stat_t _compute_arc(bool radius_f, const float position[], + float offset[], float rotations, bool clockwise, + bool full_circle) { + // Compute radius. A non-zero radius value indicates a radius arc + if (radius_f) _compute_arc_offsets_from_radius(offset, clockwise); + else // compute start radius + arc.radius = hypotf(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]); + + // Test arc specification for correctness according to: + // http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc + // "It is an error if: when the arc is projected on the selected + // plane, the distance from the current point to the center differs + // from the distance from the end point to the center by more than + // (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)." + + // Compute end radius from the center of circle (offsets) to target endpoint + float end_0 = arc.target[arc.plane_axis_0] - + position[arc.plane_axis_0] - offset[arc.plane_axis_0]; + + float end_1 = arc.target[arc.plane_axis_1] - + position[arc.plane_axis_1] - offset[arc.plane_axis_1]; + + // end radius - start radius + float err = fabs(hypotf(end_0, end_1) - arc.radius); + + if (err > ARC_RADIUS_ERROR_MAX || + (err < ARC_RADIUS_ERROR_MIN && err > arc.radius * ARC_RADIUS_TOLERANCE)) + return STAT_ARC_SPECIFICATION_ERROR; + + // Calculate the theta (angle) of the current point (position) + // arc.theta is angular starting point for the arc (also needed later for + // calculating center point) + arc.theta = atan2(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]); + + // g18_correction is used to invert G18 XZ plane arcs for proper CW + // orientation + float g18_correction = mach_get_plane() == PLANE_XZ ? -1 : 1; + float angular_travel = 0; + + if (full_circle) { + // angular travel always starts as zero for full circles + angular_travel = 0; + + // handle the valid case of a full circle arc w/ P=0 + if (fp_ZERO(rotations)) rotations = 1.0; + + } else { + float theta_end = atan2(end_0, end_1); + + // Compute the angular travel + if (fp_EQ(theta_end, arc.theta)) + // very large radii arcs can have zero angular travel (thanks PartKam) + angular_travel = 0; + + else { + // make the difference positive so we have clockwise travel + if (theta_end < arc.theta) theta_end += 2 * M_PI * g18_correction; + + // compute positive angular travel + angular_travel = theta_end - arc.theta; + + // reverse travel direction if it's CCW arc + if (!clockwise) angular_travel -= 2 * M_PI * g18_correction; + } + } + + // Add in travel for rotations + if (clockwise) angular_travel += 2 * M_PI * rotations * g18_correction; + else angular_travel -= 2 * M_PI * rotations * g18_correction; + + // Calculate travel in the depth axis of the helix and compute the time it + // should take to perform the move length is the total mm of travel of + // the helix (or just a planar arc) + float linear_travel = arc.target[arc.linear_axis] - position[arc.linear_axis]; + float planar_travel = angular_travel * arc.radius; + // hypot is insensitive to +/- signs + float length = hypotf(planar_travel, linear_travel); + + // trap zero length arcs that _compute_arc can throw + if (fp_ZERO(length)) return STAT_MINIMUM_LENGTH_MOVE; + + // get an estimate of execution time to inform segment calculation + float arc_time = _estimate_arc_time(length, linear_travel, planar_travel); + + // Find the minimum number of segments that meets these constraints... + float segments_for_chordal_accuracy = + length / sqrt(4 * CHORDAL_TOLERANCE * (2 * arc.radius - CHORDAL_TOLERANCE)); + float segments_for_minimum_distance = length / ARC_SEGMENT_LENGTH; + float segments_for_minimum_time = + arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC; + + float segments = floor(min3(segments_for_chordal_accuracy, + segments_for_minimum_distance, + segments_for_minimum_time)); + if (segments < 1) segments = 1; // at least 1 segment + + arc.segments = (uint32_t)segments; + arc.segment_theta = angular_travel / segments; + arc.segment_linear_travel = linear_travel / segments; + arc.center_0 = position[arc.plane_axis_0] - sin(arc.theta) * arc.radius; + arc.center_1 = position[arc.plane_axis_1] - cos(arc.theta) * arc.radius; + + // initialize the linear position + arc.position[arc.linear_axis] = position[arc.linear_axis]; + + return STAT_OK; +} + + +/*** Machine entry point for arc + * + * Generates an arc by queuing line segments to the move buffer. The arc is + * approximated by generating a large number of tiny, linear segments. + */ +stat_t mach_arc_feed(float values[], bool values_f[], // arc endpoints + float offsets[], bool offsets_f[], // arc offsets + float radius, bool radius_f, // radius + float P, bool P_f, // parameter + bool modal_g1_f, + motion_mode_t motion_mode) { // defined motion mode + + // Trap some precursor cases. Since motion mode (MODAL_GROUP_G1) persists + // from the previous move it's possible for non-modal commands such as F or P + // to arrive here when no motion has actually been specified. It's also + // possible to run an arc as simple as "I25" if CW or CCW motion mode was + // already set by a previous block. Here are 2 cases to handle if CW or CCW + // motion mode was set by a previous block: + // + // Case 1: F, P or other non modal is specified but no movement is specified + // (no offsets or radius). This is OK: return STAT_OK + // + // Case 2: Movement is specified w/o a new G2 or G3 word in the (new) block. + // This is OK: continue the move + // + if (!modal_g1_f && !offsets_f[0] && !offsets_f[1] && !offsets_f[2] && + !radius_f) return STAT_OK; + + // trap missing feed rate + if (fp_ZERO(mach_get_feed_rate()) || + (mach_is_inverse_time_mode() && !parser.gf.feed_rate)) + return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; + + // radius must be positive and > minimum + if (radius_f && radius < MIN_ARC_RADIUS) + return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; + + // Set the arc plane for the current G17/G18/G19 setting and test arc + // specification Plane axis 0 and 1 are the arc plane, the linear axis is + // normal to the arc plane. + switch (mach_get_plane()) { + case PLANE_XY: // G17 + arc.plane_axis_0 = AXIS_X; + arc.plane_axis_1 = AXIS_Y; + arc.linear_axis = AXIS_Z; + break; + + case PLANE_XZ: // G18 + arc.plane_axis_0 = AXIS_X; + arc.plane_axis_1 = AXIS_Z; + arc.linear_axis = AXIS_Y; + break; + + case PLANE_YZ: // G19 + arc.plane_axis_0 = AXIS_Y; + arc.plane_axis_1 = AXIS_Z; + arc.linear_axis = AXIS_X; + break; + } + + bool clockwise = motion_mode == MOTION_MODE_CW_ARC; + + // Test if endpoints are specified in the selected plane + bool full_circle = false; + if (!values_f[arc.plane_axis_0] && !values_f[arc.plane_axis_1]) { + if (radius_f) // in radius mode arcs missing both endpoints is an error + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; + else full_circle = true; // in center format arc specifies full circle + } + + // Test radius arcs for radius tolerance + if (radius_f) { + arc.radius = TO_MM(radius); // set to internal format (mm) + if (fabs(arc.radius) < MIN_ARC_RADIUS) // radius value must be > minimum + return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; + + // Test that center format absolute distance mode arcs have both offsets + } else if (mach_get_arc_distance_mode() == ABSOLUTE_MODE && + !(offsets_f[arc.plane_axis_0] && offsets_f[arc.plane_axis_1])) + return STAT_ARC_OFFSETS_MISSING_FOR_PLANE; + + // Set arc rotations + float rotations = 0; + + if (P_f) { + // If P is present it must be a positive integer + if (P < 0 || 0 < floor(P) - P) return STAT_P_WORD_IS_NOT_A_POSITIVE_INTEGER; + + rotations = P; + + } else if (full_circle) rotations = 1; // default to 1 for full circles + + // Set model target + const float *position = mach_get_position(); + mach_calc_target(arc.target, values, values_f); + + // in radius mode it's an error for start == end + if (radius_f && fp_EQ(position[AXIS_X], arc.target[AXIS_X]) && + fp_EQ(position[AXIS_Y], arc.target[AXIS_Y]) && + fp_EQ(position[AXIS_Z], arc.target[AXIS_Z])) + return STAT_ARC_ENDPOINT_IS_STARTING_POINT; + + // now get down to the rest of the work setting up the arc for execution + mach_set_motion_mode(motion_mode); + mach_update_work_offsets(); // Update resolved offsets + arc.radius = TO_MM(radius); // set arc radius or zero + + float offset[3]; + for (int i = 0; i < 3; i++) offset[i] = TO_MM(offsets[i]); + + if (mach_get_arc_distance_mode() == ABSOLUTE_MODE) { + if (offsets_f[0]) offset[0] -= position[0]; + if (offsets_f[1]) offset[1] -= position[1]; + if (offsets_f[2]) offset[2] -= position[2]; + } + + // compute arc runtime values + RITORNO(_compute_arc + (radius_f, position, offset, rotations, clockwise, full_circle)); + + // Note, arc soft limits are not tested here + + arc.running = true; // Enable arc run in callback + mach_arc_callback(); // Queue initial arc moves + mach_set_position(arc.target); // update model position + + return STAT_OK; +} + + +/*** Generate an arc + * + * Called from the controller main loop. Each time it's called it queues + * as many arc segments (lines) as it can before it blocks, then returns. + */ +void mach_arc_callback() { + while (arc.running && mp_queue_get_room()) { + if (arc.segments == 1) { // Final segment + arc.position[arc.plane_axis_0] = arc.target[arc.plane_axis_0]; + arc.position[arc.plane_axis_1] = arc.target[arc.plane_axis_1]; + arc.position[arc.linear_axis] = arc.target[arc.linear_axis]; + + } else { + arc.theta += arc.segment_theta; + + arc.position[arc.plane_axis_0] = + arc.center_0 + sin(arc.theta) * arc.radius; + arc.position[arc.plane_axis_1] = + arc.center_1 + cos(arc.theta) * arc.radius; + arc.position[arc.linear_axis] += arc.segment_linear_travel; + } + + // run the line + mach_plan_line(arc.position); + + if (!--arc.segments) arc.running = false; + } +} + + +bool mach_arc_active() {return arc.running;} + + +/// Stop arc movement without maintaining position +/// OK to call if no arc is running +void mach_abort_arc() {arc.running = false;} diff --git a/avr/src/plan/arc.h b/avr/src/plan/arc.h new file mode 100644 index 0000000..943aa9e --- /dev/null +++ b/avr/src/plan/arc.h @@ -0,0 +1,48 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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(); +bool mach_arc_active(); +void mach_abort_arc(); diff --git a/avr/src/plan/buffer.c b/avr/src/plan/buffer.c new file mode 100644 index 0000000..45146d7 --- /dev/null +++ b/avr/src/plan/buffer.c @@ -0,0 +1,156 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2012 - 2015 Rob Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 { + uint8_t space; + mp_buffer_t *tail; + mp_buffer_t *head; + mp_buffer_t bf[PLANNER_BUFFER_POOL_SIZE]; +} buffer_pool_t; + + +buffer_pool_t mb; + + +/// Zeroes the contents of a buffer +static void _clear_buffer(mp_buffer_t *bf) { + mp_buffer_t *next = bf->next; // save pointers + mp_buffer_t *prev = bf->prev; + memset(bf, 0, sizeof(mp_buffer_t)); + bf->next = next; // restore pointers + bf->prev = prev; +} + + +static void _push() { + if (!mb.space) { + ALARM(STAT_INTERNAL_ERROR); + return; + } + + mb.tail = mb.tail->next; + mb.space--; +} + + +static void _pop() { + if (mb.space == PLANNER_BUFFER_POOL_SIZE) { + ALARM(STAT_INTERNAL_ERROR); + return; + } + + mb.head = mb.head->next; + mb.space++; +} + + +/// Initializes or resets buffers +void mp_queue_init() { + memset(&mb, 0, sizeof(mb)); // clear all values + + mb.tail = mb.head = &mb.bf[0]; // init head and tail + mb.space = PLANNER_BUFFER_POOL_SIZE; + + // Setup ring pointers + for (int i = 0; i < mb.space; i++) { + mb.bf[i].next = &mb.bf[i + 1]; + mb.bf[i].prev = &mb.bf[i - 1]; + } + + mb.bf[0].prev = &mb.bf[mb.space -1]; // Fix first->prev + mb.bf[mb.space - 1].next = &mb.bf[0]; // Fix last->next + + mp_state_idle(); +} + + +uint8_t mp_queue_get_room() { + return mb.space < PLANNER_BUFFER_HEADROOM ? + 0 : mb.space - PLANNER_BUFFER_HEADROOM; +} + + +uint8_t mp_queue_get_fill() { + return PLANNER_BUFFER_POOL_SIZE - mb.space; +} + + +bool mp_queue_is_empty() {return mb.tail == mb.head;} + + +/// Get pointer to next buffer, waiting until one is available. +mp_buffer_t *mp_queue_get_tail() { + while (!mb.space) continue; // Wait for a buffer + return mb.tail; +} + + +/*** Commit the next buffer to the queue. + * + * WARNING: The routine calling mp_queue_push() must not use the write + * buffer once it has been queued. Action may start on the buffer immediately, + * invalidating its contents + */ +void mp_queue_push(buffer_cb_t cb, uint32_t line) { + mp_state_running(); + + mb.tail->ts = rtc_get_time(); + mb.tail->cb = cb; + mb.tail->line = line; + mb.tail->state = BUFFER_NEW; + + _push(); +} + + +mp_buffer_t *mp_queue_get_head() { + return mp_queue_is_empty() ? 0 : mb.head; +} + + +/// Clear and release buffer to pool +void mp_queue_pop() { + _clear_buffer(mb.head); + _pop(); +} diff --git a/avr/src/plan/buffer.h b/avr/src/plan/buffer.h new file mode 100644 index 0000000..8554fdb --- /dev/null +++ b/avr/src/plan/buffer.h @@ -0,0 +1,102 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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; + + +// Callbacks +struct mp_buffer_t; +typedef stat_t (*buffer_cb_t)(struct mp_buffer_t *bf); + + +typedef struct mp_buffer_t { // See Planning Velocity Notes + struct mp_buffer_t *prev; // pointer to previous buffer + struct mp_buffer_t *next; // pointer to next buffer + + uint32_t ts; // Time stamp + int32_t line; // gcode block line number + buffer_cb_t cb; // callback to buffer exec function + + buffer_state_t state; // buffer state + bool replannable; // true if move can be re-planned + bool hold; // hold at the start of this block + + float value; // used in dwell and other callbacks + + float target[AXES]; // XYZABC where the move should go + float unit[AXES]; // unit vector for axis scaling & planning + + float length; // total length of line or helix in mm + float head_length; + float body_length; + float tail_length; + + // See notes on these variables, in mp_aline() + float entry_velocity; // entry velocity requested for the move + float cruise_velocity; // cruise velocity requested & achieved + float exit_velocity; // exit velocity requested for the move + float braking_velocity; // current value for braking velocity + + float entry_vmax; // max junction velocity at entry of this move + float cruise_vmax; // max cruise velocity requested for move + float exit_vmax; // max exit velocity possible (redundant) + float delta_vmax; // max velocity difference for this move + + float jerk; // maximum linear jerk term for this move + float cbrt_jerk; // cube root of Jm (computed & cached) +} mp_buffer_t; + + +void mp_queue_init(); + +uint8_t mp_queue_get_room(); +uint8_t mp_queue_get_fill(); + +bool mp_queue_is_empty(); + +mp_buffer_t *mp_queue_get_tail(); +void mp_queue_push(buffer_cb_t func, uint32_t line); + +mp_buffer_t *mp_queue_get_head(); +void mp_queue_pop(); + +static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->prev;} +static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->next;} diff --git a/avr/src/plan/calibrate.c b/avr/src/plan/calibrate.c new file mode 100644 index 0000000..ee83d76 --- /dev/null +++ b/avr/src/plan/calibrate.c @@ -0,0 +1,175 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + + +#define CAL_VELOCITIES 256 +#define CAL_MIN_VELOCITY 1000 // mm/sec +#define CAL_TARGET_SG 100 +#define CAL_MAX_DELTA_SG 75 +#define CAL_WAIT_TIME 3 // ms + + +enum { + CAL_START, + CAL_ACCEL, + CAL_MEASURE, + CAL_DECEL, +}; + + +typedef struct { + bool stall_valid; + bool stalled; + bool reverse; + + uint32_t wait; + int state; + int motor; + int axis; + + float velocity; + uint16_t stallguard; +} calibrate_t; + +static calibrate_t cal = {0}; + + +static stat_t _exec_calibrate(mp_buffer_t *bf) { + const float time = MIN_SEGMENT_TIME; // In minutes + const float max_delta_v = CAL_ACCELERATION * time; + + do { + if (rtc_expired(cal.wait)) + switch (cal.state) { + case CAL_START: { + cal.axis = motor_get_axis(cal.motor); + cal.state = CAL_ACCEL; + cal.velocity = 0; + cal.stall_valid = false; + cal.stalled = false; + cal.reverse = false; + + //tmc2660_set_stallguard_threshold(cal.motor, 8); + cal.wait = rtc_get_time() + CAL_WAIT_TIME; + + break; + } + + case CAL_ACCEL: + if (CAL_MIN_VELOCITY < cal.velocity) cal.stall_valid = true; + + if (cal.velocity < CAL_MIN_VELOCITY || CAL_TARGET_SG < cal.stallguard) + cal.velocity += max_delta_v; + + if (cal.stalled) { + if (cal.reverse) { + int32_t steps = -motor_get_encoder(cal.motor); + float mm = (float)steps / motor_get_steps_per_unit(cal.motor); + STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm); + + //tmc2660_set_stallguard_threshold(cal.motor, 63); + + mp_set_cycle(CYCLE_MACHINING); // Default cycle + + return STAT_NOOP; // Done, no move queued + + } else { + motor_set_encoder(cal.motor, 0); + + cal.reverse = true; + cal.velocity = 0; + cal.stall_valid = false; + cal.stalled = false; + } + } + break; + } + } while (fp_ZERO(cal.velocity)); // Repeat if computed velocity was zero + + // Compute travel + float travel[AXES] = {0}; // In mm + travel[cal.axis] = time * cal.velocity * (cal.reverse ? -1 : 1); + + // Convert to steps + float steps[MOTORS] = {0}; + mp_kinematics(travel, steps); + + // Queue segment + st_prep_line(time, steps, 0); + + return STAT_EAGAIN; +} + + +bool calibrate_busy() {return mp_get_cycle() == CYCLE_CALIBRATING;} + + +void calibrate_set_stallguard(int motor, uint16_t sg) { + if (cal.motor != motor) return; + + if (cal.stall_valid) { + int16_t delta = sg - cal.stallguard; + if (!sg || CAL_MAX_DELTA_SG < abs(delta)) { + cal.stalled = true; + motor_end_move(cal.motor); + } + } + + cal.stallguard = sg; +} + + +uint8_t command_calibrate(int argc, char *argv[]) { + if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY) + return 0; + + // Start + memset(&cal, 0, sizeof(cal)); + mp_set_cycle(CYCLE_CALIBRATING); + cal.motor = 1; + + mp_queue_push_nonstop(_exec_calibrate, -1); + + return 0; +} diff --git a/avr/src/plan/calibrate.h b/avr/src/plan/calibrate.h new file mode 100644 index 0000000..4d97c60 --- /dev/null +++ b/avr/src/plan/calibrate.h @@ -0,0 +1,35 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..07bba61 --- /dev/null +++ b/avr/src/plan/dwell.c @@ -0,0 +1,54 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2012 - 2015 Rob Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..9d94555 --- /dev/null +++ b/avr/src/plan/dwell.h @@ -0,0 +1,32 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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" + +stat_t mp_dwell(float seconds, int32_t line); diff --git a/avr/src/plan/exec.c b/avr/src/plan/exec.c new file mode 100644 index 0000000..3ae0e03 --- /dev/null +++ b/avr/src/plan/exec.c @@ -0,0 +1,450 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2012 - 2015 Rob Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "forward_dif.h" +#include "stepper.h" +#include "motor.h" +#include "rtc.h" +#include "usart.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; + + uint32_t segment_count; // count of running segments + float segment_velocity; // computed velocity for segment + float segment_time; // actual time increment per segment + forward_dif_t fdif; // forward difference levels + bool hold_planned; // true when a feedhold has been planned + move_section_t section; // current move section + bool section_new; // true if it's a new section + bool abort; +} mp_exec_t; + + +static mp_exec_t ex = {{0}}; + + +static stat_t _exec_aline_segment() { + float target[AXES]; + + // Set target position for the segment. If the segment ends on a section + // waypoint, synchronize to the head, body or tail end. Otherwise, if not at + // a section waypoint compute target from segment time and velocity. Don't + // do waypoint correction if you are going into a hold. + if (!--ex.segment_count && !ex.section_new && !ex.hold_planned) + copy_vector(target, ex.waypoint[ex.section]); + + else { + float segment_length = ex.segment_velocity * ex.segment_time; + + for (int axis = 0; axis < AXES; axis++) + target[axis] = + mp_runtime_get_axis_position(axis) + ex.unit[axis] * segment_length; + } + + mp_runtime_set_velocity(ex.segment_velocity); + RITORNO(mp_runtime_move_to_target(target, ex.segment_time)); + + // Return EAGAIN to continue or OK if this segment is done + return ex.segment_count ? STAT_EAGAIN : STAT_OK; +} + + +/// Common code for head and tail sections +static stat_t _exec_aline_section(float length, float vin, float vout) { + if (ex.section_new) { + if (fp_ZERO(length)) return STAT_NOOP; // end the section + + // len / avg. velocity + float move_time = 2 * length / (vin + vout); + float segments = ceil(move_time / NOM_SEGMENT_TIME); + ex.segment_time = move_time / segments; + ex.segment_count = round(segments); + + if (vin == vout) ex.segment_velocity = vin; + else ex.segment_velocity = + mp_init_forward_dif(ex.fdif, vin, vout, segments); + + if (ex.segment_time < MIN_SEGMENT_TIME) + return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position + + // First segment + if (_exec_aline_segment() == STAT_OK) { + ex.section_new = false; + return STAT_OK; + } + + ex.section_new = false; + + } else { + if (vin != vout) ex.segment_velocity += mp_next_forward_dif(ex.fdif); + + // Subsequent segments + if (_exec_aline_segment() == STAT_OK) return STAT_OK; + } + + return STAT_EAGAIN; +} + + +/// Callback for tail section +static stat_t _exec_aline_tail() { + ex.section = SECTION_TAIL; + return + _exec_aline_section(ex.tail_length, ex.cruise_velocity, ex.exit_velocity); +} + + +/// Callback for body section +static stat_t _exec_aline_body() { + ex.section = SECTION_BODY; + + stat_t status = + _exec_aline_section(ex.body_length, ex.cruise_velocity, ex.cruise_velocity); + + switch (status) { + case STAT_NOOP: return _exec_aline_tail(); + case STAT_OK: + ex.section = SECTION_TAIL; + ex.section_new = true; + + return STAT_EAGAIN; + + default: return status; + } +} + + +/// Callback for head section +static stat_t _exec_aline_head() { + ex.section = SECTION_HEAD; + stat_t status = + _exec_aline_section(ex.head_length, ex.entry_velocity, ex.cruise_velocity); + + switch (status) { + case STAT_NOOP: return _exec_aline_body(); + case STAT_OK: + ex.section = SECTION_BODY; + ex.section_new = true; + + return STAT_EAGAIN; + + default: return status; + } +} + + +static float _compute_next_segment_velocity() { + if (ex.section_new) { + if (ex.section == SECTION_HEAD) return mp_runtime_get_velocity(); + else return ex.cruise_velocity; + } + + return ex.segment_velocity + + (ex.section == SECTION_BODY ? 0 : mp_next_forward_dif(ex.fdif)); +} + + +/// Replan current move to execute hold +/// +/// Holds are initiated by the planner entering STATE_STOPPING. In which case +/// _plan_hold() is called to replan the current move towards zero. If it is +/// unable to plan to zero in the remaining length of the current move it will +/// decelerate as much as possible and then wait for the next move. Once it is +/// possible to plan to zero velocity in the current move the remaining length +/// is put into the run buffer, which is still allocated, and the run buffer +/// becomes the hold point. The hold is left by a start request in state.c. At +/// this point the remaining buffers, if any, are replanned from zero up to +/// speed. +static void _plan_hold() { + mp_buffer_t *bf = mp_queue_get_head(); // working buffer pointer + if (!bf) return; // Oops! nothing's running + + // Examine and process current buffer and compute length left for decel + float available_length = + axis_get_vector_length(ex.final_target, mp_runtime_get_position()); + // Compute next_segment velocity, velocity left to shed to brake to zero + float braking_velocity = _compute_next_segment_velocity(); + // Distance to brake to zero from braking_velocity, bf is OK to use here + float braking_length = mp_get_target_length(braking_velocity, 0, bf->jerk); + + // Hack to prevent Case 2 moves for perfect-fit decels. Happens when homing. + if (available_length < braking_length && fp_ZERO(bf->exit_velocity)) + braking_length = available_length; + + // Replan to decelerate + ex.section = SECTION_TAIL; + ex.section_new = true; + ex.cruise_velocity = braking_velocity; + ex.hold_planned = true; + + // Avoid creating segments before or after the hold which are too small. + if (fabs(available_length - braking_length) < HOLD_DECELERATION_TOLERANCE) { + // Case 0: deceleration fits almost exactly + ex.exit_velocity = 0; + ex.tail_length = available_length; + + } else if (braking_length <= available_length) { + // Case 1: deceleration fits entirely into the remaining length + // Setup tail to perform the deceleration + ex.exit_velocity = 0; + ex.tail_length = braking_length; + + // Re-use bf to run the remaining block length + bf->length = available_length - braking_length; + bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); + bf->entry_vmax = 0; + bf->state = BUFFER_RESTART; // Restart the buffer when done + + } else { + // Case 2: deceleration exceeds length remaining in buffer + // Replan to minimum (but non-zero) exit velocity + ex.tail_length = available_length; + ex.exit_velocity = + braking_velocity - mp_get_target_velocity(0, available_length, bf); + } +} + + +/// Initializes move runtime with a new planner buffer +static stat_t _exec_aline_init(mp_buffer_t *bf) { + // Remove zero length lines. Short lines have already been removed. + if (fp_ZERO(bf->length)) return STAT_NOOP; + + // Initialize move + copy_vector(ex.unit, bf->unit); + copy_vector(ex.final_target, bf->target); + + ex.head_length = bf->head_length; + ex.body_length = bf->body_length; + ex.tail_length = bf->tail_length; + ex.entry_velocity = bf->entry_velocity; + ex.cruise_velocity = bf->cruise_velocity; + ex.exit_velocity = bf->exit_velocity; + + ex.section = SECTION_HEAD; + ex.section_new = true; + ex.hold_planned = false; + + // Generate waypoints for position correction at section ends. This helps + // negate floating point errors in the forward differencing code. + for (int axis = 0; axis < AXES; axis++) { + float position = mp_runtime_get_axis_position(axis); + + ex.waypoint[SECTION_HEAD][axis] = position + ex.unit[axis] * ex.head_length; + ex.waypoint[SECTION_BODY][axis] = position + + ex.unit[axis] * (ex.head_length + ex.body_length); + ex.waypoint[SECTION_TAIL][axis] = ex.final_target[axis]; + } + + return STAT_OK; +} + + +void mp_exec_abort() {ex.abort = true;} + + +/// Aline execution routines +/// +/// Everything here fires from interrupts and must be interrupt safe +/// +/// Returns: +/// +/// STAT_OK move is done +/// STAT_EAGAIN move is not finished - has more segments to run +/// STAT_NOOP cause no stepper operation - do not load the move +/// STAT_xxxxx fatal error. Ends the move and frees the bf buffer +/// +/// This routine is called from the (LO) interrupt level. The interrupt +/// sequencing relies on the correct behavior of these routines. +/// Each call to _exec_aline() must execute and prep *one and only one* +/// segment. If the segment is not the last segment in the bf buffer the +/// _aline() returns STAT_EAGAIN. If it's the last segment it returns +/// STAT_OK. If it encounters a fatal error that would terminate the move it +/// returns a valid error code. +/// +/// Notes: +/// +/// [1] Returning STAT_OK ends the move and frees the bf buffer. +/// Returning STAT_OK at does NOT advance position meaning +/// any position error will be compensated by the next move. +/// +/// Operation: +/// +/// Aline generates jerk-controlled S-curves as per Ed Red's course notes: +/// +/// http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf +/// http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations +/// +/// A full trapezoid is divided into 5 periods. Periods 1 and 2 are the +/// first and second halves of the acceleration ramp (the concave and convex +/// parts of the S curve in the "head"). Periods 3 and 4 are the first +/// and second parts of the deceleration ramp (the tail). There is also +/// a period for the constant-velocity plateau of the trapezoid (the body). +/// There are many possible degenerate trapezoids where any of the 5 periods +/// may be zero length but note that either none or both of a ramping pair can +/// be zero. +/// +/// The equations that govern the acceleration and deceleration ramps are: +/// +/// Period 1 V = Vi + Jm * (T^2) / 2 +/// Period 2 V = Vh + As * T - Jm * (T^2) / 2 +/// Period 3 V = Vi - Jm * (T^2) / 2 +/// Period 4 V = Vh + As * T + Jm * (T^2) / 2 +/// +/// move_time is the actual time of the move, accel_time is the time value +/// needed to compute the velocity taking the initial velocity into account. +/// move_time does not need to. +stat_t mp_exec_aline(mp_buffer_t *bf) { + stat_t status = STAT_OK; + + if (ex.abort) { + ex.abort = false; + mp_runtime_set_velocity(0); // Assume a hard stop + return STAT_NOOP; + } + + // Start a new move + if (bf->state == BUFFER_INIT) { + bf->state = BUFFER_ACTIVE; + status = _exec_aline_init(bf); + if (status != STAT_OK) return status; + } + + // Plan holds + if (mp_get_state() == STATE_STOPPING && !ex.hold_planned) _plan_hold(); + + // Main segment dispatch + switch (ex.section) { + case SECTION_HEAD: status = _exec_aline_head(); break; + case SECTION_BODY: status = _exec_aline_body(); break; + case SECTION_TAIL: status = _exec_aline_tail(); break; + } + + // Set runtime velocity on exit + if (status != STAT_EAGAIN) mp_runtime_set_velocity(ex.exit_velocity); + + return status; +} + + +/// Dequeues buffers, initializes them, executes their callbacks and cleans up. +/// +/// This is the guts of the planner runtime execution. Because this routine is +/// run in an interrupt the state changes must be carefully ordered. +stat_t mp_exec_move() { + // Check if we can run a buffer + mp_buffer_t *bf = mp_queue_get_head(); + if (mp_get_state() == STATE_ESTOPPED || mp_get_state() == STATE_HOLDING || + !bf) { + mp_runtime_set_velocity(0); + mp_runtime_set_busy(false); + + return STAT_NOOP; // Nothing running + } + + // Process new buffers + if (bf->state == BUFFER_NEW) { + // On restart wait a bit to give planner queue a chance to fill + if (!mp_runtime_is_busy() && mp_queue_get_fill() < PLANNER_EXEC_MIN_FILL && + !rtc_expired(bf->ts + PLANNER_EXEC_DELAY)) return STAT_NOOP; + + // Take control of buffer + bf->state = BUFFER_INIT; + bf->replannable = false; + + // Update runtime + mp_runtime_set_line(bf->line); + } + + // Execute the buffer + stat_t status = bf->cb(bf); + + // Signal that we are busy only if a move was queued. This means that + // nonstop buffers, i.e. non-plan-to-zero commands, will not cause the + // runtime to enter the busy state. This causes mp_exec_move() to continue + // to wait above for the planner buffer to fill when a new stream starts + // with some nonstop buffers. If this weren't so, the code below + // which marks the next buffer not replannable would lock the first move + // buffer and cause it to be unnecessarily planned to zero. + if (status == STAT_EAGAIN || status == STAT_OK) mp_runtime_set_busy(true); + + // Process finished buffers + if (status != STAT_EAGAIN) { + // Signal that we've encountered a stopping point + if (fp_ZERO(mp_runtime_get_velocity()) && + (mp_get_state() == STATE_STOPPING || bf->hold)) mp_state_holding(); + + // Handle buffer restarts and deallocation + if (bf->state == BUFFER_RESTART) bf->state = BUFFER_NEW; + else { + // Solves a potential race condition where the current buffer ends but + // the new buffer has not started because the current one is still + // being run by the steppers. Planning can overwrite the new buffer. + // See notes above. + mp_buffer_next(bf)->replannable = false; + + mp_queue_pop(); // Release buffer + + // Enter READY state + if (mp_queue_is_empty()) mp_state_idle(); + } + } + + // Convert return status for stepper.c + switch (status) { + case STAT_NOOP: + // Tell caller to call again if there is more in the queue + return mp_queue_is_empty() ? STAT_NOOP : STAT_EAGAIN; + case STAT_EAGAIN: return STAT_OK; // A move was queued, call again later + default: return status; + } +} diff --git a/avr/src/plan/exec.h b/avr/src/plan/exec.h new file mode 100644 index 0000000..9020e96 --- /dev/null +++ b/avr/src/plan/exec.h @@ -0,0 +1,35 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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/forward_dif.c b/avr/src/plan/forward_dif.c new file mode 100644 index 0000000..955b15e --- /dev/null +++ b/avr/src/plan/forward_dif.c @@ -0,0 +1,190 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2012 - 2015 Rob Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "forward_dif.h" + +#include + + +/// Forward differencing math +/// +/// We are using a quintic (fifth-degree) Bezier polynomial for the velocity +/// curve. This gives us a "linear pop" velocity curve; with pop being the +/// sixth derivative of position: velocity - 1st, acceleration - 2nd, jerk - +/// 3rd, snap - 4th, crackle - 5th, pop - 6th +/// +/// The Bezier curve takes the form: +/// +/// V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + +/// P_4 * B_4(t) + P_5 * B_5(t) +/// +/// Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are +/// the control points, and B_0(t) through B_5(t) are the Bernstein +/// basis as follows: +/// +/// B_0(t) = (1 - t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1 +/// B_1(t) = 5(1 - t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t +/// B_2(t) = 10(1 - t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2 +/// B_3(t) = 10(1 - t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3 +/// B_4(t) = 5(1 - t) * t^4 = -5t^5 + 5t^4 +/// B_5(t) = t^5 = t^5 +/// +/// ^ ^ ^ ^ ^ ^ +/// A B C D E F +/// +/// We use forward-differencing to calculate each position through the curve. +/// This requires a formula of the form: +/// +/// V_f(t) = A * t^5 + B * t^4 + C * t^3 + D * t^2 + E * t + F +/// +/// Looking at the above B_0(t) through B_5(t) expanded forms, if we take the +/// coefficients of t^5 through t of the Bezier form of V(t), we can determine +/// that: +/// +/// A = -P_0 + 5 * P_1 - 10 * P_2 + 10 * P_3 - 5 * P_4 + P_5 +/// B = 5 * P_0 - 20 * P_1 + 30 * P_2 - 20 * P_3 + 5 * P_4 +/// C = -10 * P_0 + 30 * P_1 - 30 * P_2 + 10 * P_3 +/// D = 10 * P_0 - 20 * P_1 + 10 * P_2 +/// E = - 5 * P_0 + 5 * P_1 +/// F = P_0 +/// +/// Now, since we will (currently) *always* want the initial acceleration and +/// jerk values to be 0, We set P_i = P_0 = P_1 = P_2 (initial velocity), and +/// P_t = P_3 = P_4 = P_5 (target velocity), which, after simplification, +/// resolves to: +/// +/// A = - 6 * P_i + 6 * P_t +/// B = 15 * P_i - 15 * P_t +/// C = -10 * P_i + 10 * P_t +/// D = 0 +/// E = 0 +/// F = P_i +/// +/// Given an interval count of I to get from P_i to P_t, we get the parametric +/// "step" size of h = 1/I. We need to calculate the initial value of forward +/// differences (F_0 - F_5) such that the inital velocity V = P_i, then we +/// iterate over the following I times: +/// +/// V += F_5 +/// F_5 += F_4 +/// F_4 += F_3 +/// F_3 += F_2 +/// F_2 += F_1 +/// +/// See +/// http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417 +/// for an example of how to calculate F_0 - F_5 for a cubic bezier curve. Since +/// this is a quintic bezier curve, we need to extend the formulas somewhat. +/// I'll not go into the long-winded step-by-step here, but it gives the +/// resulting formulas: +/// +/// a = A, b = B, c = C, d = D, e = E, f = F +/// +/// F_5(t + h) - F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 + +/// (10ah^3 + 6bh^2 + 3ch)t^2 + (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 + +/// bh^4 + ch^3 + dh^2 + eh +/// +/// a = 5ah +/// b = 10ah^2 + 4bh +/// c = 10ah^3 + 6bh^2 + 3ch +/// d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh +/// +/// After substitution, simplification, and rearranging: +/// +/// F_4(t + h) - F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 + +/// (70ah^4 + 24bh^3 + 6ch^2)t + 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2 +/// +/// a = 20ah^2 +/// b = 60ah^3 + 12bh^2 +/// c = 70ah^4 + 24bh^3 + 6ch^2 +/// +/// After substitution, simplification, and rearranging: +/// +/// F_3(t + h) - F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 + +/// 36bh^4 + 6ch^3 +/// +/// You get the picture... +/// +/// F_2(t + h) - F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4 +/// F_1(t + h) - F_1(t) = 120ah^5 +/// +/// Normally, we could then assign t = 0, use the A-F values from above, and get +/// out initial F_* values. However, for the sake of "averaging" the velocity +/// of each segment, we actually want to have the initial V be be at t = h/2 and +/// iterate I-1 times. So, the resulting F_* values are (steps not shown): +/// +/// F_5 = 121Ah^5 / 16 + 5Bh^4 + 13Ch^3 / 4 + 2Dh^2 + Eh +/// F_4 = 165Ah^5 / 2 + 29Bh^4 + 9Ch^3 + 2Dh^2 +/// F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3 +/// F_2 = 300Ah^5 + 24Bh^4 +/// F_1 = 120Ah^5 +/// +/// Note that with our current control points, D and E are actually 0. +/// +/// This can be simplified even further by subsituting Ah, Bh & Ch back in and +/// reducing to: +/// +/// F_5 = (32.5 * s^2 - 75 * s + 45.375)(Vt - Vi) * h^5 +/// F_4 = (90.0 * s^2 - 435 * s + 495.0 )(Vt - Vi) * h^5 +/// F_3 = (60.0 * s^2 - 720 * s + 1530.0 )(Vt - Vi) * h^5 +/// F_2 = ( - 360 * s + 1800.0 )(Vt - Vi) * h^5 +/// F_1 = ( 720.0 )(Vt - Vi) * h^5 +/// +float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float s) { + const float h = 1 / s; + const float s2 = square(s); + const float Vdxh5 = (Vt - Vi) * h * h * h * h * h; + + fdif[4] = (32.5 * s2 - 75.0 * s + 45.375) * Vdxh5; + fdif[3] = (90.0 * s2 - 435.0 * s + 495.0 ) * Vdxh5; + fdif[2] = (60.0 * s2 - 720.0 * s + 1530.0 ) * Vdxh5; + fdif[1] = ( - 360.0 * s + 1800.0 ) * Vdxh5; + fdif[0] = ( 720.0 ) * Vdxh5; + + // Calculate the initial velocity by calculating: + // + // V(h / 2) = + // + // ( -6Vi + 6Vt) / (2^5 * s^8) + + // ( 15Vi - 15Vt) / (2^4 * s^8) + + // (-10Vi + 10Vt) / (2^3 * s^8) + Vi = + // + // (Vt - Vi) * 1/2 * h^8 + Vi + return (Vt - Vi) * 0.5 * square(square(square(h))) + Vi; +} + + +float mp_next_forward_dif(forward_dif_t fdif) { + float delta = fdif[4]; + + for (int i = 4; i; i--) + fdif[i] += fdif[i - 1]; + + return delta; +} diff --git a/avr/src/plan/forward_dif.h b/avr/src/plan/forward_dif.h new file mode 100644 index 0000000..d1ceb75 --- /dev/null +++ b/avr/src/plan/forward_dif.h @@ -0,0 +1,34 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + + +typedef float forward_dif_t[5]; + +float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float s); +float mp_next_forward_dif(forward_dif_t fdif); diff --git a/avr/src/plan/jog.c b/avr/src/plan/jog.c new file mode 100644 index 0000000..18c5278 --- /dev/null +++ b/avr/src/plan/jog.c @@ -0,0 +1,186 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "forward_dif.h" +#include "runtime.h" +#include "machine.h" +#include "state.h" +#include "config.h" + +#include +#include +#include +#include +#include + + +typedef struct { + bool writing; + float Vi; + float Vt; + forward_dif_t fdifs[AXES]; + unsigned segments[AXES]; + + int sign[AXES]; + float velocity[AXES]; + float next_velocity[AXES]; + float target_velocity[AXES]; +} jog_runtime_t; + + +static jog_runtime_t jr; + + +static stat_t _exec_jog(mp_buffer_t *bf) { + // Load next velocity + bool changed = false; + bool done = true; + if (!jr.writing) + for (int axis = 0; axis < AXES; axis++) { + float Vn = jr.next_velocity[axis] * axis_get_velocity_max(axis); + float Vi = jr.velocity[axis]; + float Vt = jr.target_velocity[axis]; + + if (JOG_MIN_VELOCITY < fabs(Vn)) done = false; + + if (!fp_ZERO(Vi) && (Vn < 0) != (Vi < 0)) + Vn = 0; // Plan to zero on sign change + + if (fabs(Vn) < JOG_MIN_VELOCITY) Vn = 0; + + if (Vt != Vn) { + jr.target_velocity[axis] = Vn; + if (Vn) jr.sign[axis] = Vn < 0 ? -1 : 1; + changed = true; + } + } + + float velocity_sqr = 0; + + for (int axis = 0; axis < AXES; axis++) { + float Vi = fabs(jr.velocity[axis]); + float Vt = fabs(jr.target_velocity[axis]); + + if (changed) { + if (fp_EQ(Vi, Vt)) { + Vi = Vt; + jr.segments[axis] = 0; + + } else { + // Compute axis max jerk + float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER; + + // Compute length to velocity given max jerk + float length = mp_get_target_length(Vi, Vt, jerk * JOG_JERK_MULT); + + // Compute move time + float move_time = 2 * length / (Vi + Vt); + if (move_time < MIN_SEGMENT_TIME) { + Vi = Vt; + jr.segments[axis] = 0; + + } else { + jr.segments[axis] = ceil(move_time / NOM_SEGMENT_TIME); + + Vi = mp_init_forward_dif(jr.fdifs[axis], Vi, Vt, jr.segments[axis]); + jr.segments[axis]--; + } + } + + } else if (jr.segments[axis]) { + Vi += mp_next_forward_dif(jr.fdifs[axis]); + jr.segments[axis]--; + + } else Vi = Vt; + + if (JOG_MIN_VELOCITY < Vi || JOG_MIN_VELOCITY < Vt) done = false; + + velocity_sqr += square(Vi); + jr.velocity[axis] = Vi * jr.sign[axis]; + } + + // Check if we are done + if (done) { + // Update machine position + for (int axis = 0; axis < AXES; axis++) + mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); + + mp_set_cycle(CYCLE_MACHINING); // Default cycle + + return STAT_NOOP; // Done, no move executed + } + + // Compute target from velocity + float target[AXES]; + for (int axis = 0; axis < AXES; axis++) + target[axis] = mp_runtime_get_axis_position(axis) + + jr.velocity[axis] * NOM_SEGMENT_TIME; + + // Set velocity and target + mp_runtime_set_velocity(sqrt(velocity_sqr)); + stat_t status = mp_runtime_move_to_target(target, NOM_SEGMENT_TIME); + if (status != STAT_OK) return status; + + return STAT_EAGAIN; +} + + +bool mp_jog_busy() {return mp_get_cycle() == CYCLE_JOGGING;} + + +uint8_t command_jog(int argc, char *argv[]) { + if (!mp_jog_busy() && + (mp_get_state() != STATE_READY || mp_get_cycle() != CYCLE_MACHINING)) + return STAT_NOOP; + + float velocity[AXES]; + + for (int axis = 0; axis < AXES; axis++) + if (axis < argc - 1) velocity[axis] = atof(argv[axis + 1]); + else velocity[axis] = 0; + + // Reset + if (!mp_jog_busy()) memset(&jr, 0, sizeof(jr)); + + jr.writing = true; + for (int axis = 0; axis < AXES; axis++) + jr.next_velocity[axis] = velocity[axis]; + jr.writing = false; + + if (!mp_jog_busy()) { + mp_set_cycle(CYCLE_JOGGING); + mp_queue_push_nonstop(_exec_jog, -1); + } + + return STAT_OK; +} diff --git a/avr/src/plan/jog.h b/avr/src/plan/jog.h new file mode 100644 index 0000000..32554d8 --- /dev/null +++ b/avr/src/plan/jog.h @@ -0,0 +1,33 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + + +bool mp_jog_busy(); diff --git a/avr/src/plan/line.c b/avr/src/plan/line.c new file mode 100644 index 0000000..6e0c0e3 --- /dev/null +++ b/avr/src/plan/line.c @@ -0,0 +1,409 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2012 - 2015 Rob Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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" + + +/* Sonny's algorithm - simple + * + * Computes the maximum allowable junction speed by finding the velocity that + * will yield the centripetal acceleration in the corner_acceleration value. The + * value of delta sets the effective radius of curvature. Here's Sonny's + * (Sungeun K. Jeon's) explanation of what's going on: + * + * "First let's assume that at a junction we only look a centripetal + * acceleration to simplify things. At a junction of two lines, let's place a + * circle such that both lines are tangent to the circle. The circular segment + * joining the lines represents the path for constant centripetal acceleration. + * This creates a deviation from the path (let's call this delta), + * which is the distance from the junction to the edge of the circular + * segment. Delta needs to be defined, so let's replace the term max_jerk (see + * note 1) with max_junction_deviation, or "delta". This indirectly sets the + * radius of the circle, and hence limits the velocity by the centripetal + * acceleration. Think of the this as widening the race track. If a race car is + * driving on a track only as wide as a car, it'll have to slow down a lot to + * turn corners. If we widen the track a bit, the car can start to use the + * track to go into the turn. The wider it is, the faster through the corner + * it can go. + * + * Note 1: "max_jerk" refers to the old grbl/marlin "max_jerk" approximation + * term, not the TinyG jerk terms. + * + * If you do the geometry in terms of the known variables, you get: + * + * sin(theta/2) = R / (R + delta) + * + * Re-arranging in terms of circle radius (R) + * + * R = delta * sin(theta/2) / (1 - sin(theta/2)) + * + * Theta is the angle between line segments given by: + * + * cos(theta) = dot(a, b) / (norm(a) * norm(b)) + * + * Most of these calculations are already done in the planner. + * To remove the acos() and sin() computations, use the trig half + * angle identity: + * + * sin(theta/2) = +/-sqrt((1 - cos(theta)) / 2) + * + * For our applications, this should always be positive. Now just plug the + * equations into the centripetal acceleration equation: + * + * v_c = sqrt(a_max * R) + * + * You'll see that there are only two sqrt computations and no sine/cosines. + * + * How to compute the radius using brute-force trig: + * + * float theta = acos(costheta); + * float radius = delta * sin(theta/2) / (1 - sin(theta/2)); + * + * This version extends Chamnit's algorithm by computing a value for delta that + * takes the contributions of the individual axes in the move into account. + * This allows the control radius to vary by axis. This is necessary to + * support axes that have different dynamics; such as a Z axis that doesn't + * move as fast as X and Y (such as a screw driven Z axis on machine with a belt + * driven XY - like a Shapeoko), or rotary axes ABC that have completely + * different dynamics than their linear counterparts. + * + * The function takes the absolute values of the sum of the unit vector + * components as a measure of contribution to the move, then scales the delta + * values from the non-zero axes into a composite delta to be used for the + * move. Shown for an XY vector: + * + * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i]) + * Usum Length of sums Ux + Uy + * d Delta of sums (Dx * Ux + DY * UY) / Usum + */ +static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { + float costheta = 0; + for (int axis = 0; axis < AXES; axis++) + costheta -= a_unit[axis] * b_unit[axis]; + + if (costheta < -0.99) return 10000000; // straight line cases + if (0.99 < costheta) return 0; // reversal cases + + // Fuse the junction deviations into a vector sum + float a_delta = 0; + float b_delta = 0; + + for (int axis = 0; axis < AXES; axis++) { + a_delta += square(a_unit[axis] * axis_get_junction_dev(axis)); + b_delta += square(b_unit[axis] * axis_get_junction_dev(axis)); + } + + if (!a_delta || !b_delta) return 0; // One or both unit vectors are null + + float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2; + float sintheta_over2 = sqrt((1 - costheta) / 2); + float radius = delta * sintheta_over2 / (1 - sintheta_over2); + float velocity = sqrt(radius * JUNCTION_ACCELERATION); + + return velocity; +} + + +/* Find the axis for which the jerk cannot be exceeded - the 'jerk_axis'. + * This is the axis for which the time to decelerate from the target velocity + * to zero would be the longest. + * + * We can determine the "longest" deceleration in terms of time or distance. + * + * The math for time-to-decelerate T from speed S to speed E with constant + * jerk J is: + * + * T = 2 * sqrt((S - E) / J) + * + * Since E is always zero in this calculation, we can simplify: + * + * T = 2 * sqrt(S / J) + * + * The math for distance-to-decelerate l from speed S to speed E with + * constant jerk J is: + * + * l = (S + E) * sqrt((S - E) / J) + * + * Since E is always zero in this calculation, we can simplify: + * + * l = S * sqrt(S / J) + * + * The final value we want to know is which one is *longest*, compared to the + * others, so we don't need the actual value. This means that the scale of + * the value doesn't matter, so for T we can remove the "2 *" and for L we can + * remove the "S *". This leaves both to be simply "sqrt(S / J)". Since we + * don't need the scale, it doesn't matter what speed we're coming from, so S + * can be replaced with 1. + * + * However, we *do* need to compensate for the fact that each axis contributes + * differently to the move, so we will scale each contribution C[n] by the + * proportion of the axis movement length D[n]. Using that, we construct the + * following, with these definitions: + * + * J[n] = max_jerk for axis n + * D[n] = distance traveled for this move for axis n + * C[n] = "axis contribution" of axis n + * + * For each axis n: + * + * C[n] = sqrt(1 / J[n]) * D[n] + * + * Keeping in mind that we only need a rank compared to the other axes, we can + * further optimize the calculations: + * + * Square the expression to remove the square root: + * + * C[n]^2 = 1 / J[n] * D[n]^2 + * + * We don't care that C is squared, so we'll use it that way. Also note that + * we already have 1 / J[n] calculated for each axis. + */ +int mp_find_jerk_axis(const float axis_square[]) { + float C; + float maxC = 0; + int jerk_axis = 0; + + for (int axis = 0; axis < AXES; axis++) + if (axis_square[axis]) { // Do not use fp_ZERO here + // Squaring axis_length ensures it's positive + C = axis_square[axis] * axis_get_recip_jerk(axis); + + if (maxC < C) { + maxC = C; + jerk_axis = axis; + } + } + + return jerk_axis; +} + + +/// Determine jerk value to use for the block. +static float _calc_jerk(const float axis_square[], const float unit[]) { + int axis = mp_find_jerk_axis(axis_square); + + // Finally, the selected jerk term needs to be scaled by the + // reciprocal of the absolute value of the axis's unit + // vector term. This way when the move is finally decomposed into + // its constituent axes for execution the jerk for that axis will be + // at it's maximum value. + return axis_get_jerk_max(axis) * JERK_MULTIPLIER / fabs(unit[axis]); +} + + +/// Compute cached jerk terms used by planning +static void _calc_and_cache_jerk_values(mp_buffer_t *bf) { + static float jerk = 0; + static float cbrt_jerk = 0; + + if (JERK_MATCH_PRECISION < fabs(bf->jerk - jerk)) { // Tolerance comparison + jerk = bf->jerk; + cbrt_jerk = cbrt(bf->jerk); + } + + bf->cbrt_jerk = cbrt_jerk; +} + + +static void _calc_max_velocities(mp_buffer_t *bf, float move_time, + bool exact_stop) { + float junction_velocity = + _get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit); + + bf->cruise_vmax = bf->length / move_time; // target velocity requested + bf->entry_vmax = min(bf->cruise_vmax, junction_velocity); + bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); + bf->exit_vmax = min(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax)); + bf->braking_velocity = bf->delta_vmax; + + // Zero out exact stop cases + if (exact_stop) bf->entry_vmax = bf->exit_vmax = 0; + else bf->replannable = true; +} + + +/* Compute optimal and minimum move times + * + * "Minimum time" is the fastest the move can be performed given the velocity + * constraints on each participating axis - regardless of the feed rate + * requested. The minimum time is the time limited by the rate-limiting + * axis. The minimum time is needed to compute the optimal time and is recorded + * for possible feed override computation. + * + * "Optimal time" is either the time resulting from the requested feed rate or + * the minimum time if the requested feed rate is not achievable. Optimal times + * for rapids are always the minimum time. + * + * The following times are compared and the longest is returned: + * - G93 inverse time (if G93 is active) + * - time for coordinated move at requested feed rate + * - time that the slowest axis would require for the move + * + * NIST RS274NGC_v3 Guidance + * + * The following is verbatim text from NIST RS274NGC_v3. As I interpret A for + * moves that combine both linear and rotational movement, the feed rate should + * apply to the XYZ movement, with the rotational axis (or axes) timed to start + * and end at the same time the linear move is performed. It is possible under + * this case for the rotational move to rate-limit the linear move. + * + * 2.1.2.5 Feed Rate + * + * The rate at which the controlled point or the axes move is nominally a steady + * rate which may be set by the user. In the Interpreter, the interpretation of + * the feed rate is as follows unless inverse time feed rate mode is being used + * in the RS274/NGC view (see Section 3.5.19). The machining functions view of + * feed rate, as described in Section 4.3.5.1, has conditions under which the + * set feed rate is applied differently, but none of these is used in the + * Interpreter. + * + * A. For motion involving one or more of the X, Y, and Z axes (with or without + * simultaneous rotational axis motion), the feed rate means length units + * per minute along the programmed XYZ path, as if the rotational axes were + * not moving. + * + * B. For motion of one rotational axis with X, Y, and Z axes not moving, the + * feed rate means degrees per minute rotation of the rotational axis. + * + * C. For motion of two or three rotational axes with X, Y, and Z axes not + * moving, the rate is applied as follows. Let dA, dB, and dC be the angles + * in degrees through which the A, B, and C axes, respectively, must move. + * Let D = sqrt(dA^2 + dB^2 + dC^2). Conceptually, D is a measure of total + * angular motion, using the usual Euclidean metric. Let T be the amount of + * time required to move through D degrees at the current feed rate in + * degrees per minute. The rotational axes should be moved in coordinated + * linear motion so that the elapsed time from the start to the end of the + * motion is T plus any time required for acceleration or deceleration. + */ +static float _calc_move_time(const float axis_length[], + const float axis_square[], bool rapid, + bool inverse_time, float feed_rate, + float feed_override) { + float max_time = 0; + + // Compute times for feed motion + if (!rapid) { + if (inverse_time) max_time = feed_rate; + else { + // Compute length of linear move in millimeters. Feed rate in mm/min. + max_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + + axis_square[AXIS_Z]) / feed_rate; + + // If no linear axes, compute length of multi-axis rotary move in degrees. + // Feed rate is provided as degrees/min + if (fp_ZERO(max_time)) + max_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + + axis_square[AXIS_C]) / feed_rate; + } + } + + // Apply feed override + max_time /= feed_override; + + // Compute time required for rate-limiting axis + for (int axis = 0; axis < AXES; axis++) { + float time = fabs(axis_length[axis]) / + (rapid ? axis_get_velocity_max(axis) : axis_get_feedrate_max(axis)); + + if (max_time < time) max_time = time; + } + + return max_time < MIN_SEGMENT_TIME ? MIN_SEGMENT_TIME : max_time; +} + + +/*** Plan line acceleration / deceleration + * + * This function uses constant jerk motion equations to plan acceleration and + * deceleration. Jerk is the rate of change of acceleration; it's the 1st + * derivative of acceleration, and the 3rd derivative of position. Jerk is a + * measure of impact to the machine. Controlling jerk smooths transitions + * between moves and allows for faster feeds while controlling machine + * oscillations and other undesirable side-effects. + * + * Notes: + * [1] All math is done in absolute coordinates using single precision floats. + * + * [2] Returning a status that is not STAT_OK means the endpoint is NOT + * advanced. So lines that are too short to move will accumulate and get + * executed once the accumulated error exceeds the minimums. + * + * @param reed_rate is in minutes when @param inverse_time is true. + * See mach_set_feed_rate() + */ +stat_t mp_aline(const float target[], bool rapid, bool inverse_time, + bool exact_stop, float feed_rate, float feed_override, + int32_t line) { + // Compute axis and move lengths + float axis_length[AXES]; + float axis_square[AXES]; + float length_square = 0; + + for (int axis = 0; axis < AXES; axis++) { + axis_length[axis] = target[axis] - mp_get_axis_position(axis); + axis_square[axis] = square(axis_length[axis]); + length_square += axis_square[axis]; + } + + float length = sqrt(length_square); + if (fp_ZERO(length)) return STAT_OK; + + // Get a buffer. Note, new buffers are initialized to zero. + mp_buffer_t *bf = mp_queue_get_tail(); // current move pointer + + // Set buffer values + bf->length = length; + copy_vector(bf->target, target); + + // Compute unit vector + for (int axis = 0; axis < AXES; axis++) + bf->unit[axis] = axis_length[axis] / length; + + // Compute and cache jerk values + bf->jerk = _calc_jerk(axis_square, bf->unit); + _calc_and_cache_jerk_values(bf); + + // Compute move time and velocities + float time = _calc_move_time(axis_length, axis_square, rapid, inverse_time, + feed_rate, feed_override); + _calc_max_velocities(bf, time, exact_stop); + + // Note, the following lines must remain in order. + bf->line = line; // Planner needs then when planning steps + mp_plan(bf); // Plan block list + mp_set_position(target); // Set planner position before committing buffer + mp_queue_push(mp_exec_aline, line); // After position update + + return STAT_OK; +} diff --git a/avr/src/plan/line.h b/avr/src/plan/line.h new file mode 100644 index 0000000..9b109e7 --- /dev/null +++ b/avr/src/plan/line.h @@ -0,0 +1,38 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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_aline(const float target[], bool rapid, bool inverse_time, + bool exact_stop, float feed_rate, float feed_override, + int32_t line); +int mp_find_jerk_axis(const float axis_square[]); diff --git a/avr/src/plan/planner.c b/avr/src/plan/planner.c new file mode 100644 index 0000000..56966a6 --- /dev/null +++ b/avr/src/plan/planner.c @@ -0,0 +1,752 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2012 - 2015 Rob Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 \ + (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->entry_velocity)) +#define MIN_TAIL_LENGTH \ + (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->exit_velocity)) +#define MIN_BODY_LENGTH (MIN_SEGMENT_TIME_PLUS_MARGIN * bf->cruise_velocity) + + +/*** Calculate move acceleration / deceleration + * + * This rather brute-force and long-ish function sets section lengths and + * velocities based on the line length and velocities requested. It modifies + * the incoming bf buffer and returns accurate head, body and tail lengths, and + * accurate or reasonably approximate velocities. We care about accuracy on + * lengths, less so for velocity, as long as velocity errs on the side of too + * slow. + * + * Note: We need the velocities to be set even for zero-length sections (Note: + * sections, not moves) so we can compute entry and exits for adjacent sections. + * + * Inputs used are: + * + * bf->length - actual block length, length is never changed + * bf->entry_velocity - requested Ve, entry velocity is never changed + * bf->cruise_velocity - requested Vt, is often changed + * bf->exit_velocity - requested Vx, may change for degenerate cases + * bf->cruise_vmax - used in some comparisons + * bf->delta_vmax - used to degrade velocity of short blocks + * + * Variables that may be set/updated are: + * + * bf->entry_velocity - requested Ve + * bf->cruise_velocity - requested Vt + * bf->exit_velocity - requested Vx + * bf->head_length - bf->length allocated to head + * bf->body_length - bf->length allocated to body + * bf->tail_length - bf->length allocated to tail + * + * Note: The following conditions must be met on entry: + * + * bf->length must be non-zero (filter these out upstream) + * bf->entry_velocity <= bf->cruise_velocity >= bf->exit_velocity + * + * Classes of moves: + * + * Requested-Fit - The move has sufficient length to achieve the target + * velocity (cruise velocity). I.e it will accommodate the acceleration / + * deceleration profile in the given length. + * + * Rate-Limited-Fit - The move does not have sufficient length to achieve + * target velocity. In this case the cruise velocity will be set lower than + * the requested velocity (incoming bf->cruise_velocity). The entry and + * exit velocities are satisfied. + * + * Degraded-Fit - The move does not have sufficient length to transition from + * the entry velocity to the exit velocity in the available length. These + * velocities are not negotiable, so a degraded solution is found. + * + * In worst cases, the move cannot be executed as the required execution + * time is less than the minimum segment time. The first degradation is to + * reduce the move to a body-only segment with an average velocity. If that + * still doesn't fit then the move velocity is reduced so it fits into a + * minimum segment. This will reduce the velocities in that region of the + * planner buffer as the moves are replanned to that worst-case move. + * + * Various cases handled (H=head, B=body, T=tail) + * + * Requested-Fit cases: + * + * HBT 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) { + // RULE #1 of mp_calculate_trapezoid(): Don't change bf->length + + if (!bf->length) return; + + // F case: Block is too short - run time < minimum segment time + // Force block into a single segment body with limited velocities + // Accept the entry velocity, limit the cruise, and go for the best exit + // velocity you can get given the delta_vmax (maximum velocity slew). + + float naive_move_time = + 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average + + if (naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) { + bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN; + bf->exit_velocity = + max(0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax))); + bf->body_length = bf->length; + bf->head_length = 0; + bf->tail_length = 0; + + // Violating jerk but since it's a single segment move we don't use it. + return; + } + + // B" case: Block is short, but fits into a single body segment + if (naive_move_time <= NOM_SEGMENT_TIME) { + bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity; + + if (fp_NOT_ZERO(bf->entry_velocity)) { + bf->cruise_velocity = bf->entry_velocity; + bf->exit_velocity = bf->entry_velocity; + + } else { + bf->cruise_velocity = bf->delta_vmax / 2; + bf->exit_velocity = bf->delta_vmax; + } + + bf->body_length = bf->length; + bf->head_length = 0; + bf->tail_length = 0; + + // Violating jerk but since it's a single segment move we don't use it. + return; + } + + // B case: Velocities all match (or close enough) + // This occurs frequently in normal gcode files with lots of short lines. + // This case is not really necessary, but saves lots of processing time. + if (((bf->cruise_velocity - bf->entry_velocity) < + TRAPEZOID_VELOCITY_TOLERANCE) && + ((bf->cruise_velocity - bf->exit_velocity) < + TRAPEZOID_VELOCITY_TOLERANCE)) { + bf->body_length = bf->length; + bf->head_length = 0; + bf->tail_length = 0; + + return; + } + + // Head-only and tail-only short-line cases + // H" and T" degraded-fit cases + // H' and T' requested-fit cases where the body residual is less than + // MIN_BODY_LENGTH + bf->body_length = 0; + float minimum_length = + mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf->jerk); + + // head-only & tail-only cases + if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) { + // tail-only cases (short decelerations) + if (bf->entry_velocity > bf->exit_velocity) { + // T" (degraded case) + if (bf->length < minimum_length) + bf->entry_velocity = + mp_get_target_velocity(bf->exit_velocity, bf->length, bf); + + bf->cruise_velocity = bf->entry_velocity; + bf->tail_length = bf->length; + bf->head_length = 0; + + return; + } + + // head-only cases (short accelerations) + if (bf->entry_velocity < bf->exit_velocity) { + // H" (degraded case) + if (bf->length < minimum_length) + bf->exit_velocity = + mp_get_target_velocity(bf->entry_velocity, bf->length, bf); + + bf->cruise_velocity = bf->exit_velocity; + bf->head_length = bf->length; + bf->tail_length = 0; + + return; + } + } + + // Set head and tail lengths for evaluating the next cases + bf->head_length = + mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk); + bf->tail_length = + mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk); + if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;} + if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;} + + // Rate-limited HT and HT' cases + if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited + + // Symmetric rate-limited case (HT) + if (fabs(bf->entry_velocity - bf->exit_velocity) < + TRAPEZOID_VELOCITY_TOLERANCE) { + bf->head_length = bf->length/2; + bf->tail_length = bf->head_length; + bf->cruise_velocity = + min(bf->cruise_vmax, + mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf)); + + if (bf->head_length < MIN_HEAD_LENGTH) { + // Convert this to a body-only move + bf->body_length = bf->length; + bf->head_length = 0; + bf->tail_length = 0; + + // Average the entry speed and computed best cruise-speed + bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity)/2; + bf->entry_velocity = bf->cruise_velocity; + bf->exit_velocity = bf->cruise_velocity; + } + + return; + } + + // Asymmetric HT' rate-limited case. This is relatively expensive but it's + // not called very often + float computed_velocity = bf->cruise_vmax; + do { + // initialize from previous iteration + bf->cruise_velocity = computed_velocity; + bf->head_length = + mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk); + bf->tail_length = + mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk); + + if (bf->head_length > bf->tail_length) { + bf->head_length = + (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length; + computed_velocity = + mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf); + + } else { + bf->tail_length = + (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length; + computed_velocity = + mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf); + } + + } while ((fabs(bf->cruise_velocity - computed_velocity) / + computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT); + + // set velocity and clean up any parts that are too short + bf->cruise_velocity = computed_velocity; + bf->head_length = + mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk); + bf->tail_length = bf->length - bf->head_length; + + if (bf->head_length < MIN_HEAD_LENGTH) { + // adjust the move to be all tail... + bf->tail_length = bf->length; + bf->head_length = 0; + } + + if (bf->tail_length < MIN_TAIL_LENGTH) { + bf->head_length = bf->length; //...or all head + bf->tail_length = 0; + } + + return; + } + + // Requested-fit cases: remaining of: HBT, HB, BT, BT, H, T, B, cases + bf->body_length = bf->length - bf->head_length - bf->tail_length; + + // If a non-zero body is < minimum length distribute it to the head and/or + // tail. This will generate small (acceptable) velocity errors in runtime + // execution but preserve correct distance, which is more important. + if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) { + if (fp_NOT_ZERO(bf->head_length)) { + if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT + bf->head_length += bf->body_length / 2; + bf->tail_length += bf->body_length / 2; + + } else bf->head_length += bf->body_length; // HB reduces to H + } else bf->tail_length += bf->body_length; // BT reduces to T + + bf->body_length = 0; + + // If the body is a standalone make the cruise velocity match the entry + // velocity. This removes a potential velocity discontinuity at the expense + // of top speed + } else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length))) + bf->cruise_velocity = bf->entry_velocity; +} + + +#if 0 +/// Prints the entire planning queue as comma separated values embedded in +/// JSON ``msg`` entries. Used for debugging. +void mp_print_queue(mp_buffer_t *bf) { + printf_P(PSTR("{\"msg\":\"id,replannable,callback," + "length,head_length,body_length,tail_length," + "entry_velocity,cruise_velocity,exit_velocity,braking_velocity," + "entry_vmax,cruise_vmax,exit_vmax\"}\n")); + + int i = 0; + mp_buffer_t *bp = bf; + while (bp) { + printf_P(PSTR("{\"msg\":\"%d,%d,0x%04x," + "%0.2f,%0.2f,%0.2f,%0.2f," + "%0.2f,%0.2f,%0.2f,%0.2f," + "%0.2f,%0.2f,%0.2f\"}\n"), + i++, bp->replannable, bp->cb, + bp->length, bp->head_length, bp->body_length, bp->tail_length, + bp->entry_velocity, bp->cruise_velocity, bp->exit_velocity, + bp->braking_velocity, + bp->entry_vmax, bp->cruise_vmax, bp->exit_vmax); + + bp = mp_buffer_prev(bp); + if (bp == bf || bp->state == BUFFER_OFF) break; + } + + while (!usart_tx_empty()) continue; +} +#endif + + +/*** Plans the entire queue + * + * The block list is the circular buffer of planner buffers (bl's). The block + * currently being planned is the "bl" block. The "first block" is the next + * block to execute; queued immediately behind the currently executing block, + * aka the "running" block. In some cases, there is no first block because the + * list is empty or there is only one block and it is already running. + * + * If blocks following the first block are already optimally planned (non + * replannable) the first block that is not optimally planned becomes the + * effective first block. + * + * mp_plan() plans all blocks between and including the (effective) + * first block and the bl. It sets entry, exit and cruise v's from vmax's then + * calls trapezoid generation. + * + * Variables that must be provided in the mp_buffer_t that will be processed: + * + * bl (function arg) - end of block list (last block in time) + * bl->replannable - start of block list set by last FALSE value + * [Note 1] + * bl->move_type - typically MOVE_TYPE_ALINE. Other move_types should + * be set to length=0, entry_vmax=0 and exit_vmax=0 + * and are treated as a momentary stop (plan to zero + * and from zero). + * bl->length - provides block length + * bl->entry_vmax - used during forward planning to set entry velocity + * bl->cruise_vmax - used during forward planning to set cruise velocity + * bl->exit_vmax - used during forward planning to set exit velocity + * bl->delta_vmax - used during forward planning to set exit velocity + * bl->cbrt_jerk - used during trapezoid generation + * + * Variables that will be set during processing: + * + * bl->replannable - set if the block becomes optimally planned + * bl->braking_velocity - set during backward planning + * bl->entry_velocity - set during forward planning + * bl->cruise_velocity - set during forward planning + * bl->exit_velocity - set during forward planning + * bl->head_length - set during trapezoid generation + * bl->body_length - set during trapezoid generation + * bl->tail_length - set during trapezoid generation + * + * Variables that are ignored but here's what you would expect them to be: + * + * bl->state - BUFFER_NEW for all blocks but the earliest + * bl->target[] - block target position + * bl->unit[] - block unit vector + * bl->jerk - source of the other jerk variables. + * + * Notes: + * + * [1] Whether or not a block is planned is controlled by the bl->replannable + * setting. Replan flags are checked during the backwards pass. They prune + * the replan list to include only the latest blocks that require planning. + * + * In normal operation, the first block (currently running block) is not + * replanned, but may be for feedholds and feed overrides. In these cases, + * the prep routines modify the contents of the (ex) buffer and re-shuffle + * the block list, re-enlisting the current bl buffer with new parameters. + * These routines also set all blocks in the list to be replannable so the + * list can be recomputed regardless of exact stops and previous replanning + * optimizations. + */ +void mp_plan(mp_buffer_t *bl) { + mp_buffer_t *bp = bl; + + // Backward planning pass. Find first block and update braking velocities. + // By the end bp points to the buffer before the first block. + mp_buffer_t *next = bp; + while ((bp = mp_buffer_prev(bp)) != bl) { + if (!bp->replannable) break; + + bp->braking_velocity = + min(next->entry_vmax, next->braking_velocity) + bp->delta_vmax; + + next = bp; + } + + // Forward planning pass. Recompute trapezoids from the first block to bl. + mp_buffer_t *prev = bp; + while ((bp = mp_buffer_next(bp)) != bl) { + mp_buffer_t *next = mp_buffer_next(bp); + + bp->entry_velocity = prev == bl ? bp->entry_vmax : prev->exit_velocity; + bp->cruise_velocity = bp->cruise_vmax; + bp->exit_velocity = min4(bp->exit_vmax, next->entry_vmax, + next->braking_velocity, + bp->entry_velocity + bp->delta_vmax); + + if (mp.plan_steps && bp->line != next->line) { + bp->exit_velocity = 0; + bp->hold = true; + + } else bp->hold = false; + + mp_calculate_trapezoid(bp); + + // Test for optimally planned trapezoids by checking exit conditions + if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) || + fp_EQ(bp->exit_velocity, next->entry_vmax)) || + (!prev->replannable && + fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax)))) + bp->replannable = false; + + prev = bp; + } + + // Finish last block + bl->entry_velocity = prev->exit_velocity; + bl->cruise_velocity = bl->cruise_vmax; + bl->exit_velocity = 0; + + mp_calculate_trapezoid(bl); +} + + +void mp_replan_all() { + ASSERT(mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING); + + // Get next buffer + mp_buffer_t *bf = mp_queue_get_head(); + if (!bf) return; + + mp_buffer_t *bp = bf; + + // Mark all blocks replanable + while (true) { + bp->replannable = true; + mp_buffer_t *next = mp_buffer_next(bp); + if (next->state == BUFFER_OFF || next == bf) break; // Avoid wrap around + bp = next; + } + + // Plan blocks + mp_plan(bp); +} + + +/// Push a non-stop command to the queue. I.e. one that does not cause the +/// planner to plan to zero. +void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line) { + mp_buffer_t *bp = mp_queue_get_tail(); + + bp->entry_vmax = bp->cruise_vmax = bp->exit_vmax = INFINITY; + copy_vector(bp->unit, bp->prev->unit); + bp->replannable = true; + + mp_queue_push(cb, line); +} + + +/*** Derive accel/decel length from delta V and jerk + * + * A convenient function for determining the optimal_length (L) of a line + * given the initial velocity (Vi), final velocity (Vf) and maximum jerk (Jm). + * + * Definitions: + * + * Jm = the given maximum jerk + * T = time of the entire move + * Vi = initial velocity + * Vf = final velocity + * T = time + * As = accel at inflection point between convex & concave portions of S-curve + * Ar = ramp acceleration + * + * Formulas: + * + * T = 2 * sqrt((Vt - Vi) / Jm) + * As = (Jm * T) / 2 + * Ar = As / 2 = (Jm * T) / 4 + * + * Then the length (distance) equation is: + * + * a) L = (Vf - Vi) * T - (Ar * T^2) / 2 + * + * Substituting for Ar and T: + * + * b) L = (Vf - Vi) * 2 * sqrt((Vf - Vi) / Jm) - + * (2 * sqrt((Vf - Vi) / Jm) * (Vf - Vi)) / 2 + * + * Reducing b). See Wolfram Alpha: + * + * c) L = (Vf - Vi)^(3/2) / sqrt(Jm) + * + * Assuming Vf >= Vi [Note 2]: + * + * d) L = (Vf - Vi) * sqrt((Vf - Vi) / Jm) + * + * Notes: + * + * [1] Assuming Vi, Vf and L are positive or zero. + * [2] Cannot assume Vf >= Vi due to rounding errors and use of + * PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs() + */ +float mp_get_target_length(float Vi, float Vf, float jerk) { + return fp_EQ(Vi, Vf) ? 0 : fabs(Vi - Vf) * invsqrt(jerk / fabs(Vi - Vf)); +} + + +#define GET_VELOCITY_ITERATIONS 0 // must be zero or more + +/*** Derive velocity achievable from delta V and length + * + * A convenient function for determining Vf target velocity for a given + * initial velocity (Vi), length (L), and maximum jerk (Jm). Equation e) is + * b) solved for Vf. Equation f) is c) solved for Vf. Use f) (obviously) + * + * e) Vf = (sqrt(L) * (L / sqrt(1 / Jm))^(1/6) + + * (1 / Jm)^(1/4) * Vi) / (1 / Jm)^(1/4) + * + * f) Vf = L^(2/3) * Jm^(1/3) + Vi + * + * FYI: Here's an expression that returns the jerk for a given deltaV and L: + * + * cube(deltaV / (pow(L, 0.66666666))); + * + * We do some Newton-Raphson iterations to narrow it down. + * We need a formula that includes known variables except the one we want to + * find, and has a root [Z(x) = 0] at the value (x) we are looking for. + * + * Z(x) = zero at x + * + * We calculate the value from the knowns and the estimate (see below) and then + * subtract the known value to get zero (root) if x is the correct value. + * + * Vi = initial velocity (known) + * Vf = estimated final velocity + * J = jerk (known) + * L = length (know) + * + * There are (at least) two such functions we can use: + * + * L from J, Vi, and Vf: + * + * L = sqrt((Vf - Vi) / J) * (Vi + Vf) + * + * Replacing Vf with x, and subtracting the known L we get: + * + * 0 = sqrt((x - Vi) / J) * (Vi + x) - L + * Z(x) = sqrt((x - Vi) / J) * (Vi + x) - L + * + * Or J from L, Vi, and Vf: + * + * J = ((Vf - Vi) * (Vi + Vf)^2) / L^2 + * + * Replacing Vf with x, and subtracting the known J we get: + * + * 0 = ((x - Vi) * (Vi + x)^2) / L^2 - J + * Z(x) = ((x - Vi) * (Vi + x)^2) / L^2 - J + * + * L doesn't resolve to the value very quickly (its graph is nearly vertical). + * So, we'll use J, which resolves in < 10 iterations, often in only two or + * three with a good estimate. + * + * In order to do a Newton-Raphson iteration, we need the derivative. Here + * they are for both the (unused) L and the (used) J formulas above: + * + * J > 0, Vi > 0, Vf > 0 + * A = sqrt((x - Vi) * J) + * B = sqrt((x - Vi) / J) + * + * L'(x) = B + (Vi + x) / (2 * J) + (Vi + x) / (2 * A) + * + * J'(x) = (2 * Vi * x - Vi^2 + 3 * x^2) / L^2 + */ +float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf) { + // 0 iterations (a reasonable estimate) + float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate + +#if (GET_VELOCITY_ITERATIONS > 0) + const float L2 = L * L; + const float Vi2 = Vi * Vi; + + for (int i = 0; i < GET_VELOCITY_ITERATIONS; i++) + // x' = x - Z(x) / J'(x) + x = x - ((x - Vi) * square(Vi + x) / L2 - bf->jerk) / + ((2 * Vi * x - Vi2 + 3 * x * x) / L2); +#endif + + return x; +} diff --git a/avr/src/plan/planner.h b/avr/src/plan/planner.h new file mode 100644 index 0000000..04c5a6d --- /dev/null +++ b/avr/src/plan/planner.h @@ -0,0 +1,89 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2013 - 2015 Alden S. Hart, Jr. + Copyright (c) 2013 - 2015 Robert Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + +#define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) +#define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) + +#define MIN_SEGMENT_TIME_PLUS_MARGIN \ + ((MIN_SEGMENT_USEC + 1) / MICROSECONDS_PER_MINUTE) + +/// Error percentage for iteration convergence. As percent - 0.01 = 1% +#define TRAPEZOID_ITERATION_ERROR_PERCENT 0.1 + +/// Adaptive velocity tolerance term +#define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100)) + +/*** If the absolute value of the remaining deceleration length would be less + * than this value then finish the deceleration in the current move. This is + * used to avoid creating segements before or after the hold which are too + * short to process correctly. + */ +#define HOLD_DECELERATION_TOLERANCE 1 // In mm + + +typedef enum { + SECTION_HEAD, // acceleration + SECTION_BODY, // cruise + SECTION_TAIL, // deceleration +} move_section_t; + + +void mp_init(); + +void mp_set_axis_position(int axis, float position); +float mp_get_axis_position(int axis); +void mp_set_position(const float p[]); +void mp_set_plan_steps(bool plan_steps); + +void mp_flush_planner(); +void mp_kinematics(const float travel[], float steps[]); + +void mp_plan(mp_buffer_t *bf); +void mp_replan_all(); + +void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line); + +float mp_get_target_length(float Vi, float Vf, float jerk); +float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf); diff --git a/avr/src/plan/runtime.c b/avr/src/plan/runtime.c new file mode 100644 index 0000000..01de326 --- /dev/null +++ b/avr/src/plan/runtime.c @@ -0,0 +1,213 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2012 - 2015 Rob Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + + float feed; + feed_mode_t feed_mode; + float feed_override; + float spindle_override; + + plane_t plane; + units_t units; + coord_system_t coord_system; + bool absolute_mode; + path_mode_t path_mode; + distance_mode_t distance_mode; + distance_mode_t arc_distance_mode; + + float previous_error[MOTORS]; +} mp_runtime_t; + +static mp_runtime_t rt = {0}; + + +bool mp_runtime_is_busy() {return rt.busy;} +void mp_runtime_set_busy(bool busy) {rt.busy = busy;} +int32_t mp_runtime_get_line() {return rt.line;} +void mp_runtime_set_line(int32_t line) {rt.line = line; report_request();} +uint8_t mp_runtime_get_tool() {return rt.tool;} +void mp_runtime_set_tool(uint8_t tool) {rt.tool = tool; report_request();} + + +/// Returns current segment velocity +float mp_runtime_get_velocity() {return rt.velocity;} + + +void mp_runtime_set_velocity(float velocity) { + rt.velocity = velocity; + report_request(); +} + + +/// Set encoder counts to the runtime position +void mp_runtime_set_steps_from_position() { + // Convert lengths to steps in floating point + float steps[MOTORS]; + mp_kinematics(rt.position, steps); + + for (int motor = 0; motor < MOTORS; motor++) + // Write steps to encoder register + motor_set_encoder(motor, steps[motor]); +} + + +/* Since steps are in motor space you have to run the position vector + * through inverse kinematics to get the right numbers. This means + * that in a non-Cartesian robot changing any position can result in + * changes to multiple step values. So this operation is provided as a + * single function and always uses the new position vector as an + * input. + * + * Keeping track of position is complicated by the fact that moves + * exist in several reference frames. The scheme to keep this + * straight is: + * + * - mp_position - start and end position for planning + * - rt.position - current position of runtime segment + * - rt.steps.* - position in steps + * + * Note that position is set immediately when called and may not be + * an accurate representation of the tool position. The motors + * are still processing the action and the real tool position is + * still close to the starting point. + */ + + +/// Set runtime position for a single axis +void mp_runtime_set_axis_position(uint8_t axis, const float position) { + rt.position[axis] = position; + report_request(); +} + + +/// Returns current axis position in machine coordinates +float mp_runtime_get_axis_position(uint8_t axis) {return rt.position[axis];} +float *mp_runtime_get_position() {return rt.position;} + + +void mp_runtime_set_position(float position[]) { + copy_vector(rt.position, position); + report_request(); +} + + +/// Returns axis position in work coordinates that were in effect at plan time +float mp_runtime_get_work_position(uint8_t axis) { + return rt.position[axis] - rt.work_offset[axis]; +} + + +/// Set offsets +void mp_runtime_set_work_offsets(float offset[]) { + copy_vector(rt.work_offset, offset); +} + + +/// Segment runner +stat_t mp_runtime_move_to_target(float target[], float time) { + // Convert target position to steps. + float steps[MOTORS]; + mp_kinematics(target, steps); + +#ifdef STEP_CORRECTION + int32_t error[MOTORS]; + st_get_error(error); + + float travel[MOTORS]; + float new_length_sqr = 0; + float old_length_sqr = 0; + + for (int motor = 0; motor < MOTORS; motor++) { + travel[motor] = steps[motor] - motor_get_position(motor); + + if (fp_ZERO(travel[motor])) { + motor[travel] = 0; + motor[error] = 0; + } + + error[motor] = 0.5 * (error[motor] - rt.previous_error[motor]); + rt.previous_error[motor] = error[motor]; + + if (error[motor] < -MAX_STEP_CORRECTION) + error[motor] = -MAX_STEP_CORRECTION; + else if (MAX_STEP_CORRECTION < error[motor]) + error[motor] = MAX_STEP_CORRECTION; + + old_length_sqr += square(travel[motor]); + new_length_sqr += square(travel[motor] - error[motor]); + } + + bool use_error = false; + if (!fp_ZERO(new_length_sqr)) { + float new_time = time * invsqrt(old_length_sqr / new_length_sqr); + + if (!isnan(new_time) && !isinf(new_time) && + EPSILON <= new_time && new_time <= MAX_SEGMENT_TIME) { + time = new_time; + use_error = true; + } + } + + if (!use_error) memset(error, 0, sizeof(error)); + +#else + const int32_t error[MOTORS] = {0}; +#endif + + // Call the stepper prep function + RITORNO(st_prep_line(time, steps, error)); + + // Update positions + mp_runtime_set_position(target); + + return STAT_OK; +} diff --git a/avr/src/plan/runtime.h b/avr/src/plan/runtime.h new file mode 100644 index 0000000..ba51b1e --- /dev/null +++ b/avr/src/plan/runtime.h @@ -0,0 +1,58 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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, const float position); +float mp_runtime_get_axis_position(uint8_t axis); + +float *mp_runtime_get_position(); +void mp_runtime_set_position(float position[]); + +float mp_runtime_get_work_position(uint8_t axis); +void mp_runtime_set_work_offsets(float offset[]); + +stat_t mp_runtime_move_to_target(float target[], float time); diff --git a/avr/src/plan/state.c b/avr/src/plan/state.c new file mode 100644 index 0000000..b53ef6f --- /dev/null +++ b/avr/src/plan/state.c @@ -0,0 +1,262 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2013 - 2015 Alden S. Hart, Jr. + Copyright (c) 2013 - 2015 Robert Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "report.h" + +#include + + +typedef struct { + mp_state_t state; + mp_cycle_t cycle; + mp_hold_reason_t hold_reason; + + bool hold_requested; + bool flush_requested; + bool start_requested; + bool resume_requested; + bool optional_pause_requested; +} planner_state_t; + + +static planner_state_t ps = { + .flush_requested = true, // Start out flushing +}; + + +PGM_P mp_get_state_pgmstr(mp_state_t state) { + switch (state) { + case STATE_READY: return PSTR("READY"); + case STATE_ESTOPPED: return PSTR("ESTOPPED"); + case STATE_RUNNING: return PSTR("RUNNING"); + case STATE_STOPPING: return PSTR("STOPPING"); + case STATE_HOLDING: return PSTR("HOLDING"); + } + + return PSTR("INVALID"); +} + + +PGM_P mp_get_cycle_pgmstr(mp_cycle_t cycle) { + switch (cycle) { + case CYCLE_MACHINING: return PSTR("MACHINING"); + case CYCLE_HOMING: return PSTR("HOMING"); + case CYCLE_PROBING: return PSTR("PROBING"); + case CYCLE_CALIBRATING: return PSTR("CALIBRATING"); + case CYCLE_JOGGING: return PSTR("JOGGING"); + } + + return PSTR("INVALID"); +} + + +PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason) { + switch (reason) { + case HOLD_REASON_USER_PAUSE: return PSTR("USER"); + case HOLD_REASON_PROGRAM_PAUSE: return PSTR("PROGRAM"); + case HOLD_REASON_PROGRAM_END: return PSTR("END"); + case HOLD_REASON_PALLET_CHANGE: return PSTR("PALLET"); + case HOLD_REASON_TOOL_CHANGE: return PSTR("TOOL"); + } + + return PSTR("INVALID"); +} + + +mp_state_t mp_get_state() {return ps.state;} +mp_cycle_t mp_get_cycle() {return ps.cycle;} + + +static void _set_state(mp_state_t state) { + if (ps.state == state) return; // No change + if (ps.state == STATE_ESTOPPED) return; // Can't leave EStop state + if (state == STATE_READY) mp_runtime_set_line(0); + ps.state = state; + report_request(); +} + + +void mp_set_cycle(mp_cycle_t cycle) { + if (ps.cycle == cycle) return; // No change + + if (ps.state != STATE_READY && cycle != CYCLE_MACHINING) { + STATUS_ERROR(STAT_INTERNAL_ERROR, "Cannot transition to %S while %S", + mp_get_cycle_pgmstr(cycle), + mp_get_state_pgmstr(ps.state)); + return; + } + + if (ps.cycle != CYCLE_MACHINING && cycle != CYCLE_MACHINING) { + STATUS_ERROR(STAT_INTERNAL_ERROR, + "Cannot transition to cycle %S while in %S", + mp_get_cycle_pgmstr(cycle), + mp_get_cycle_pgmstr(ps.cycle)); + return; + } + + ps.cycle = cycle; + report_request(); +} + + +mp_hold_reason_t mp_get_hold_reason() {return ps.hold_reason;} + + +void mp_set_hold_reason(mp_hold_reason_t reason) { + if (ps.hold_reason == reason) return; // No change + ps.hold_reason = reason; + report_request(); +} + + +bool mp_is_flushing() {return ps.flush_requested && !ps.resume_requested;} +bool mp_is_resuming() {return ps.resume_requested;} + + +bool mp_is_quiescent() { + return (mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING) && + !st_is_busy() && !mp_runtime_is_busy(); +} + + +void mp_state_optional_pause() { + if (ps.optional_pause_requested) { + mp_set_hold_reason(HOLD_REASON_USER_PAUSE); + mp_state_holding(); + } +} + + +void mp_state_holding() { + _set_state(STATE_HOLDING); + mp_set_plan_steps(false); +} + + +void mp_state_running() { + if (mp_get_state() == STATE_READY) _set_state(STATE_RUNNING); +} + + +void mp_state_idle() { + if (mp_get_state() == STATE_RUNNING) _set_state(STATE_READY); +} + + +void mp_state_estop() {_set_state(STATE_ESTOPPED);} + + +void mp_request_hold() {ps.hold_requested = true;} +void mp_request_start() {ps.start_requested = true;} +void mp_request_flush() {ps.flush_requested = true;} +void mp_request_resume() {if (ps.flush_requested) ps.resume_requested = true;} +void mp_request_optional_pause() {ps.optional_pause_requested = true;} + + +void mp_request_step() { + mp_set_plan_steps(true); + ps.start_requested = true; +} + + +/*** Feedholds, queue flushes and starts are all related. Request functions + * set flags. The callback interprets the flags according to these rules: + * + * A hold request received: + * - during motion is honored + * - during a feedhold is ignored and reset + * - when already stopped is ignored and reset + * + * A flush request received: + * - during motion is ignored but not reset + * - during a feedhold is deferred until the feedhold enters HOLDING state. + * I.e. until deceleration is complete. + * - when stopped or holding and the planner is not busy, is honored + * + * A start request received: + * - during motion is ignored and reset + * - during a feedhold is deferred until the feedhold enters HOLDING state. + * I.e. until deceleration is complete. If a queue flush request is also + * present the queue flush is done first + * - when stopped is honored and starts to run anything in the planner queue + */ +void mp_state_callback() { + if (ps.hold_requested || ps.flush_requested) { + ps.hold_requested = false; + mp_set_hold_reason(HOLD_REASON_USER_PAUSE); + + if (mp_get_state() == STATE_RUNNING) _set_state(STATE_STOPPING); + } + + // Only flush queue when idle or holding. + if (ps.flush_requested && mp_is_quiescent()) { + mach_abort_arc(); + + if (!mp_queue_is_empty()) { + mp_flush_planner(); + + // NOTE The following uses low-level mp calls for absolute position. + // Reset to actual machine position. Otherwise machine is set to the + // position of the last queued move. + for (int axis = 0; axis < AXES; axis++) + mach_set_axis_position(axis, mp_runtime_get_axis_position(axis)); + } + + // Resume + if (ps.resume_requested) { + ps.flush_requested = ps.resume_requested = false; + _set_state(STATE_READY); + } + } + + // Don't start while flushing or stopping + if (ps.start_requested && !ps.flush_requested && + mp_get_state() != STATE_STOPPING) { + ps.start_requested = false; + ps.optional_pause_requested = false; + + if (mp_get_state() == STATE_HOLDING) { + // Check if any moves are buffered + if (!mp_queue_is_empty()) { + // Always replan when coming out of a hold + mp_replan_all(); + _set_state(STATE_RUNNING); + + } else _set_state(STATE_READY); + } + } +} diff --git a/avr/src/plan/state.h b/avr/src/plan/state.h new file mode 100644 index 0000000..a6a62aa --- /dev/null +++ b/avr/src/plan/state.h @@ -0,0 +1,93 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2013 - 2015 Alden S. Hart, Jr. + Copyright (c) 2013 - 2015 Robert Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 { + STATE_READY, + STATE_ESTOPPED, + STATE_RUNNING, + STATE_STOPPING, + STATE_HOLDING, +} mp_state_t; + + +typedef enum { + CYCLE_MACHINING, + CYCLE_HOMING, + CYCLE_PROBING, + CYCLE_CALIBRATING, + CYCLE_JOGGING, +} mp_cycle_t; + + +typedef enum { + HOLD_REASON_USER_PAUSE, + HOLD_REASON_PROGRAM_PAUSE, + HOLD_REASON_PROGRAM_END, + HOLD_REASON_PALLET_CHANGE, + HOLD_REASON_TOOL_CHANGE, +} mp_hold_reason_t; + + +PGM_P mp_get_state_pgmstr(mp_state_t state); +PGM_P mp_get_cycle_pgmstr(mp_cycle_t cycle); +PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason); + +mp_state_t mp_get_state(); + +mp_cycle_t mp_get_cycle(); +void mp_set_cycle(mp_cycle_t cycle); + +mp_hold_reason_t mp_get_hold_reason(); +void mp_set_hold_reason(mp_hold_reason_t reason); + +bool mp_is_flushing(); +bool mp_is_resuming(); +bool mp_is_quiescent(); + +void mp_state_optional_pause(); +void mp_state_holding(); +void mp_state_running(); +void mp_state_idle(); +void mp_state_estop(); + +void mp_request_hold(); +void mp_request_start(); +void mp_request_flush(); +void mp_request_resume(); +void mp_request_optional_pause(); +void mp_request_step(); + +void mp_state_callback(); diff --git a/avr/src/probing.c b/avr/src/probing.c new file mode 100644 index 0000000..5085847 --- /dev/null +++ b/avr/src/probing.c @@ -0,0 +1,216 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 "machine.h" +#include "axis.h" +#include "spindle.h" +#include "switch.h" +#include "util.h" + +#include "plan/planner.h" +#include "plan/runtime.h" +#include "plan/state.h" + +#include + +#include +#include +#include +#include + + +#define MINIMUM_PROBE_TRAVEL 0.254 + + +typedef enum { + PROBE_FAILED, // probe reached endpoint without triggering + PROBE_SUCCEEDED, // probe was triggered, pb.results has position + PROBE_WAITING, // probe is waiting to be started +} probe_state_t; + + +typedef struct { + probe_state_t state; + float results[AXES]; // probing results + + void (*func)(); // binding for callback function state machine + + // state saved from gcode model + uint8_t saved_distance_mode; // G90, G91 global setting + uint8_t saved_coord_system; // G54 - G59 setting + + // probe destination + float start_position[AXES]; + float target[AXES]; + bool flags[AXES]; +} probing_t; + + +static probing_t pb = {0}; + + +/* Note: When coding a cycle (like this one) you get to perform one + * queued move per entry into the continuation, then you must exit. + * + * Another Note: When coding a cycle (like this one) you must wait + * until the last move has actually been queued (or has finished) + * before declaring the cycle to be done. Otherwise there is a nasty + * race condition that will accept the next command before the position + * of the final move has been recorded in the Gcode model. + */ + + +static void _probe_restore_settings() { + // we should be stopped now, but in case of switch closure + mp_flush_planner(); + + // restore coordinate system and distance mode + mach_set_coord_system(pb.saved_coord_system); + mach_set_distance_mode(pb.saved_distance_mode); + + // update the model with actual position + mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); + + mp_set_cycle(CYCLE_MACHINING); // Default cycle +} + + +static void _probing_error_exit(stat_t status) { + _probe_restore_settings(); // clean up and exit + status_error(status); +} + + +static void _probing_finish() { + bool closed = switch_is_active(SW_PROBE); + pb.state = closed ? PROBE_SUCCEEDED : PROBE_FAILED; + + for (int axis = 0; axis < AXES; axis++) { + // if we got here because of a feed hold keep the model position correct + mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); + + // store the probe results + pb.results[axis] = mach_get_axis_position(axis); + } + + _probe_restore_settings(); +} + + +static void _probing_start() { + // initial probe state, don't probe if we're already contacted! + bool closed = switch_is_active(SW_PROBE); + + if (!closed) { + stat_t status = mach_feed(pb.target, pb.flags); + if (status) return _probing_error_exit(status); + } + + pb.func = _probing_finish; +} + + +/* G38.2 homing cycle using limit switches + * + * These initializations are required before starting the probing cycle. + * They must be done after the planner has exhasted all current CYCLE moves as + * they affect the runtime (specifically the switch modes). Side effects would + * include limit switches initiating probe actions instead of just killing + * movement + */ +static void _probing_init() { + // NOTE: it is *not* an error condition for the probe not to trigger. + // it is an error for the limit or homing switches to fire, or for some other + // configuration error. + pb.state = PROBE_FAILED; + mp_set_cycle(CYCLE_PROBING); + + // initialize the axes + for (int axis = 0; axis < AXES; axis++) + pb.start_position[axis] = mach_get_axis_position(axis); + + // error if the probe target is too close to the current position + if (axis_get_vector_length(pb.start_position, pb.target) < + MINIMUM_PROBE_TRAVEL) + _probing_error_exit(STAT_PROBE_INVALID_DEST); + + // error if the probe target requires a move along the A/B/C axes + for (int axis = 0; axis < AXES; axis++) + if (fp_NE(pb.start_position[axis], pb.target[axis])) + _probing_error_exit(STAT_MOVE_DURING_PROBE); + + // probe in absolute machine coords + pb.saved_coord_system = mach_get_coord_system(); + pb.saved_distance_mode = mach_get_distance_mode(); + mach_set_distance_mode(ABSOLUTE_MODE); + mach_set_coord_system(ABSOLUTE_COORDS); + + mach_set_spindle_mode(SPINDLE_OFF); + + // start the move + pb.func = _probing_start; +} + + +bool mach_is_probing() { + return mp_get_cycle() == CYCLE_PROBING || pb.state == PROBE_WAITING; +} + + +/// G38.2 homing cycle using limit switches +stat_t mach_probe(float target[], bool flags[]) { + // trap zero feed rate condition + if (mach_get_feed_mode() != INVERSE_TIME_MODE && + fp_ZERO(mach_get_feed_rate())) + return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; + + // trap no axes specified + if (!flags[AXIS_X] && !flags[AXIS_Y] && !flags[AXIS_Z]) + return STAT_GCODE_AXIS_IS_MISSING; + + // set probe move endpoint + copy_vector(pb.target, target); // set probe move endpoint + copy_vector(pb.flags, flags); // set axes involved on the move + clear_vector(pb.results); // clear the old probe position. + // NOTE: relying on pb.results will not detect a probe to (0, 0, 0). + + // wait until planner queue empties before completing initialization + pb.state = PROBE_WAITING; + pb.func = _probing_init; // bind probing initialization function + + return STAT_OK; +} + + +/// main loop callback for running the homing cycle +void mach_probe_callback() { + // sync to planner move ends + if (!mach_is_probing() || mp_get_state() != STATE_READY) return; + + pb.func(); // execute the current homing move +} diff --git a/avr/src/probing.h b/avr/src/probing.h new file mode 100644 index 0000000..0e3fc01 --- /dev/null +++ b/avr/src/probing.h @@ -0,0 +1,37 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 mach_is_probing(); +stat_t mach_probe(float target[], bool flags[]); +void mach_probe_callback(); diff --git a/avr/src/pwm_spindle.c b/avr/src/pwm_spindle.c new file mode 100644 index 0000000..25d40eb --- /dev/null +++ b/avr/src/pwm_spindle.c @@ -0,0 +1,152 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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" + + +typedef struct { + uint16_t freq; // base frequency for PWM driver, in Hz + float min_rpm; + float max_rpm; + float min_duty; + float max_duty; + bool reverse; + bool enable_invert; + bool estop; +} pwm_spindle_t; + + +static pwm_spindle_t spindle = { + .freq = SPINDLE_PWM_FREQUENCY, + .min_rpm = SPINDLE_MIN_RPM, + .max_rpm = SPINDLE_MAX_RPM, + .min_duty = SPINDLE_MIN_DUTY, + .max_duty = SPINDLE_MAX_DUTY, + .reverse = SPINDLE_REVERSE, + .enable_invert = false, + .estop = false, +}; + + +static void _spindle_set_pwm(spindle_mode_t mode, float speed) { + if (mode == SPINDLE_OFF || speed < spindle.min_rpm || spindle.estop) { + TIMER_PWM.CTRLA = 0; + return; + } + + // Clamp speed + if (spindle.max_rpm < speed) speed = spindle.max_rpm; + + // Set clock period and optimal prescaler value + float prescale = F_CPU / 65536.0 / spindle.freq; + if (prescale <= 1) { + TIMER_PWM.PER = F_CPU / spindle.freq; + TIMER_PWM.CTRLA = TC_CLKSEL_DIV1_gc; + + } else if (prescale <= 2) { + TIMER_PWM.PER = F_CPU / 2 / spindle.freq; + TIMER_PWM.CTRLA = TC_CLKSEL_DIV2_gc; + + } else if (prescale <= 4) { + TIMER_PWM.PER = F_CPU / 4 / spindle.freq; + TIMER_PWM.CTRLA = TC_CLKSEL_DIV4_gc; + + } else if (prescale <= 8) { + TIMER_PWM.PER = F_CPU / 8 / spindle.freq; + TIMER_PWM.CTRLA = TC_CLKSEL_DIV8_gc; + + } else if (prescale <= 64) { + TIMER_PWM.PER = F_CPU / 64 / spindle.freq; + TIMER_PWM.CTRLA = TC_CLKSEL_DIV64_gc; + + } else TIMER_PWM.CTRLA = 0; + + // Map RPM to duty cycle + float duty = (speed - spindle.min_rpm) / (spindle.max_rpm - spindle.min_rpm) * + (spindle.max_duty - spindle.min_duty) + spindle.min_duty; + + TIMER_PWM.CCB = TIMER_PWM.PER * duty; +} + + +static void _spindle_set_enable(bool enable) { + if (enable ^ spindle.enable_invert) + PORT(SPIN_ENABLE_PIN)->OUTSET = BM(SPIN_ENABLE_PIN); + else PORT(SPIN_ENABLE_PIN)->OUTCLR = BM(SPIN_ENABLE_PIN); +} + + +static void _spindle_set_dir(bool forward) { + if (forward ^ spindle.reverse) PORT(SPIN_DIR_PIN)->OUTCLR = BM(SPIN_DIR_PIN); + else PORT(SPIN_DIR_PIN)->OUTSET = BM(SPIN_DIR_PIN); +} + + +void pwm_spindle_init() { + // Configure IO + PORT(SPIN_PWM_PIN)->DIRSET = BM(SPIN_PWM_PIN); // PWM Output + _spindle_set_dir(true); + PORT(SPIN_DIR_PIN)->DIRSET = BM(SPIN_DIR_PIN); // Dir Output + _spindle_set_enable(false); + PORT(SPIN_ENABLE_PIN)->DIRSET = BM(SPIN_ENABLE_PIN); // Enable output + + // Configure clock + TIMER_PWM.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SINGLESLOPE_gc; +} + + +void pwm_spindle_set(spindle_mode_t mode, float speed) { + _spindle_set_dir(mode == SPINDLE_CW); + _spindle_set_pwm(mode, speed); + _spindle_set_enable(mode != SPINDLE_OFF && TIMER_PWM.CTRLA); +} + + +void pwm_spindle_estop() { + spindle.estop = true; + _spindle_set_pwm(SPINDLE_OFF, 0); +} + + +// TODO these need more effort and should work with the huanyang spindle too +float get_max_spin(int index) {return spindle.max_rpm;} +void set_max_spin(int axis, float value) {spindle.max_rpm = value;} +float get_min_spin(int index) {return spindle.min_rpm;} +void set_min_spin(int axis, float value) {spindle.min_rpm = value;} +float get_spin_min_pulse(int index) {return spindle.min_duty;} +void set_spin_min_pulse(int axis, float value) {spindle.min_duty = value;} +float get_spin_max_pulse(int index) {return spindle.max_duty;} +void set_spin_max_pulse(int axis, float value) {spindle.max_duty = value;} +uint8_t get_spin_reverse(int index) {return spindle.reverse;} +void set_spin_reverse(int axis, uint8_t value) {spindle.reverse = value;} +float get_spin_up(int index) {return 0;} +void set_spin_up(int axis, float value) {} +float get_spin_down(int index) {return 0;} +void set_spin_down(int axis, float value) {} diff --git a/avr/src/pwm_spindle.h b/avr/src/pwm_spindle.h new file mode 100644 index 0000000..2177876 --- /dev/null +++ b/avr/src/pwm_spindle.h @@ -0,0 +1,35 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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_estop(); diff --git a/avr/src/report.c b/avr/src/report.c new file mode 100644 index 0000000..b39bfd1 --- /dev/null +++ b/avr/src/report.c @@ -0,0 +1,66 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + +#include +#include + + +static bool report_requested = false; +static bool report_full = false; +static uint32_t last_report = 0; + + +void report_request() { + report_requested = true; +} + + +void report_request_full() { + report_requested = report_full = true; +} + + +void report_callback() { + if (usart_tx_full()) return; // Wait for buffer space + + if (report_requested && usart_tx_empty()) { + uint32_t now = rtc_get_time(); + if (now - last_report < 100) return; + last_report = now; + + vars_report(report_full); + report_requested = report_full = false; + } +} diff --git a/avr/src/report.h b/avr/src/report.h new file mode 100644 index 0000000..c66f05a --- /dev/null +++ b/avr/src/report.h @@ -0,0 +1,35 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..eff59c9 --- /dev/null +++ b/avr/src/ringbuf.def @@ -0,0 +1,158 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..73f5947 --- /dev/null +++ b/avr/src/rtc.c @@ -0,0 +1,77 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2013 Alden S. Hart Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..3d69237 --- /dev/null +++ b/avr/src/rtc.h @@ -0,0 +1,38 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2013 Alden S. Hart Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..7a57fb5 --- /dev/null +++ b/avr/src/spindle.c @@ -0,0 +1,113 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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_PWM, + SPINDLE_TYPE_HUANYANG, +} spindle_type_t; + + +typedef struct { + spindle_type_t type; + spindle_mode_t mode; + float speed; +} spindle_t; + + +static spindle_t spindle = {.type = SPINDLE_TYPE}; + + +void spindle_init() { + pwm_spindle_init(); + huanyang_init(); +} + + +void _spindle_set(spindle_mode_t mode, float speed) { + spindle.mode = mode; + spindle.speed = speed; + + switch (spindle.type) { + case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, speed); break; + case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, speed); break; + } +} + + +void spindle_set_mode(spindle_mode_t mode) { + spindle.mode = mode; + + switch (spindle.type) { + case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, spindle.speed); break; + case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, spindle.speed); break; + } +} + + +void spindle_set_speed(float speed) { + spindle.speed = speed; + + switch (spindle.type) { + case SPINDLE_TYPE_PWM: pwm_spindle_set(spindle.mode, speed); break; + case SPINDLE_TYPE_HUANYANG: huanyang_set(spindle.mode, speed); break; + } +} + + +spindle_mode_t spindle_get_mode() {return spindle.mode;} +float spindle_get_speed() {return spindle.speed;} + + +void spindle_estop() { + switch (spindle.type) { + case SPINDLE_TYPE_PWM: pwm_spindle_estop(); break; + case SPINDLE_TYPE_HUANYANG: huanyang_estop(); break; + } +} + + +uint8_t get_spindle_type() {return spindle.type;} + + +void set_spindle_type(uint8_t value) { + if (value != spindle.type) { + spindle_mode_t mode = spindle.mode; + float speed = spindle.speed; + + _spindle_set(SPINDLE_OFF, 0); + spindle.type = value; + _spindle_set(mode, speed); + } +} diff --git a/avr/src/spindle.h b/avr/src/spindle.h new file mode 100644 index 0000000..b583d55 --- /dev/null +++ b/avr/src/spindle.h @@ -0,0 +1,39 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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_estop(); diff --git a/avr/src/status.c b/avr/src/status.c new file mode 100644 index 0000000..ad06bd4 --- /dev/null +++ b/avr/src/status.c @@ -0,0 +1,120 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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\":\"%S\",\"msg\":\""), status_level_pgmstr(level)); + + // Message + if (msg) { + va_start(args, msg); + vfprintf_P(stdout, msg, args); + va_end(args); + + } else printf_P(status_to_pgmstr(code)); + + putchar('"'); + + // Code + if (code) printf_P(PSTR(", \"code\": %d"), code); + + // Location + if (location) printf_P(PSTR(", \"where\": \"%S\""), location); + + putchar('}'); + putchar('\n'); + + return code; +} + + +void status_help() { + putchar('{'); + + for (int i = 0; i < STAT_MAX; i++) { + if (i) putchar(','); + putchar('\n'); + printf_P(PSTR(" \"%d\": \"%S\""), i, status_to_pgmstr(i)); + } + + putchar('\n'); + putchar('}'); + putchar('\n'); +} + + +/// Alarm state; send an exception report and stop processing input +stat_t status_alarm(const char *location, stat_t code) { + status_message_P(location, STAT_LEVEL_ERROR, code, 0); + estop_trigger(code); + while (!usart_tx_empty()) continue; + return code; +} diff --git a/avr/src/status.h b/avr/src/status.h new file mode 100644 index 0000000..2615003 --- /dev/null +++ b/avr/src/status.h @@ -0,0 +1,87 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 + + +// RITORNO is a handy way to provide exception returns +// It returns only if an error occurred. (ritorno is Italian for return) +#define RITORNO(a) if ((status_code = a) != STAT_OK) {return status_code;} + +typedef enum { +#define STAT_MSG(NAME, TEXT) STAT_##NAME, +#include "messages.def" +#undef STAT_MSG + + STAT_DO_NOT_EXCEED = 255 // Do not exceed 255 +} stat_t; + + +typedef enum { + STAT_LEVEL_INFO, + STAT_LEVEL_DEBUG, + STAT_LEVEL_WARNING, + STAT_LEVEL_ERROR, +} status_level_t; + + +extern stat_t status_code; + +const char *status_to_pgmstr(stat_t code); +const char *status_level_pgmstr(status_level_t level); +stat_t status_error(stat_t code); +stat_t status_message_P(const char *location, status_level_t level, + stat_t code, const char *msg, ...); +void status_help(); + +/// Enter alarm state. returns same status code +stat_t status_alarm(const char *location, stat_t status); + +#define TO_STRING(x) _TO_STRING(x) +#define _TO_STRING(x) #x + +#define STATUS_LOCATION PSTR(__FILE__ ":" TO_STRING(__LINE__)) + +#define STATUS_MESSAGE(LEVEL, CODE, MSG, ...) \ + status_message_P(STATUS_LOCATION, LEVEL, CODE, PSTR(MSG), ##__VA_ARGS__) + +#define STATUS_INFO(MSG, ...) \ + STATUS_MESSAGE(STAT_LEVEL_INFO, STAT_OK, MSG, ##__VA_ARGS__) + +#define STATUS_DEBUG(MSG, ...) \ + STATUS_MESSAGE(STAT_LEVEL_WARNING, STAT_OK, MSG, ##__VA_ARGS__) + +#define STATUS_WARNING(MSG, ...) \ + STATUS_MESSAGE(STAT_LEVEL_WARNING, STAT_OK, MSG, ##__VA_ARGS__) + +#define STATUS_ERROR(CODE, MSG, ...) \ + STATUS_MESSAGE(STAT_LEVEL_ERROR, CODE, MSG, ##__VA_ARGS__) + +#define ALARM(CODE) status_alarm(STATUS_LOCATION, CODE) +#define ASSERT(COND) do {if (!(COND)) ALARM(STAT_INTERNAL_ERROR);} while (0) diff --git a/avr/src/stepper.c b/avr/src/stepper.c new file mode 100644 index 0000000..c929d25 --- /dev/null +++ b/avr/src/stepper.c @@ -0,0 +1,233 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2013 - 2015 Robert Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 +#include + + +typedef enum { + MOVE_TYPE_NULL, // null move - does a no-op + MOVE_TYPE_ALINE, // acceleration planned line + MOVE_TYPE_DWELL, // delay with no movement +} move_type_t; + + +typedef struct { + // Runtime + bool busy; + bool requesting; + uint16_t dwell; + + // Move prep + bool move_ready; // prepped move ready for loader + bool move_queued; // prepped move queued + move_type_t move_type; + uint16_t seg_period; + uint32_t prep_dwell; +} stepper_t; + + +static stepper_t st = {0}; + + +void stepper_init() { + // Setup step timer + TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // waveform mode + TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // interrupt mode + TIMER_STEP.PER = STEP_TIMER_POLL; // timer idle rate + TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // start step timer +} + + +void st_shutdown() { + for (int motor = 0; motor < MOTORS; motor++) + motor_enable(motor, false); + + st.dwell = 0; + st.move_type = MOVE_TYPE_NULL; +} + + +/// Return true if motors or dwell are running +bool st_is_busy() {return st.busy;} + + +/// Interrupt handler for calling move exec function. +/// ADC channel 0 triggered by load ISR as a "software" interrupt. +ISR(ADCB_CH0_vect) { + while (true) { + stat_t status = mp_exec_move(); + + switch (status) { + case STAT_NOOP: st.busy = false; break; // No command executed + case STAT_EAGAIN: continue; // No command executed, try again + + case STAT_OK: // Move executed + if (!st.move_queued) ALARM(STAT_EXPECTED_MOVE); // No move was queued + st.move_queued = false; + st.move_ready = true; + break; + + default: ALARM(status); break; + } + + break; + } + + ADCB_CH0_INTCTRL = 0; + st.requesting = false; +} + + +static void _request_exec_move() { + if (st.requesting) return; + st.requesting = true; + + // Use ADC as a "software" interrupt to trigger next move exec + ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc; // LO level interrupt + ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm; +} + + +/// Step timer interrupt routine +/// Dequeue move and load into stepper struct +ISR(STEP_TIMER_ISR) { + // Dwell + if (st.dwell && --st.dwell) return; + + // End last move + TIMER_STEP.PER = STEP_TIMER_POLL; + + DMA.INTFLAGS = 0xff; // clear all interrups + for (int motor = 0; motor < MOTORS; motor++) + motor_end_move(motor); + + if (estop_triggered()) { + st.move_type = MOVE_TYPE_NULL; + return; + } + + // If the next move is not ready try to load it + if (!st.move_ready) { + _request_exec_move(); + return; + } + + // Wait until all motors have energized + if (motor_energizing()) return; + + // Start move + if (st.seg_period) { + for (int motor = 0; motor < MOTORS; motor++) + motor_load_move(motor); + + TIMER_STEP.PER = st.seg_period; + st.busy = true; + + // Start dwell + st.dwell = st.prep_dwell; + } + + // We are done with this move + st.move_type = MOVE_TYPE_NULL; + st.seg_period = 0; // clear timer + st.prep_dwell = 0; // clear dwell + st.move_ready = false; // flip the flag back + + // Request next move if not currently in a dwell. Requesting the next move + // may power up motors and the motors should not be powered up during a dwell. + if (!st.dwell) _request_exec_move(); +} + + +/* Prepare the next move + * + * This function precomputes the next pulse segment (move) so it can + * be executed quickly in the ISR. It works in steps, rather than + * length units. All args are provided as floats which converted here + * to integer values. + * + * Args: + * @param target signed position in steps for each motor. + * Steps are fractional. Their sign indicates direction. Motors not in the + * move have 0 steps. + * + * @param time is segment run time in minutes. If timing is not 100% + * accurate this will affect the move velocity but not travel distance. + */ +stat_t st_prep_line(float time, const float target[], const int32_t error[]) { + // Trap conditions that would prevent queueing the line + if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR); + if (isinf(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_INFINITE); + if (isnan(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_NAN); + if (time < EPSILON) return ALARM(STAT_MINIMUM_TIME_MOVE); + if (MAX_SEGMENT_TIME < time) return ALARM(STAT_MAXIMUM_TIME_MOVE); + + // Setup segment parameters + st.move_type = MOVE_TYPE_ALINE; + st.seg_period = round(time * 60 * STEP_TIMER_FREQ); // Must fit 16-bit + int32_t seg_clocks = (int32_t)st.seg_period * STEP_TIMER_DIV; + + // Prepare motor moves + for (int motor = 0; motor < MOTORS; motor++) + RITORNO + (motor_prep_move(motor, seg_clocks, target[motor], error[motor], time)); + + st.move_queued = true; // signal prep buffer ready (do this last) + + return STAT_OK; +} + + +/// Add a dwell to the move buffer +void st_prep_dwell(float seconds) { + if (st.move_ready) ALARM(STAT_INTERNAL_ERROR); + st.move_type = MOVE_TYPE_DWELL; + st.seg_period = STEP_TIMER_FREQ * 0.001; // 1 ms + st.prep_dwell = seconds * 1000; // convert to ms + st.move_queued = true; // signal prep buffer ready +} + + +void st_get_error(int32_t error[]) { + for (int motor = 0; motor < MOTORS; motor++) + error[motor] = motor_get_error(motor); +} diff --git a/avr/src/stepper.h b/avr/src/stepper.h new file mode 100644 index 0000000..3aa57b9 --- /dev/null +++ b/avr/src/stepper.h @@ -0,0 +1,43 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + Copyright (c) 2012 - 2015 Rob Giseburt + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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(); +bool st_is_busy(); +stat_t st_prep_line(float time, const float target[], const int32_t error[]); +void st_prep_dwell(float seconds); +void st_get_error(int32_t error[]); diff --git a/avr/src/switch.c b/avr/src/switch.c new file mode 100644 index 0000000..d97632a --- /dev/null +++ b/avr/src/switch.c @@ -0,0 +1,204 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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" + +\******************************************************************************/ + +/* Normally open switches (NO) trigger an interrupt on the + * falling edge and lockout subsequent interrupts for the defined + * lockout period. This approach beats doing debouncing as an + * integration as switches fire immediately. + * + * Normally closed switches (NC) trigger an interrupt on the + * rising edge and lockout subsequent interrupts for the defined + * lockout period. + * + * These functions interact with each other to process switch closures + * and firing. Each switch has a counter which is initially set to + * negative SW_DEGLITCH_TICKS. When a switch closure is DETECTED the + * count increments for each RTC tick. When the count reaches zero + * the switch is tripped and action occurs. The counter continues to + * increment positive until the lockout is exceeded. + */ + +#include "switch.h" +#include "config.h" + +#include + +#include + + +typedef enum { + SW_IDLE, + SW_DEGLITCHING, + SW_LOCKOUT +} switch_debounce_t; + + +typedef struct { + uint8_t pin; + switch_type_t type; + + switch_callback_t cb; + bool state; + switch_debounce_t debounce; + int16_t count; +} switch_t; + + + +static switch_t switches[SWITCHES] = { + {.pin = MIN_X_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MAX_X_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MIN_Y_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MAX_X_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MIN_Z_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MAX_Z_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MIN_A_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MAX_A_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = ESTOP_PIN, .type = SW_NORMALLY_OPEN}, + // {.pin = PROBE_PIN, .type = SW_NORMALLY_OPEN}, +}; + + +static bool _read_state(const switch_t *s) { + // A normally open switch drives the pin low when thrown + return (s->type == SW_NORMALLY_OPEN) ^ IN_PIN(s->pin); +} + + +static void _switch_isr() { + for (int i = 0; i < SWITCHES; i++) { + switch_t *s = &switches[i]; + bool state = _read_state(s); + + if (state == s->state || s->type == SW_DISABLED) continue; + + s->debounce = SW_DEGLITCHING; + s->count = -SW_DEGLITCH_TICKS; // reset deglitch count + s->state = state; + PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0 + } +} + + +// Switch interrupt handler vectors +ISR(PORTA_INT0_vect) {_switch_isr();} +ISR(PORTB_INT0_vect) {_switch_isr();} +ISR(PORTC_INT0_vect) {_switch_isr();} +ISR(PORTD_INT0_vect) {_switch_isr();} +ISR(PORTE_INT0_vect) {_switch_isr();} +ISR(PORTF_INT0_vect) {_switch_isr();} + + +void _switch_enable(switch_t *s, bool enable) { + if (enable) { + PORT(s->pin)->INT0MASK |= BM(s->pin); // Enable INT0 + s->state = _read_state(s); // Initialize state + s->debounce = SW_IDLE; + + } else PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0 +} + + +void switch_init() { + for (int i = 0; i < SWITCHES; i++) { + switch_t *s = &switches[i]; + + // Pull up and trigger on both edges + PINCTRL_PIN(s->pin) = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; + DIRCLR_PIN(s->pin); // Input + PORT(s->pin)->INTCTRL |= SWITCH_INTLVL; // Set interrupt level + + _switch_enable(s, s->type != SW_DISABLED); + } +} + + +/// Called from RTC on each tick +void switch_rtc_callback() { + for (int i = 0; i < SWITCHES; i++) { + switch_t *s = &switches[i]; + + if (s->type == SW_DISABLED || s->debounce == SW_IDLE) continue; + + // state is either lockout or deglitching + if (++s->count == SW_LOCKOUT_TICKS) { + PORT(s->pin)->INT0MASK |= BM(s->pin); // Renable INT0 + bool state = _read_state(s); + s->debounce = SW_IDLE; + + // check if the state has changed while we were in lockout + if (s->state != state) { + s->state = state; + s->debounce = SW_DEGLITCHING; + s->count = -SW_DEGLITCH_TICKS; + } + + continue; + } + + if (!s->count) { // switch triggered + s->debounce = SW_LOCKOUT; + if (s->cb) s->cb(i, s->state); + } + } +} + + +bool switch_is_active(int index) { + return switches[index].state; +} + + +bool switch_is_enabled(int index) { + return switch_get_type(index) != SW_DISABLED; +} + + +switch_type_t switch_get_type(int index) { + return switches[index].type; +} + + +void switch_set_type(int index, switch_type_t type) { + switch_t *s = &switches[index]; + + if (s->type != type) { + s->type = type; + _switch_enable(s, type != SW_DISABLED); + } +} + + +void switch_set_callback(int index, switch_callback_t cb) { + switches[index].cb = cb; +} + + +// Var callbacks +uint8_t get_switch_type(int index) {return switch_get_type(index);} +void set_switch_type(int index, uint8_t value) {switch_set_type(index, value);} diff --git a/avr/src/switch.h b/avr/src/switch.h new file mode 100644 index 0000000..3d0db61 --- /dev/null +++ b/avr/src/switch.h @@ -0,0 +1,69 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..7d66f63 --- /dev/null +++ b/avr/src/usart.c @@ -0,0 +1,282 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..ccb3ac1 --- /dev/null +++ b/avr/src/usart.h @@ -0,0 +1,77 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 new file mode 100644 index 0000000..74ea051 --- /dev/null +++ b/avr/src/util.c @@ -0,0 +1,49 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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" + + + + +/// Fast inverse square root originally from Quake III Arena code. Original +/// comments left intact. +/// See: https://en.wikipedia.org/wiki/Fast_inverse_square_root +float invsqrt(float number) { + const float threehalfs = 1.5F; + + float x2 = number * 0.5; + float y = number; + long i = *(long *)&y; // evil floating point bit level hacking + i = 0x5f3759df - (i >> 1); // what the fuck? + y = *(float *)&i; + y = y * (threehalfs - x2 * y * y); // 1st iteration + y = y * (threehalfs - x2 * y * y); // 2nd iteration, this can be removed + + return y; //isnan(y) ? 1.0 / sqrt(number) : y; +} diff --git a/avr/src/util.h b/avr/src/util.h new file mode 100644 index 0000000..581bbb3 --- /dev/null +++ b/avr/src/util.h @@ -0,0 +1,68 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2014 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 float min(float a, float b) {return a < b ? a : b;} +inline float max(float a, float b) {return a < b ? b : a;} +inline float min3(float a, float b, float c) {return min(min(a, b), c);} +inline float max3(float a, float b, float c) {return max(max(a, b), c);} +inline float min4(float a, float b, float c, float d) +{return min(min(a, b), min(c, d));} +inline float max4(float a, float b, float c, float d) +{return max(max(a, b), max(c, d));} + +float invsqrt(float number); + +// Floating-point utilities +#define EPSILON 0.00001 // allowable rounding error for floats +inline bool fp_EQ(float a, float b) {return fabs(a - b) < EPSILON;} +inline bool fp_NE(float a, float b) {return fabs(a - b) > EPSILON;} +inline bool fp_ZERO(float a) {return fabs(a) < EPSILON;} +inline bool fp_NOT_ZERO(float a) {return !fp_ZERO(a);} +inline bool fp_FALSE(float a) {return fp_ZERO(a);} +inline bool fp_TRUE(float a) {return !fp_ZERO(a);} + +// Constants +#define MM_PER_INCH 25.4 +#define INCHES_PER_MM (1 / 25.4) +#define MICROSECONDS_PER_MINUTE 60000000.0 diff --git a/avr/src/varcb.c b/avr/src/varcb.c new file mode 100644 index 0000000..8f425ad --- /dev/null +++ b/avr/src/varcb.c @@ -0,0 +1,89 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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_position(int index) {return mp_runtime_get_axis_position(index);} + +// GCode +int32_t get_line() {return mp_runtime_get_line();} +PGM_P get_unit() {return gs_get_units_pgmstr(mach_get_units());} +float get_speed() {return spindle_get_speed();} +float get_feed() {return mach_get_feed_rate();} // TODO get runtime value +uint8_t get_tool() {return mp_runtime_get_tool();} + + +PGM_P get_feed_mode() { + return gs_get_feed_mode_pgmstr(mach_get_feed_mode()); +} + + +PGM_P get_plane() {return gs_get_plane_pgmstr(mach_get_plane());} + + +PGM_P get_coord_system() { + return gs_get_coord_system_pgmstr(mach_get_coord_system()); +} + + +bool get_abs_override() {return mach_get_absolute_mode();} +PGM_P get_path_mode() {return gs_get_path_mode_pgmstr(mach_get_path_mode());} + + +PGM_P get_distance_mode() { + return gs_get_distance_mode_pgmstr(mach_get_distance_mode()); +} + + +PGM_P get_arc_dist_mode() { + return gs_get_distance_mode_pgmstr(mach_get_arc_distance_mode()); +} + + +float get_feed_override() {return mach_get_feed_override();} +float get_speed_override() {return mach_get_spindle_override();} +bool get_mist_coolant() {return coolant_get_mist();} +bool get_flood_coolant() {return coolant_get_flood();} + +// System +float get_velocity() {return mp_runtime_get_velocity();} +bool get_echo() {return usart_is_set(USART_ECHO);} +void set_echo(bool value) {return usart_set(USART_ECHO, value);} +PGM_P get_state() {return mp_get_state_pgmstr(mp_get_state());} +PGM_P get_cycle() {return mp_get_cycle_pgmstr(mp_get_cycle());} + + +PGM_P get_hold_reason() { + return mp_get_hold_reason_pgmstr(mp_get_hold_reason()); +} diff --git a/avr/src/vars.c b/avr/src/vars.c new file mode 100644 index 0000000..6896ee6 --- /dev/null +++ b/avr/src/vars.c @@ -0,0 +1,513 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 +#include +#include +#include +#include +#include +#include + +#include +#include + + +typedef uint8_t flags_t; +typedef const char *string; +typedef PGM_P pstring; + + +// Format strings +static const char code_fmt[] PROGMEM = "\"%s\":"; +static const char indexed_code_fmt[] PROGMEM = "\"%c%s\":"; + + +// Type names +static const char bool_name [] PROGMEM = ""; +#define TYPE_NAME(TYPE) static const char TYPE##_name [] PROGMEM = "<" #TYPE ">" +MAP(TYPE_NAME, SEMI, flags_t, string, pstring, float, uint8_t, uint16_t, + int32_t, char); + + +// String +static void var_print_string(string s) { + printf_P(PSTR("\"%s\""), s); +} + +// Program string +static void var_print_pstring(pstring s) { + printf_P(PSTR("\"%S\""), s); +} + + +// Flags +extern void print_status_flags(uint8_t x); + +static void var_print_flags_t(uint8_t x) { + print_status_flags(x); +} + + +// Float +static void var_print_float(float x) { + char buf[20]; + + int len = sprintf_P(buf, PSTR("%.6f"), x); + + // Remove trailing zeros + for (int i = len; 0 < i; i--) { + if (buf[i - 1] == '.') buf[i - 1] = 0; + else if (buf[i - 1] == '0') { + buf[i - 1] = 0; + continue; + } + + break; + } + + printf(buf); +} + + +static float var_parse_float(const char *value) { + return strtod(value, 0); +} + + +// Bool +static void var_print_bool(bool x) { + printf_P(x ? PSTR("true") : PSTR("false")); +} + + +static bool var_parse_bool(const char *value) { + return !strcasecmp(value, "true") || var_parse_float(value); +} + + +static uint8_t eeprom_read_bool(bool *addr) { + return eeprom_read_byte((uint8_t *)addr); +} + + +static void eeprom_update_bool(bool *addr, bool value) { + eeprom_update_byte((uint8_t *)addr, value); +} + + +// Char +static void var_print_char(char x) {putchar('"'); putchar(x); putchar('"');} +static char var_parse_char(const char *value) {return value[0];} + + +static uint8_t eeprom_read_char(char *addr) { + return eeprom_read_byte((uint8_t *)addr); +} + + +static void eeprom_update_char(char *addr, char value) { + eeprom_update_byte((uint8_t *)addr, value); +} + + +// int8 +#if 0 +static void var_print_int8_t(int8_t x) { + printf_P(PSTR("%"PRIi8), x); +} + + +static int8_t var_parse_int8_t(const char *value) { + return strtol(value, 0, 0); +} + + +static int8_t eeprom_read_int8_t(int8_t *addr) { + return eeprom_read_byte((uint8_t *)addr); +} + + +static void eeprom_update_int8_t(int8_t *addr, int8_t value) { + eeprom_update_byte((uint8_t *)addr, value); +} +#endif + +// uint8 +static void var_print_uint8_t(uint8_t x) { + printf_P(PSTR("%"PRIu8), x); +} + + +static uint8_t var_parse_uint8_t(const char *value) { + return strtol(value, 0, 0); +} + + +static uint8_t eeprom_read_uint8_t(uint8_t *addr) { + return eeprom_read_byte(addr); +} + + +static void eeprom_update_uint8_t(uint8_t *addr, uint8_t value) { + eeprom_update_byte(addr, value); +} + + +// unit16 +static void var_print_uint16_t(uint16_t x) { + printf_P(PSTR("%"PRIu16), x); +} + + +static uint16_t var_parse_uint16_t(const char *value) { + return strtoul(value, 0, 0); +} + + +static uint16_t eeprom_read_uint16_t(uint16_t *addr) { + return eeprom_read_word(addr); +} + + +static void eeprom_update_uint16_t(uint16_t *addr, uint16_t value) { + eeprom_update_word(addr, value); +} + + +// int32 +static void var_print_int32_t(uint32_t x) { + printf_P(PSTR("%"PRIi32), x); +} + + +// Var forward declarations +#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ + TYPE get_##NAME(IF(INDEX)(int index)); \ + IF(SET) \ + (void set_##NAME(IF(INDEX)(int index,) TYPE value);) + +#include "vars.def" +#undef VAR + +// Var names, count & help +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, HELP) \ + static const char NAME##_name[] PROGMEM = #NAME; \ + static const char NAME##_help[] PROGMEM = HELP; + +#include "vars.def" +#undef VAR + +// EEPROM storage +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, HELP) \ + IF(SAVE) \ + (static TYPE NAME##_eeprom IF(INDEX)([INDEX]) EEMEM;) + +#include "vars.def" +#undef VAR + +static uint16_t eeprom_crc EEMEM; + +// Last value +#define VAR(NAME, CODE, TYPE, INDEX, ...) \ + static TYPE NAME##_state IF(INDEX)([INDEX]); + +#include "vars.def" +#undef VAR + + + +void vars_init() { + vars_restore(); + + // Initialize var state +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ + IF(INDEX)(for (int i = 0; i < INDEX; i++)) \ + (NAME##_state)IF(INDEX)([i]) = get_##NAME(IF(INDEX)(i)); + +#include "vars.def" +#undef VAR +} + + +void vars_report(bool full) { + // Save and disable watchdog + uint8_t wd_state = hw_disable_watchdog(); + + bool reported = false; + +#define VAR(NAME, CODE, TYPE, INDEX, ...) \ + IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \ + TYPE value = get_##NAME(IF(INDEX)(i)); \ + TYPE last = (NAME##_state)IF(INDEX)([i]); \ + \ + if (value != last || full) { \ + (NAME##_state)IF(INDEX)([i]) = value; \ + \ + if (!reported) { \ + reported = true; \ + putchar('{'); \ + } else putchar(','); \ + \ + printf_P \ + (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt), \ + IF(INDEX)(INDEX##_LABEL[i],) #CODE); \ + \ + var_print_##TYPE(value); \ + } \ + } + +#include "vars.def" +#undef VAR + + if (reported) printf("}\n"); + + // Restore watchdog + hw_restore_watchdog(wd_state); +} + + +int vars_find(const char *name) { + uint8_t i = 0; + uint8_t n = 0; + unsigned len = strlen(name); + + if (!len) return -1; + +#define VAR(NAME, CODE, TYPE, INDEX, ...) \ + if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \ + IF(INDEX) \ + (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ + if (INDEX <= i) return -1); \ + return n; \ + } \ + n++; + +#include "vars.def" +#undef VAR + + return -1; +} + + +bool vars_print(const char *name) { + uint8_t i; + unsigned len = strlen(name); + + if (!len) return false; + +#define VAR(NAME, CODE, TYPE, INDEX, ...) \ + if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \ + IF(INDEX) \ + (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ + if (INDEX <= i) return false); \ + \ + putchar('{'); \ + printf_P \ + (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt), \ + IF(INDEX)(INDEX##_LABEL[i],) #CODE); \ + var_print_##TYPE(get_##NAME(IF(INDEX)(i))); \ + putchar('}'); \ + \ + return true; \ + } + +#include "vars.def" +#undef VAR + + return false; +} + + +bool vars_set(const char *name, const char *value) { + uint8_t i; + unsigned len = strlen(name); + + if (!len) return false; + +#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ + IF(SET) \ + (if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \ + IF(INDEX) \ + (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ + if (INDEX <= i) return false); \ + \ + TYPE x = var_parse_##TYPE(value); \ + set_##NAME(IF(INDEX)(i,) x); \ + \ + return true; \ + }) \ + +#include "vars.def" +#undef VAR + + return false; +} + + +int vars_parser(char *vars) { + if (!*vars || !*vars++ == '{') return STAT_OK; + + while (*vars) { + while (isspace(*vars)) vars++; + if (*vars == '}') return STAT_OK; + if (*vars != '"') return STAT_COMMAND_NOT_ACCEPTED; + + // Parse name + vars++; // Skip " + const char *name = vars; + while (*vars && *vars != '"') vars++; + *vars++ = 0; + + while (isspace(*vars)) vars++; + if (*vars != ':') return STAT_COMMAND_NOT_ACCEPTED; + vars++; + while (isspace(*vars)) vars++; + + // Parse value + const char *value = vars; + while (*vars && *vars != ',' && *vars != '}') vars++; + if (*vars) { + char c = *vars; + *vars++ = 0; + vars_set(name, value); + if (c == '}') break; + } + } + + return STAT_OK; +} + + +void vars_print_help() { + static const char fmt[] PROGMEM = " $%-5s %-20S %-16S %S\n"; + + // Save and disable watchdog + uint8_t wd_state = hw_disable_watchdog(); + +#define VAR(NAME, CODE, TYPE, ...) \ + printf_P(fmt, #CODE, NAME##_name, TYPE##_name, NAME##_help); +#include "vars.def" +#undef VAR + + // Restore watchdog + hw_restore_watchdog(wd_state); +} + + +uint16_t vars_crc() { + // Save and disable watchdog + uint8_t wd_state = hw_disable_watchdog(); + + CRC.CTRL = CRC_RESET_RESET0_gc; + CRC.CTRL = CRC_SOURCE_IO_gc; // Must be after reset + +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ + IF(SAVE) \ + ({ \ + for (int j = 0; ; j++) { \ + char c = pgm_read_byte(&NAME##_name[j]); \ + if (!c) break; \ + CRC.DATAIN = c; \ + } \ + \ + CRC.DATAIN = INDEX; \ + }) + +#include "vars.def" +#undef VAR + + // Restore watchdog + hw_restore_watchdog(wd_state); + + CRC.STATUS = CRC_BUSY_bm; // Done + return CRC.CHECKSUM1 << 8 | CRC.CHECKSUM0; +} + + +void vars_save() { + // Save and disable watchdog + uint8_t wd_state = hw_disable_watchdog(); + +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ + IF(SAVE) \ + (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \ + TYPE value = get_##NAME(IF(INDEX)(i)); \ + eeprom_update_##TYPE(&NAME##_eeprom IF(INDEX)([i]), value); \ + }) \ + +#include "vars.def" +#undef VAR + + // Restore watchdog + hw_restore_watchdog(wd_state); + + // Save CRC + eeprom_update_word(&eeprom_crc, vars_crc()); +} + + + +bool vars_valid() { + return eeprom_read_word(&eeprom_crc) == vars_crc(); +} + + +stat_t vars_restore() { + if (!vars_valid()) return STAT_EEPROM_DATA_INVALID; + + // Save and disable watchdog + uint8_t wd_state = hw_disable_watchdog(); + +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ + IF(SAVE) \ + (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \ + TYPE value = eeprom_read_##TYPE(&NAME##_eeprom IF(INDEX)([i])); \ + set_##NAME(IF(INDEX)(i,) value); \ + NAME##_state IF(INDEX)([i]) = value; \ + }) \ + +#include "vars.def" +#undef VAR + + // Restore watchdog + hw_restore_watchdog(wd_state); + + return STAT_OK; +} + + +void vars_clear() { + eeprom_update_word(&eeprom_crc, -1); +} diff --git a/avr/src/vars.def b/avr/src/vars.def new file mode 100644 index 0000000..c7f9dee --- /dev/null +++ b/avr/src/vars.def @@ -0,0 +1,122 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 SWITCHES_LABEL "0123456789" + +// VAR(name, code, type, index, settable, save, help) + +// Motor +VAR(axis_name, an, char, MOTORS, 1, 1, "Maps motor to axis name") +VAR(step_angle, sa, float, MOTORS, 1, 1, "In degrees per full step") +VAR(travel, tr, float, MOTORS, 1, 1, "Travel in mm per revolution") +VAR(microstep, mi, uint16_t, MOTORS, 1, 1, "Microsteps per full step") +VAR(reverse, rv, uint8_t, MOTORS, 1, 1, "Reverse motor polarity") + +VAR(power_mode, pm, uint8_t, MOTORS, 1, 1, "Motor power mode") +VAR(drive_power, dp, float, MOTORS, 1, 1, "Motor drive power") +VAR(idle_power, ip, float, MOTORS, 1, 1, "Motor idle power") +VAR(current_power, cp, float, MOTORS, 0, 0, "Motor power now") +VAR(status_flags, mf, uint8_t, MOTORS, 0, 0, "Motor status flags") +VAR(status_strings, ms, flags_t, MOTORS, 0, 0, "Motor status strings") + +VAR(motor_fault, mf, bool, 0, 0, 0, "Motor fault status") + +VAR(velocity_max, vm, float, MOTORS, 1, 1, "Maxium velocity in mm/min") +VAR(feedrate_max, fr, float, MOTORS, 1, 1, "Maxium feedrate in mm/min") +VAR(jerk_max, jm, float, MOTORS, 1, 1, "Maxium jerk in mm/min^3") +VAR(junction_dev, jd, float, MOTORS, 1, 1, "Junction deviation") +VAR(radius, ra, float, MOTORS, 1, 1, "Axis radius or zero") + +// Homing +VAR(homing_mode, hm, uint8_t, MOTORS, 1, 1, "Homing type") +VAR(travel_min, tn, float, MOTORS, 1, 1, "Minimum soft limit") +VAR(travel_max, tm, float, MOTORS, 1, 1, "Maximum soft limit") +VAR(search_velocity,sv, float, MOTORS, 1, 1, "Homing search velocity") +VAR(latch_velocity, lv, float, MOTORS, 1, 1, "Homing latch velocity") +VAR(latch_backoff, lb, float, MOTORS, 1, 1, "Homing latch backof") +VAR(zero_backoff, zb, float, MOTORS, 1, 1, "Homing zero backof") + +// Axis +VAR(position, p, float, AXES, 0, 0, "Current axis position") + +// Spindle +VAR(spindle_type, st, uint8_t, 0, 1, 1, "PWM=0 or HUANYANG=1") +VAR(spin_reverse, sr, bool, 0, 1, 1, "Reverse spin") +VAR(max_spin, sx, float, 0, 1, 1, "Maximum spindle speed") +VAR(min_spin, sm, float, 0, 1, 1, "Minimum spindle speed") +VAR(spin_min_pulse, np, float, 0, 1, 1, "Minimum pulse width") +VAR(spin_max_pulse, mp, float, 0, 1, 1, "Maximum pulse width") +VAR(spin_up, su, float, 0, 1, 1, "Spin up velocity") +VAR(spin_down, sd, float, 0, 1, 1, "Spin down velocity") + +// Huanyang spindle +VAR(huanyang_id, hi, uint8_t, 0, 1, 1, "Huanyang ID") +VAR(huanyang_freq, hz, float, 0, 0, 0, "Huanyang actual freq") +VAR(huanyang_current, hc, float, 0, 0, 0, "Huanyang actual current") +VAR(huanyang_rpm, hr, uint16_t, 0, 0, 0, "Huanyang actual RPM") +//VAR(huanyang_dcv, hd, uint16_t, 0, 0, 0, "Huanyang DC voltage") +//VAR(huanyang_acv, ha, uint16_t, 0, 0, 0, "Huanyang AC voltage") +VAR(huanyang_temp, ht, uint16_t, 0, 0, 0, "Huanyang temperature") +VAR(huanyang_max_freq, hx, float, 0, 0, 0, "Huanyang max freq") +VAR(huanyang_min_freq, hm, float, 0, 0, 0, "Huanyang min freq") +VAR(huanyang_rated_rpm, hq, uint16_t, 0, 0, 0, "Huanyang rated RPM") +VAR(huanyang_status, hs, uint8_t, 0, 0, 0, "Huanyang status flags") +VAR(huanyang_debug, hb, bool, 0, 1, 0, "Huanyang debugging") +VAR(huanyang_connected, he, bool, 0, 0, 0, "Huanyang connected") + +// Switches +VAR(switch_type, sw, uint8_t, SWITCHES, 1, 1, "Normally open or closed") + +// GCode +VAR(line, ln, int32_t, 0, 0, 0, "Last GCode line executed") +VAR(unit, u, pstring, 0, 0, 0, "Current unit of measure") +VAR(speed, s, float, 0, 0, 0, "Current spindle speed") +VAR(feed, f, float, 0, 0, 0, "Current feed rate") +VAR(tool, t, uint8_t, 0, 0, 0, "Current tool") +VAR(feed_mode, fm, pstring, 0, 0, 0, "Current feed rate mode") +VAR(plane, pa, pstring, 0, 0, 0, "Current plane") +VAR(coord_system, cs, pstring, 0, 0, 0, "Current coordinate system") +VAR(abs_override, ao, bool, 0, 0, 0, "Absolute override enabled") +VAR(path_mode, pc, pstring, 0, 0, 0, "Current path control mode") +VAR(distance_mode, dm, pstring, 0, 0, 0, "Current distance mode") +VAR(arc_dist_mode, ad, pstring, 0, 0, 0, "Current arc distance mode") +VAR(mist_coolant, mc, bool, 0, 0, 0, "Mist coolant enabled") +VAR(flood_coolant, fc, bool, 0, 0, 0, "Flood coolant enabled") +VAR(feed_override, fo, float, 0, 0, 0, "Feed rate override") +VAR(speed_override, so, float, 0, 0, 0, "Spindle speed override") + +// System +VAR(velocity, v, float, 0, 0, 0, "Current velocity") +VAR(hw_id, id, string, 0, 0, 0, "Hardware ID") +VAR(echo, ec, bool, 0, 1, 0, "Enable or disable echo") +VAR(estop, es, bool, 0, 1, 0, "Emergency stop") +VAR(estop_reason, er, pstring, 0, 0, 0, "Emergency stop reason") +VAR(state, x, pstring, 0, 0, 0, "Machine state") +VAR(cycle, c, pstring, 0, 0, 0, "Machine cycle") +VAR(hold_reason, pr, pstring, 0, 0, 0, "Machine pause reason") diff --git a/avr/src/vars.h b/avr/src/vars.h new file mode 100644 index 0000000..e918e91 --- /dev/null +++ b/avr/src/vars.h @@ -0,0 +1,47 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public 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 vars_init(); + +void vars_report(bool full); +int vars_find(const char *name); +bool vars_print(const char *name); +bool vars_set(const char *name, const char *value); +int vars_parser(char *vars); +void vars_print_help(); + +void vars_save(); +bool vars_valid(); +stat_t vars_restore(); +void vars_clear(); diff --git a/avr/src/xboot/eeprom_driver.c b/avr/src/xboot/eeprom_driver.c new file mode 100644 index 0000000..38b6eea --- /dev/null +++ b/avr/src/xboot/eeprom_driver.c @@ -0,0 +1,58 @@ +/************************************************************************/ +/* XMEGA EEPROM Driver */ +/* */ +/* eeprom.c */ +/* */ +/* Alex Forencich */ +/* */ +/* Copyright (c) 2011 Alex Forencich */ +/* */ +/* Permission is hereby granted, free of charge, to any person */ +/* obtaining a copy of this software and associated documentation */ +/* files(the "Software"), to deal in the Software without restriction, */ +/* including without limitation the rights to use, copy, modify, merge, */ +/* publish, distribute, sublicense, and/or sell copies of the Software, */ +/* and to permit persons to whom the Software is furnished to do so, */ +/* subject to the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ +/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ +/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ +/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ +/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ +/* SOFTWARE. */ +/* */ +/************************************************************************/ + +#include "eeprom_driver.h" + + +static inline void NVM_EXEC(void) { + void *z = (void *)&NVM_CTRLA; + + __asm__ volatile("out %[ccp], %[ioreg]" "\n\t" + "st z, %[cmdex]" + : + : [ccp] "I" (_SFR_IO_ADDR(CCP)), + [ioreg] "d" (CCP_IOREG_gc), + [cmdex] "r" (NVM_CMDEX_bm), + [z] "z" (z) + ); +} + + +void wait_for_nvm() { + while (NVM.STATUS & NVM_NVMBUSY_bm) continue; +} + + +void EEPROM_erase_all() { + wait_for_nvm(); + NVM.CMD = NVM_CMD_ERASE_EEPROM_gc; + NVM_EXEC(); +} diff --git a/avr/src/xboot/eeprom_driver.h b/avr/src/xboot/eeprom_driver.h new file mode 100644 index 0000000..c139a43 --- /dev/null +++ b/avr/src/xboot/eeprom_driver.h @@ -0,0 +1,56 @@ +/************************************************************************/ +/* XMEGA EEPROM Driver */ +/* */ +/* eeprom.h */ +/* */ +/* Alex Forencich */ +/* */ +/* Copyright (c) 2011 Alex Forencich */ +/* */ +/* Permission is hereby granted, free of charge, to any person */ +/* obtaining a copy of this software and associated documentation */ +/* files(the "Software"), to deal in the Software without restriction, */ +/* including without limitation the rights to use, copy, modify, merge, */ +/* publish, distribute, sublicense, and/or sell copies of the Software, */ +/* and to permit persons to whom the Software is furnished to do so, */ +/* subject to the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ +/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ +/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ +/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ +/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ +/* SOFTWARE. */ +/* */ +/************************************************************************/ + +#pragma once + +#include +#include +#include + +#include "xboot.h" + +#ifndef EEPROM_PAGE_SIZE +#define EEPROM_PAGE_SIZE E2PAGESIZE +#endif + +#define EEPROM_read_byte(addr) \ + eeprom_read_byte((const uint8_t *)((uint16_t)(addr))) + +#define EEPROM_write_byte(addr, value) \ + eeprom_write_byte((uint8_t *)((uint16_t)(addr)), (value)) + +#define EEPROM_read_block(addr, dest, len) \ + eeprom_read_block((dest), (void *)((uint16_t)(addr)), (len)) + +#define EEPROM_write_block(addr, src, len) \ + eeprom_write_block((src), (void *)((uint16_t)(addr)), (len)) + +void EEPROM_erase_all(); diff --git a/avr/src/xboot/protocol.h b/avr/src/xboot/protocol.h new file mode 100644 index 0000000..4336094 --- /dev/null +++ b/avr/src/xboot/protocol.h @@ -0,0 +1,102 @@ +/************************************************************************/ +/* XBoot Extensible AVR Bootloader */ +/* */ +/* XBoot Protocol Definition */ +/* */ +/* protocol.h */ +/* */ +/* Alex Forencich */ +/* */ +/* Copyright (c) 2010 Alex Forencich */ +/* */ +/* Permission is hereby granted, free of charge, to any person */ +/* obtaining a copy of this software and associated documentation */ +/* files(the "Software"), to deal in the Software without restriction, */ +/* including without limitation the rights to use, copy, modify, merge, */ +/* publish, distribute, sublicense, and/or sell copies of the Software, */ +/* and to permit persons to whom the Software is furnished to do so, */ +/* subject to the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ +/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ +/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ +/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ +/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ +/* SOFTWARE. */ +/* */ +/************************************************************************/ + +#pragma once + +#include "xboot.h" + +// General Commands +#define CMD_SYNC '\x1b' + +// Informational Commands +#define CMD_CHECK_AUTOINCREMENT 'a' +#define CMD_CHECK_BLOCK_SUPPORT 'b' +#define CMD_PROGRAMMER_TYPE 'p' +#define CMD_DEVICE_CODE 't' +#define CMD_PROGRAM_ID 'S' +#define CMD_VERSION 'V' +#define CMD_READ_SIGNATURE 's' + +// Addressing +#define CMD_SET_ADDRESS 'A' +#define CMD_SET_EXT_ADDRESS 'H' + +// Erase +#define CMD_CHIP_ERASE 'e' + +// Block Access +#define CMD_BLOCK_LOAD 'B' +#define CMD_BLOCK_READ 'g' + +// Byte Access +#define CMD_READ_BYTE 'R' +#define CMD_WRITE_LOW_BYTE 'c' +#define CMD_WRITE_HIGH_BYTE 'C' +#define CMD_WRITE_PAGE 'm' +#define CMD_WRITE_EEPROM_BYTE 'D' +#define CMD_READ_EEPROM_BYTE 'd' + +// Lock and Fuse Bits +#define CMD_WRITE_LOCK_BITS 'l' +#define CMD_READ_LOCK_BITS 'r' +#define CMD_READ_LOW_FUSE_BITS 'F' +#define CMD_READ_HIGH_FUSE_BITS 'N' +#define CMD_READ_EXT_FUSE_BITS 'Q' + +// Bootloader Commands +#define CMD_ENTER_PROG_MODE 'P' +#define CMD_LEAVE_PROG_MODE 'L' +#define CMD_EXIT_BOOTLOADER 'E' +#define CMD_SET_LED 'x' +#define CMD_CLEAR_LED 'y' +#define CMD_SET_TYPE 'T' + +#define CMD_CRC 'h' + +// Memory types for block access +#define MEM_EEPROM 'E' +#define MEM_FLASH 'F' +#define MEM_USERSIG 'U' +#define MEM_PRODSIG 'P' + +// Sections for CRC checks +#define SECTION_FLASH 'F' +#define SECTION_APPLICATION 'A' +#define SECTION_BOOT 'B' +#define SECTION_APP 'a' +#define SECTION_APP_TEMP 't' + +// Command Responses +#define REPLY_ACK '\r' +#define REPLY_YES 'Y' +#define REPLY_ERROR '?' diff --git a/avr/src/xboot/sp_driver.S b/avr/src/xboot/sp_driver.S new file mode 100644 index 0000000..e6fff99 --- /dev/null +++ b/avr/src/xboot/sp_driver.S @@ -0,0 +1,794 @@ +;****************************************************************************** +;* +;* XMEGA Self-programming driver assembly source file. +;* +;* This file contains the low-level implementations for the +;* XMEGA Self-programming driver. It is written for the GCC Assembler. +;* +;* If any SPM instructions are used, the linker file must define +;* a segment named bootloader which must be located in the device Boot section. +;* This can be done by passing "-Wl,--section-start=.BOOT=0x020000" to the +;* linker with the correct address for the boot section. +;* +;* None of these routines clean up the NVM Command Register after use. +;* It is therefore important to write NVM_CMD_NO_OPERATION_gc (0x00) to this +;* register when you are finished using any of the functions in this driver. +;* +;* For all routines, it is important that any interrupt handlers do not +;* perform any NVM operations. The user must implement a scheme for mutually +;* exclusive access to the NVM. However, the 4-cycle timeout will work fine, +;* since writing to the Configuration Change Protection register (CCP) +;* automatically disables interrupts for 4 instruction cycles. +;* +;* Note on IAR calling convention: +;* Scratch registers: R18-R27, R30-R31 +;* Preserved registers: R2-R17, R28-R29 +;* Parameter registers: R8-R25 (2-,4-, or 8- byte alignment) +;* Return registers: R18-R25 (up to 64-bit) +;* +;* Application note: +;* AVR1316: XMEGA Self-programming +;* +;* Documentation +;* For comprehensive code documentation, supported compilers, compiler +;* settings and supported devices see readme.html +;* +;* Atmel Corporation: http:;www.atmel.com \n +;* Support email: avr@atmel.com +;* +;* $Revision: 1153 $ +;* $Date: 2007-12-18 09:48:23 +0100 (ti, 18 des 2007) $ +;* +;* Copyright (c) 2007, Atmel Corporation All rights reserved. +;* +;* Redistribution and use in source and binary forms, with or without +;* modification, are permitted provided that the following conditions are met: +;* +;* 1. Redistributions of source code must retain the above copyright notice, +;* this list of conditions and the following disclaimer. +;* +;* 2. Redistributions in binary form must reproduce the above copyright notice, +;* this list of conditions and the following disclaimer in the documentation +;* and/or other materials provided with the distribution. +;* +;* 3. The name of ATMEL may not be used to endorse or promote products derived +;* from this software without specific prior written permission. +;* +;* THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED +;* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +;* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND +;* SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, +;* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +;* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +;* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +;* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +;* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +;* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +;****************************************************************************** + +#include +;#include "Flash_Defines.h" + +/* Define the size of the flash page if not defined in the header files. */ +#ifndef APP_SECTION_PAGE_SIZE + #error APP_SECTION_PAGE_SIZE must be defined if not defined in header files. + //#define APP_SECTION_PAGE_SIZE 512 +#endif /*APP_SECTION_PAGE_SIZE*/ + +/* Defines not yet included in header file. */ +#define NVM_CMD_NO_OPERATION_gc (0x00<<0) // Noop/Ordinary LPM +#define NVM_CMD_READ_USER_SIG_ROW_gc (0x01<<0) // Read user signature row +#define NVM_CMD_READ_CALIB_ROW_gc (0x02<<0) // Read calibration row +#define NVM_CMD_READ_EEPROM_gc (0x06<<0) // Read EEPROM +#define NVM_CMD_READ_FUSES_gc (0x07<<0) // Read fuse byte +#define NVM_CMD_WRITE_LOCK_BITS_gc (0x08<<0) // Write lock bits +#define NVM_CMD_ERASE_USER_SIG_ROW_gc (0x18<<0) // Erase user signature row +#define NVM_CMD_WRITE_USER_SIG_ROW_gc (0x1A<<0) // Write user signature row +#define NVM_CMD_ERASE_APP_gc (0x20<<0) // Erase Application Section +#define NVM_CMD_ERASE_APP_PAGE_gc (0x22<<0) // Erase Application Section page +#define NVM_CMD_LOAD_FLASH_BUFFER_gc (0x23<<0) // Load Flash page buffer +#define NVM_CMD_WRITE_APP_PAGE_gc (0x24<<0) // Write Application Section page +#define NVM_CMD_ERASE_WRITE_APP_PAGE_gc (0x25<<0) // Erase-and-write Application Section page +#define NVM_CMD_ERASE_FLASH_BUFFER_gc (0x26<<0) // Erase/flush Flash page buffer +#define NVM_CMD_ERASE_BOOT_PAGE_gc (0x2A<<0) // Erase Boot Section page +#define NVM_CMD_WRITE_BOOT_PAGE_gc (0x2C<<0) // Write Boot Section page +#define NVM_CMD_ERASE_WRITE_BOOT_PAGE_gc (0x2D<<0) // Erase-and-write Boot Section page +#define NVM_CMD_ERASE_EEPROM_gc (0x30<<0) // Erase EEPROM +#define NVM_CMD_ERASE_EEPROM_PAGE_gc (0x32<<0) // Erase EEPROM page +#define NVM_CMD_LOAD_EEPROM_BUFFER_gc (0x33<<0) // Load EEPROM page buffer +#define NVM_CMD_WRITE_EEPROM_PAGE_gc (0x34<<0) // Write EEPROM page +#define NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc (0x35<<0) // Erase-and-write EEPROM page +#define NVM_CMD_ERASE_EEPROM_BUFFER_gc (0x36<<0) // Erase/flush EEPROM page buffer +#define NVM_CMD_APP_CRC_gc (0x38<<0) // Generate Application section CRC +#define NVM_CMD_BOOT_CRC_gc (0x39<<0) // Generate Boot Section CRC +#define NVM_CMD_FLASH_RANGE_CRC_gc (0x3A<<0) // Generate Flash Range CRC +#define CCP_SPM_gc (0x9D<<0) // SPM Instruction Protection +#define CCP_IOREG_gc (0xD8<<0) // IO Register Protection + + + +; --- +; This routine reads a byte from flash given by the address in +; R25:R24:R23:R22. +; +; Input: +; R25:R24:R23:R22. +; +; Returns: +; R24 - Read byte. +; --- + +.section .text +.global SP_ReadByte + +SP_ReadByte: + in r19, RAMPZ ; Save RAMPZ. + out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. + movw ZL, r22 ; Move the low bytes to the Z pointer + elpm r24, Z ; Extended load byte from address pointed to by Z. + out RAMPZ, r19 ; Restore RAMPZ register. + ret + + + +; --- +; This routine reads a word from flash given by the address in +; R25:R24:R23:R22. +; +; Input: +; R25:R24:R23:R22. +; +; Returns: +; R25:R24 - Read word. +; --- + +.section .text +.global SP_ReadWord + +SP_ReadWord: + in r19, RAMPZ ; Save RAMPZ. + out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. + movw ZL, r22 ; Move the low bytes to the Z pointer + elpm r24, Z+ ; Extended load byte from address pointed to by Z. + elpm r25, Z ; Extended load byte from address pointed to by Z. + out RAMPZ, r19 ; Restore RAMPZ register. + ret + + + +; --- +; This routine reads the calibration byte given by the index in R24. +; +; Input: +; R24 - Byte index. +; +; Returns: +; R24 - Calibration byte. +; --- + +.section .text +.global SP_ReadCalibrationByte + +SP_ReadCalibrationByte: + ldi r20, NVM_CMD_READ_CALIB_ROW_gc ; Prepare NVM command in R20. + rjmp SP_CommonLPM ; Jump to common LPM code. + + + +; --- +; This routine reads the user signature byte given by the index in R25:R24. +; +; Input: +; R25:R24 - Byte index. +; +; Returns: +; R24 - Signature byte. +; --- + +.section .text +.global SP_ReadUserSignatureByte + +SP_ReadUserSignatureByte: + ldi r20, NVM_CMD_READ_USER_SIG_ROW_gc ; Prepare NVM command in R20. + rjmp SP_CommonLPM ; Jump to common LPM code. + + + +; --- +; This routine reads the fuse byte given by the index in R24. +; +; Input: +; R24 - Byte index. +; +; Returns: +; R24 - Fuse byte. +; --- + +.section .text +.global SP_ReadFuseByte + +SP_ReadFuseByte: + sts NVM_ADDR0, r24 ; Load fuse byte index into NVM Address Register 0. + clr r24 ; Prepare a zero. + sts NVM_ADDR1, r24 ; Load zero into NVM Address Register 1. + sts NVM_ADDR2, r24 ; Load zero into NVM Address Register 2. + ldi r20, NVM_CMD_READ_FUSES_gc ; Prepare NVM command in R20. + rcall SP_CommonCMD ; Jump to common NVM Action code. + movw r24, r22 ; Move low byte to 1 byte return address. + ret + + + +; --- +; This routine sets the lock bits from R24. Note that unlocking is only +; possible by doing a full chip erase, not available from software. +; +; Input: +; R24 - Lock bits. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_WriteLockBits + +SP_WriteLockBits: + sts NVM_DATA0, r24 ; Load lock bits into NVM Data Register 0. + ldi r20, NVM_CMD_WRITE_LOCK_BITS_gc ; Prepare NVM command in R20. + rjmp SP_CommonCMD ; Jump to common NVM Action code. + + + +; --- +; This routine reads the lock bits. +; +; Input: +; Nothing. +; +; Returns: +; R24 - Lock bits. +; --- + +.section .text +.global SP_ReadLockBits + +SP_ReadLockBits: + lds r24, NVM_LOCKBITS ; Read IO-mapped lock bits. + ret + + + +; --- +; This routine erases the user signature row. +; +; Input: +; Nothing. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_EraseUserSignatureRow + +SP_EraseUserSignatureRow: + in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. + ldi r20, NVM_CMD_ERASE_USER_SIG_ROW_gc ; Prepare NVM command in R20. + jmp SP_CommonSPM ; Jump to common SPM code. + + + +; --- +; This routine writes the flash buffer to the user signature row. +; +; Input: +; Nothing. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_WriteUserSignatureRow + +SP_WriteUserSignatureRow: + in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. + ldi r20, NVM_CMD_WRITE_USER_SIG_ROW_gc ; Prepare NVM command in R20. + jmp SP_CommonSPM ; Jump to common SPM code. + + + +; --- +; This routine erases the entire application section. +; +; Input: +; Nothing. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_EraseApplicationSection + +SP_EraseApplicationSection: + in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. + clr r24 ; Prepare a zero. + clr r25 + out RAMPZ, r24 ; Point into Application area. + ldi r20, NVM_CMD_ERASE_APP_gc ; Prepare NVM command in R20. + jmp SP_CommonSPM ; Jump to common SPM code. + + + +; --- +; This routine erases the page at address R25:R24:R23:R22 in the application +; section. The address can point anywhere inside the page. +; +; Input: +; R25:R24:R23:R22 - Byte address into Flash page. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_EraseApplicationPage + +SP_EraseApplicationPage: + in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. + out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. + movw r24, r22 ; Move low bytes for ZH:ZL to R25:R24 + ldi r20, NVM_CMD_ERASE_APP_PAGE_gc ; Prepare NVM command in R20. + jmp SP_CommonSPM ; Jump to common SPM code. + + + +; --- +; This routine writes the word from R23:R22 into the Flash page buffer at +; address R25:R24. +; +; Input: +; R25:R24 - Byte address into Flash page. +; R23:R22 - Word to write. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_LoadFlashWord + +SP_LoadFlashWord: + in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. + movw r0, r22 ; Prepare flash word in R1:R0. + ldi r20, NVM_CMD_LOAD_FLASH_BUFFER_gc ; Prepare NVM command in R20. + jmp SP_CommonSPM ; Jump to common SPM code. + + + +; --- +; This routine writes an entire page from the SRAM buffer at +; address R25:R24 into the Flash page buffer. +; +; Note that you must define "-Wl,--section-start=.BOOT=0x020000" for the +; linker to place this function in the boot section with the correct address. +; +; Input: +; R25:R24 - 16-bit pointer to SRAM buffer. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_LoadFlashPage + +SP_LoadFlashPage: + clr ZL ; Clear low byte of Z, to indicate start of page. + clr ZH ; Clear high byte of Z, to indicate start of page. + + out RAMPX, r1 ; Clear RAMPX pointer. + movw XL, r24 ; Load X with data buffer address. + + ldi r20, NVM_CMD_LOAD_FLASH_BUFFER_gc ; Prepare NVM command code in R20. + sts NVM_CMD, r20 ; Load it into NVM command register. + +#if APP_SECTION_PAGE_SIZE > 512 + ldi r22, ((APP_SECTION_PAGE_SIZE/2) >> 8) +#endif + + ldi r21, ((APP_SECTION_PAGE_SIZE/2)&0xFF) ; Load R21 with page word count. + ldi r18, CCP_SPM_gc ; Prepare Protect SPM signature in R16. + +SP_LoadFlashPage_1: + ld r0, X+ ; Load low byte from buffer into R0. + ld r1, X+ ; Load high byte from buffer into R1. + sts CCP, r18 ; Enable SPM operation (this disables interrupts for 4 cycles). + spm ; Self-program. + adiw ZL, 2 ; Move Z to next Flash word. + +#if APP_SECTION_PAGE_SIZE > 512 + subi r21, 1 ; Decrement word count. + sbci r22, 0 +#else + dec r21 ; Decrement word count. +#endif + + brne SP_LoadFlashPage_1 ; Repeat until word cont is zero. + + clr r1 ; Clear R1 for GCC _zero_reg_ to function properly. + ret + + + +; --- +; This routine reads an entire Flash page from address R23:R22:R21:R20 into the +; SRAM buffer at address R25:R24. +; +; +; Input: +; R23:R22:R21:R20 - Flash byte address. +; R25:R24 - 16-bit pointer to SRAM buffer. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_ReadFlashPage + +SP_ReadFlashPage: + + in r19, RAMPZ ; Save RAMPZ during assembly. + out RAMPZ, r22 ; Load RAMPZ with MSB of address + movw ZL, r20 ; Load Z with Flash address. + + out RAMPX, r1 ; Load RAMPX with data pointer + movw XL, r24 ; Load X with data buffer address. + + ldi r20, NVM_CMD_NO_OPERATION_gc ; Prepare NVM command code in R20. + sts NVM_CMD, r20 ; Set NVM command to No Operation so that LPM reads Flash. + +#if APP_SECTION_PAGE_SIZE > 512 + ldi r22, ((APP_SECTION_PAGE_SIZE/2) >> 8) ; Load R22 with byte cont high if flash page is large. +#endif + + ldi r21, ((APP_SECTION_PAGE_SIZE)&0xFF) ; Load R21 with byte count. + +SP_ReadFlashPage_1: + elpm r24, Z+ ; Load Flash bytes into R18:r19 + elpm r25, Z+ + st X+, r24 ; Write bytes to buffer. + st X+, r25 + +#if APP_SECTION_PAGE_SIZE > 512 + subi r21, 1 ; Decrement word count. + sbci r22, 0 +#else + dec r21 ; Decrement word count. +#endif + + brne SP_ReadFlashPage_1 ; Repeat until byte count is zero. + + out RAMPZ, r19 + ret + + + +; --- +; This routine writes the page buffer to the Flash page at address R25:R24:R23:R22 +; in the application section. The address can point anywhere inside the page. +; +; Input: +; R25:R24:R23:R22 - Byte address into Flash page. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_WriteApplicationPage + +SP_WriteApplicationPage: + in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. + out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. + movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22 + ldi r20, NVM_CMD_WRITE_APP_PAGE_gc ; Prepare NVM command in R20. + jmp SP_CommonSPM ; Jump to common SPM code. + + + +; --- +; This routine erases first and then writes the page buffer to the +; Flash page at address R25:R24:R23:R22 in the application section. The address +; can point anywhere inside the page. +; +; Input: +; R25:R24:R23:R22 - Byte address into Flash page. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_EraseWriteApplicationPage + +SP_EraseWriteApplicationPage: + in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. + out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. + movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22 + ldi r20, NVM_CMD_ERASE_WRITE_APP_PAGE_gc ; Prepare NVM command in R20. + jmp SP_CommonSPM ; Jump to common SPM code. + + + +; --- +; This routine flushes the Flash page buffer. +; +; Input: +; Nothing. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_EraseFlashBuffer + +SP_EraseFlashBuffer: + in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. + ldi r20, NVM_CMD_ERASE_FLASH_BUFFER_gc ; Prepare NVM command in R20. + jmp SP_CommonSPM ; Jump to common SPM code. + + + +; --- +; This routine erases the page at address R25:R24:R23:R22 in the Boot section. The +; address can point anywhere inside the page. +; +; Input: +; R25:R24:R23:R22 - Byte address into Flash page. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_EraseBootPage + +SP_EraseBootPage: + in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. + out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. + movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22 + ldi r20, NVM_CMD_ERASE_BOOT_PAGE_gc ; Prepare NVM command in R20. + jmp SP_CommonSPM ; Jump to common SPM code. + + + +; --- +; This routine writes the page buffer to the Flash page at address R25:R24:R23:R22 +; in the BOOT section. The address can point anywhere inside the page. +; +; Input: +; R25:R24:R23:R22 - Byte address into Flash page. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_WriteBootPage + +SP_WriteBootPage: + in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. + out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. + movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22 + ldi r20, NVM_CMD_WRITE_BOOT_PAGE_gc ; Prepare NVM command in R20. + jmp SP_CommonSPM ; Jump to common SPM code. + + + +; --- +; This routine erases first and then writes the page buffer to the +; Flash page at address R25:R24:R23:R22 in the Boot section. The address +; can point anywhere inside the page. +; +; Input: +; R25:R24:R23:R22 - Byte address into Flash page. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_EraseWriteBootPage + +SP_EraseWriteBootPage: + in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. + out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. + movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22 + ldi r20, NVM_CMD_ERASE_WRITE_BOOT_PAGE_gc ; Prepare NVM command in R20. + jmp SP_CommonSPM ; Jump to common SPM code. + + + +; --- +; This routine calculates a CRC for the application section. +; +; Input: +; Nothing. +; +; Returns: +; R25:R24:R23:R22 - 32-bit CRC result (actually only 24-bit used). +; --- + +.section .text +.global SP_ApplicationCRC + +SP_ApplicationCRC: + ldi r20, NVM_CMD_APP_CRC_gc ; Prepare NVM command in R20. + rjmp SP_CommonCMD ; Jump to common NVM Action code. + + + +; --- +; This routine calculates a CRC for the Boot section. +; +; Input: +; Nothing. +; +; Returns: +; R25:R24:R23:R22 - 32-bit CRC result (actually only 24-bit used). +; --- + +.section .text +.global SP_BootCRC + +SP_BootCRC: + ldi r20, NVM_CMD_BOOT_CRC_gc ; Prepare NVM command in R20. + rjmp SP_CommonCMD ; Jump to common NVM Action code. + + + +; --- +; This routine locks all further access to SPM operations until next reset. +; +; Input: +; Nothing. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_LockSPM + +SP_LockSPM: + ldi r18, CCP_IOREG_gc ; Prepare Protect IO-register signature in R18. + sts CCP, r18 ; Enable IO-register operation (this disables interrupts for 4 cycles). + ldi r18, NVM_SPMLOCK_bm ; Prepare bitmask for locking SPM into R18. + sts NVM_CTRLB, r18 ; Load bitmask into NVM Control Register B, which locks SPM. + ret + + + +; --- +; This routine wait for the SPM to finish and clears the command register. +; +; Note that this routine is blocking, and will halt any execution until the SPM +; is finished. +; +; Input: +; Nothing. +; +; Returns: +; Nothing. +; --- + +.section .text +.global SP_WaitForSPM + +SP_WaitForSPM: + lds r18, NVM_STATUS ; Load the NVM Status register. + sbrc r18, NVM_NVMBUSY_bp ; Check if bit is cleared. + rjmp SP_WaitForSPM ; Repeat check if bit is not cleared. + clr r18 + sts NVM_CMD, r18 ; Clear up command register to NO_OPERATION. + ret + + + +; --- +; This routine is called by several other routines, and contains common code +; for executing an NVM command, including the return statement itself. +; +; If the operation (NVM command) requires the NVM Address registers to be +; prepared, this must be done before jumping to this routine. +; +; Note that R25:R24:R23:R22 is used for returning results, even if the +; C-domain calling function only expects a single byte or even void. +; +; Input: +; R20 - NVM Command code. +; +; Returns: +; R25:R24:R23:R22 - 32-bit result from NVM operation. +; --- + +.section .text + +SP_CommonCMD: + sts NVM_CMD, r20 ; Load command into NVM Command register. + ldi r18, CCP_IOREG_gc ; Prepare Protect IO-register signature in R18. + ldi r19, NVM_CMDEX_bm ; Prepare bitmask for setting NVM Command Execute bit into R19. + sts CCP, r18 ; Enable IO-register operation (this disables interrupts for 4 cycles). + sts NVM_CTRLA, r19 ; Load bitmask into NVM Control Register A, which executes the command. + lds r22, NVM_DATA0 ; Load NVM Data Register 0 into R22. + lds r23, NVM_DATA1 ; Load NVM Data Register 1 into R23. + lds r24, NVM_DATA2 ; Load NVM Data Register 2 into R24. + clr r25 ; Clear R25 in order to return a clean 32-bit value. + ret + + + +; --- +; This routine is called by several other routines, and contains common code +; for executing an LPM command, including the return statement itself. +; +; Note that R24 is used for returning results, even if the +; C-domain calling function expects a void. +; +; Input: +; R25:R24 - Low bytes of Z pointer. +; R20 - NVM Command code. +; +; Returns: +; R24 - Result from LPM operation. +; --- + +.section .text + +SP_CommonLPM: + movw ZL, r24 ; Load index into Z. + sts NVM_CMD, r20 ; Load prepared command into NVM Command register. + lpm r24,Z + ret + + + +; --- +; This routine is called by several other routines, and contains common code +; for executing an SPM command, including the return statement itself. +; +; If the operation (SPM command) requires the R1:R0 registers to be +; prepared, this must be done before jumping to this routine. +; +; Note that you must define "-Wl,--section-start=.BOOT=0x020000" for the +; linker to place this function in the boot section with the correct address. +; +; Input: +; R1:R0 - Optional input to SPM command. +; R25:R24 - Low bytes of Z pointer. +; R20 - NVM Command code. +; +; Returns: +; Nothing. +; --- + +.section .text + +SP_CommonSPM: + movw ZL, r24 ; Load R25:R24 into Z. + sts NVM_CMD, r20 ; Load prepared command into NVM Command register. + ldi r18, CCP_SPM_gc ; Prepare Protect SPM signature in R18 + sts CCP, r18 ; Enable SPM operation (this disables interrupts for 4 cycles). + spm ; Self-program. + clr r1 ; Clear R1 for GCC _zero_reg_ to function properly. + out RAMPZ, r19 ; Restore RAMPZ register. + ret + + +; END OF FILE + diff --git a/avr/src/xboot/sp_driver.h b/avr/src/xboot/sp_driver.h new file mode 100644 index 0000000..0bcbce9 --- /dev/null +++ b/avr/src/xboot/sp_driver.h @@ -0,0 +1,298 @@ +/* This file has been prepared for Doxygen automatic documentation generation.*/ +/*! \file ********************************************************************* + * + * \brief XMEGA Self-programming driver header file. + * + * This file contains the function prototypes for the + * XMEGA Self-programming driver. + * If any SPM instructions are used, the linker file must define + * a segment named BOOT which must be located in the device boot section. + * + * + * None of these functions clean up the NVM Command Register after use. + * It is therefore important to write NVMCMD_NO_OPERATION (0x00) to this + * register when you are finished using any of the functions in this + * driver. + * + * For all functions, it is important that no interrupt handlers perform + * any NVM operations. The user must implement a scheme for mutually + * exclusive access to the NVM. However, the 4-cycle timeout will work + * fine, since writing to the Configuration Change Protection register + * (CCP) automatically disables interrupts for 4 instruction cycles. + * + * \par Application note: + * AVR1316: XMEGA Self-programming + * + * \par Documentation + * For comprehensive code documentation, supported compilers, compiler + * settings and supported devices see readme.html + * + * \author + * Atmel Corporation: http://www.atmel.com \n + * Support email: avr@atmel.com + * + * $Revision: 1691 $ + * $Date: 2008-07-29 13:25:40 +0200 (ti, 29 jul 2008) $ \n + * + * Copyright (c) 2008, Atmel Corporation All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of ATMEL may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND + * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, + * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *****************************************************************************/ +#ifndef SP_DRIVER_H +#define SP_DRIVER_H + +//#include "avr_compiler.h" +//#include "Flash_Defines.h" + + + +/* Define the size of the flash page if not defined in the header files. */ +#ifndef APP_SECTION_PAGE_SIZE +#error APP_SECTION_PAGE_SIZE must be defined if not defined in header files. +//#define APP_SECTION_PAGE_SIZE 512 +#endif /*FLASH_PAGE_SIZE*/ + +// Define the Start of the application table if not defined in the header files. +#ifndef APPTABLE_SECTION_START +#error APPTABLE_SECTION_START must be defined if not defined in header files. +//#define APPTABLE_SECTION_START 0x01E000 //APPTABLE address for ATxmega128A1 +#endif /*APPTABLE_SECTION_START*/ + +/*! \brief Read a byte from flash. + * + * This function reads one byte from the flash. + * + * \note Both IAR and GCC have functions to do this, but + * we include the fucntions for easier use. + * + * \param address Address to the location of the byte to read. + * + * \retval Byte read from flash. + */ +uint8_t SP_ReadByte(uint32_t address); + +/*! \brief Read a word from flash. + * + * This function reads one word from the flash. + * + * \note Both IAR and GCC have functions to do this automatically, but + * we include the fucntions for easier use. + * + * \param address Address to the location of the word to read. + * + * \retval word read from flash. + */ +uint16_t SP_ReadWord(uint32_t address); + +/*! \brief Read calibration byte at given index. + * + * This function reads one calibration byte from the Calibration signature row. + * + * \param index Index of the byte in the calibration signature row. + * + * \retval Calibration byte + */ +uint8_t SP_ReadCalibrationByte(uint8_t index); + +/*! \brief Read fuse byte from given index. + * + * This function reads the fuse byte at the given index. + * + * \param index Index of the fuse byte. + * + * \retval Fuse byte + */ +uint8_t SP_ReadFuseByte(uint8_t index); + +/*! \brief Write lock bits. + * + * This function changes the lock bits. + * + * \note It is only possible to change the lock bits to a higher security l + * evel. + * + * \param data The new value of the lock bits. + */ +void SP_WriteLockBits(uint8_t data); + +/*! \brief Read lock bits. + * + * This function reads the lock bits. + * + * \retval Lock bits + */ +uint8_t SP_ReadLockBits(void); + +/*! \brief Read user signature at given index. + * + * This function reads one byte from the user signature row. + * + * \param index Index of the byte in the user signature row. + * + * \retval User signature byte + */ +uint8_t SP_ReadUserSignatureByte(uint16_t index); + +/*! \brief Erase user signature row. + * + * This function erase the entire user signature row. + */ +void SP_EraseUserSignatureRow(void); + +/*! \brief Write user signature row. + * + * This function write the flash buffer in the user signature row. + */ +void SP_WriteUserSignatureRow(void); + +/*! \brief Erase entire application section. + * + * This function erases the entire application and application table section + * + * \note If the lock bits is set to not allow spm in the application or + * application table section the erase is not done. + */ +void SP_EraseApplicationSection(void); + +/*! \brief Erase page at byte address in application or application table + * section. + * + * This function erase one page given by the byte address. + * + * \param address Byte address for flash page. + */ +void SP_EraseApplicationPage(uint32_t address); + +/*! \brief Erase and write page buffer to application or application table + * section at byte address. + * + * This function does a combined erase and write to a flash page in the + * application or application table section. + * + * \param address Byte address for flash page. + */ +void SP_EraseWriteApplicationPage(uint32_t address); + +/*! \brief Write page buffer to application or application table section at + * byte address. + * + * This function writes the Flash page buffer to a page in the application or + * application table section given by the byte address. + * + * \note The page that is written to must be erased before it is written to. + * + * \param address Byte address for flash page. + */ +void SP_WriteApplicationPage(uint32_t address); + +/*! \brief Load one word into Flash page buffer. + * + * This function Loads one word into the Flash page buffer. + * + * \param address Position in inside the flash page buffer. + * \param data Value to be put into the buffer. + */ +void SP_LoadFlashWord(uint16_t address, uint16_t data); + +/*! \brief Load entire page from SRAM buffer into Flash page buffer. + * + * This function load an entire page from SRAM. + * + * \param data Pointer to the data to put in buffer. + * + * \note The __near keyword limits the pointer to two bytes which means that + * only data up to 64K (internal SRAM) can be used. + */ +void SP_LoadFlashPage(const uint8_t * data); + +/*! \brief Read entire Flash page into SRAM buffer. + * + * This function reads an entire flash page and puts it to SRAM. + * + * \param data Pointer to where to store the data. + * \param address Address to page to read from flash. + */ +void SP_ReadFlashPage(const uint8_t * data, uint32_t address); + +/*! \brief Flush Flash page buffer. + * + * This function flush the Flash page buffer. + */ +void SP_EraseFlashBuffer(void); + +/*! \brief Erase page at byte address in boot section. + * + * This function erase one page given by the byte address. + * + * \param address Byte address for flash page. + */ +void SP_EraseBootPage(uint32_t address); + +/*! \brief Erase and write page buffer to boot section at byte address. + * + * This function does a combined erase and write to a flash page in the boot + * section. + * + * \param address Byte address for flash page. + */ +void SP_EraseWriteBootPage(uint32_t address); + +/*! \brief Write page buffer to boot section at byte address. + * + * This function writes the Flash page buffer to a page in the boot section + * given by the byte address. + * + * \note The page that is written to must be erased before it is written to. + * + * \param address Byte address for flash page. + */ +void SP_WriteBootPage(uint32_t address); + +/*! \brief Generate CRC from application section. + * + * \retval 24-bit CRC value + */ +uint32_t SP_ApplicationCRC(void); + +/*! \brief Generate CRC from boot section. + * + * \retval 24-bit CRC value + */ +uint32_t SP_BootCRC(void); + +/*! \brief Lock SPM instruction. + * + * This function locks the SPM instruction, and will disable the use of + * SPM until the next reset occurs. + */ +void SP_LockSPM(void); + +/*! \brief Wait for SPM to finish. + * + * This routine waits for the SPM to finish and clears the command register. + */ +void SP_WaitForSPM(void); + +#endif /* SP_DRIVER_H */ diff --git a/avr/src/xboot/uart.c b/avr/src/xboot/uart.c new file mode 100644 index 0000000..c4cf091 --- /dev/null +++ b/avr/src/xboot/uart.c @@ -0,0 +1,72 @@ +/************************************************************************/ +/* XBoot Extensible AVR Bootloader */ +/* */ +/* UART Module */ +/* */ +/* uart.c */ +/* */ +/* Alex Forencich */ +/* */ +/* Copyright (c) 2010 Alex Forencich */ +/* */ +/* Permission is hereby granted, free of charge, to any person */ +/* obtaining a copy of this software and associated documentation */ +/* files(the "Software"), to deal in the Software without restriction, */ +/* including without limitation the rights to use, copy, modify, merge, */ +/* publish, distribute, sublicense, and/or sell copies of the Software, */ +/* and to permit persons to whom the Software is furnished to do so, */ +/* subject to the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ +/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ +/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ +/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ +/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ +/* SOFTWARE. */ +/* */ +/************************************************************************/ + +#include "uart.h" +#include "xboot.h" + + +bool uart_has_char() {return UART_DEVICE.STATUS & USART_RXCIF_bm;} +uint8_t uart_recv_char() {return UART_DEVICE.DATA;} + + +void uart_send_char_blocking(uint8_t c) { + UART_DEVICE.DATA = c; + while (!(UART_DEVICE.STATUS & USART_TXCIF_bm)) continue; + UART_DEVICE.STATUS |= USART_TXCIF_bm; +} + + +void uart_init() { + UART_PORT.DIRSET = 1 << UART_TX_PIN; + UART_DEVICE.BAUDCTRLA = UART_BSEL_VALUE & USART_BSEL_gm; + UART_DEVICE.BAUDCTRLB = + ((UART_BSCALE_VALUE << USART_BSCALE_gp) & USART_BSCALE_gm) | + ((UART_BSEL_VALUE >> 8) & ~USART_BSCALE_gm); + +#if UART_CLK2X + UART_DEVICE.CTRLB = USART_RXEN_bm | USART_CLK2X_bm | USART_TXEN_bm; +#else + UART_DEVICE.CTRLB = USART_RXEN_bm | USART_TXEN_bm; +#endif // UART_CLK2X + + PORTC.OUTCLR = 1 << 4; // CTS Lo (enable) + PORTC.DIRSET = 1 << 4; // CTS Output +} + + +void uart_deinit() { + UART_DEVICE.CTRLB = 0; + UART_DEVICE.BAUDCTRLA = 0; + UART_DEVICE.BAUDCTRLB = 0; + UART_PORT.DIRCLR = 1 << UART_TX_PIN; +} diff --git a/avr/src/xboot/uart.h b/avr/src/xboot/uart.h new file mode 100644 index 0000000..0392f7e --- /dev/null +++ b/avr/src/xboot/uart.h @@ -0,0 +1,43 @@ +/************************************************************************/ +/* XBoot Extensible AVR Bootloader */ +/* */ +/* UART Module */ +/* */ +/* uart.h */ +/* */ +/* Alex Forencich */ +/* */ +/* Copyright (c) 2010 Alex Forencich */ +/* */ +/* Permission is hereby granted, free of charge, to any person */ +/* obtaining a copy of this software and associated documentation */ +/* files(the "Software"), to deal in the Software without restriction, */ +/* including without limitation the rights to use, copy, modify, merge, */ +/* publish, distribute, sublicense, and/or sell copies of the Software, */ +/* and to permit persons to whom the Software is furnished to do so, */ +/* subject to the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ +/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ +/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ +/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ +/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ +/* SOFTWARE. */ +/* */ +/************************************************************************/ + +#pragma once + +#include +#include + +bool uart_has_char(); +uint8_t uart_recv_char(); +void uart_send_char_blocking(uint8_t c); +void uart_init(void); +void uart_deinit(void); diff --git a/avr/src/xboot/watchdog.c b/avr/src/xboot/watchdog.c new file mode 100644 index 0000000..a7e1064 --- /dev/null +++ b/avr/src/xboot/watchdog.c @@ -0,0 +1,51 @@ +/************************************************************************/ +/* XBoot Extensible AVR Bootloader */ +/* */ +/* Watchdog Module */ +/* */ +/* watchdog.c */ +/* */ +/* Alex Forencich */ +/* */ +/* Copyright (c) 2010 Alex Forencich */ +/* */ +/* Permission is hereby granted, free of charge, to any person */ +/* obtaining a copy of this software and associated documentation */ +/* files(the "Software"), to deal in the Software without restriction, */ +/* including without limitation the rights to use, copy, modify, merge, */ +/* publish, distribute, sublicense, and/or sell copies of the Software, */ +/* and to permit persons to whom the Software is furnished to do so, */ +/* subject to the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ +/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ +/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ +/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ +/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ +/* SOFTWARE. */ +/* */ +/************************************************************************/ + +#include "watchdog.h" + + +void WDT_EnableAndSetTimeout() { + uint8_t temp = WDT_ENABLE_bm | WDT_CEN_bm | WATCHDOG_TIMEOUT; + CCP = CCP_IOREG_gc; + WDT.CTRL = temp; + + // Wait for WD to synchronize with new settings. + while (WDT_IsSyncBusy()) continue; +} + + +void WDT_Disable() { + uint8_t temp = (WDT.CTRL & ~WDT_ENABLE_bm) | WDT_CEN_bm; + CCP = CCP_IOREG_gc; + WDT.CTRL = temp; +} diff --git a/avr/src/xboot/watchdog.h b/avr/src/xboot/watchdog.h new file mode 100644 index 0000000..f9fcc74 --- /dev/null +++ b/avr/src/xboot/watchdog.h @@ -0,0 +1,42 @@ +/************************************************************************/ +/* XBoot Extensible AVR Bootloader */ +/* */ +/* Watchdog Module */ +/* */ +/* watchdog.c */ +/* */ +/* Alex Forencich */ +/* */ +/* Copyright (c) 2010 Alex Forencich */ +/* */ +/* Permission is hereby granted, free of charge, to any person */ +/* obtaining a copy of this software and associated documentation */ +/* files(the "Software"), to deal in the Software without restriction, */ +/* including without limitation the rights to use, copy, modify, merge, */ +/* publish, distribute, sublicense, and/or sell copies of the Software, */ +/* and to permit persons to whom the Software is furnished to do so, */ +/* subject to the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ +/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ +/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ +/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ +/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ +/* SOFTWARE. */ +/* */ +/************************************************************************/ + +#pragma once + +#include "xboot.h" + +#define WDT_IsSyncBusy() (WDT.STATUS & WDT_SYNCBUSY_bm) +#define WDT_Reset() asm("wdr") + +void WDT_EnableAndSetTimeout(); +void WDT_Disable(); diff --git a/avr/src/xboot/xboot.c b/avr/src/xboot/xboot.c new file mode 100644 index 0000000..7fd5ac0 --- /dev/null +++ b/avr/src/xboot/xboot.c @@ -0,0 +1,401 @@ +/************************************************************************/ +/* XBoot Extensible AVR Bootloader */ +/* */ +/* xboot.c */ +/* */ +/* Alex Forencich */ +/* */ +/* Copyright (c) 2010 Alex Forencich */ +/* */ +/* Permission is hereby granted, free of charge, to any person */ +/* obtaining a copy of this software and associated documentation */ +/* files(the "Software"), to deal in the Software without restriction, */ +/* including without limitation the rights to use, copy, modify, merge, */ +/* publish, distribute, sublicense, and/or sell copies of the Software, */ +/* and to permit persons to whom the Software is furnished to do so, */ +/* subject to the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ +/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ +/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ +/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ +/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ +/* SOFTWARE. */ +/* */ +/************************************************************************/ + +#include "xboot.h" + +#include + +#include + + +uint8_t buffer[SPM_PAGESIZE]; + + +void NVM_Wait() { + while (NVM_STATUS & NVM_NVMBUSY_bp) + // reset watchdog while waiting for erase completion + WDT_Reset(); +} + + +int main() { + // Initialization section + // Entry point and communication methods are initialized here + // -------------------------------------------------- +#ifdef USE_32MHZ_RC +#if (F_CPU != 32000000L) +#error F_CPU must match oscillator setting! +#endif // F_CPU + OSC.CTRL |= OSC_RC32MEN_bm; // turn on 32 MHz oscillator + while (!(OSC.STATUS & OSC_RC32MRDY_bm)) continue; // wait for it to start + CCP = CCP_IOREG_gc; + CLK.CTRL = CLK_SCLKSEL_RC32M_gc; +#ifdef USE_DFLL + OSC.CTRL |= OSC_RC32KEN_bm; + while(!(OSC.STATUS & OSC_RC32KRDY_bm)); + DFLLRC32M.CTRL = DFLL_ENABLE_bm; +#endif // USE_DFLL +#else // USE_32MHZ_RC +#if (F_CPU != 2000000L) +#error F_CPU must match oscillator setting! +#endif // F_CPU +#ifdef USE_DFLL + OSC.CTRL |= OSC_RC32KEN_bm; + while(!(OSC.STATUS & OSC_RC32KRDY_bm)); + DFLLRC2M.CTRL = DFLL_ENABLE_bm; +#endif // USE_DFLL +#endif // USE_32MHZ_RC + + // Initialize UART + uart_init(); + + // End initialization section + + uint32_t address = 0; + uint16_t i = 0; + + bool in_bootloader = false; + uint16_t j = ENTER_WAIT; + while (!in_bootloader && 0 < j--) { + // Check for trigger + if (uart_has_char()) in_bootloader = uart_recv_char() == CMD_SYNC; + + _delay_ms(1); + } + + WDT_EnableAndSetTimeout(); + + // Main bootloader + while (in_bootloader) { + uint8_t val = get_char(); + WDT_Reset(); + + // Main bootloader parser + switch (val) { + case CMD_CHECK_AUTOINCREMENT: send_char(REPLY_YES); break; + + case CMD_SET_ADDRESS: + address = get_word(); + send_char(REPLY_ACK); + break; + + case CMD_SET_EXT_ADDRESS: + address = ((uint32_t)get_char() << 16) | get_word(); + send_char(REPLY_ACK); + break; + + case CMD_CHIP_ERASE: + // Erase the application section + SP_EraseApplicationSection(); + + // Wait for completion + NVM_Wait(); + + // Erase EEPROM + EEPROM_erase_all(); + + send_char(REPLY_ACK); + break; + + case CMD_CHECK_BLOCK_SUPPORT: + send_char(REPLY_YES); + // Send block size (page size) + send_char((SPM_PAGESIZE >> 8) & 0xFF); + send_char(SPM_PAGESIZE & 0xFF); + break; + + case CMD_BLOCK_LOAD: + i = get_word(); // Block size + val = get_char(); // Memory type + send_char(BlockLoad(i, val, &address)); // Load it + break; + + case CMD_BLOCK_READ: + i = get_word(); // Block size + val = get_char(); // Memory type + BlockRead(i, val, &address); // Read it + break; + + case CMD_READ_BYTE: { + unsigned w = SP_ReadWord((address << 1)); + + send_char(w >> 8); + send_char(w); + + address++; + break; + } + + case CMD_WRITE_LOW_BYTE: + i = get_char(); // get low byte + send_char(REPLY_ACK); + break; + + case CMD_WRITE_HIGH_BYTE: + i |= (get_char() << 8); // get high byte; combine + SP_LoadFlashWord((address << 1), i); + address++; + send_char(REPLY_ACK); + break; + + case CMD_WRITE_PAGE: + if (address >= (APP_SECTION_SIZE >> 1)) + send_char(REPLY_ERROR); // don't allow bootloader overwrite + + else { + SP_WriteApplicationPage(address << 1); + send_char(REPLY_ACK); + } + break; + + case CMD_WRITE_EEPROM_BYTE: + EEPROM_write_byte(address, get_char()); + address++; + send_char(REPLY_ACK); + break; + + case CMD_READ_EEPROM_BYTE: + send_char(EEPROM_read_byte(address)); + address++; + break; + + case CMD_READ_LOW_FUSE_BITS: send_char(SP_ReadFuseByte(0)); break; + case CMD_READ_HIGH_FUSE_BITS: send_char(SP_ReadFuseByte(1)); break; + case CMD_READ_EXT_FUSE_BITS: send_char(SP_ReadFuseByte(2)); break; + + case CMD_ENTER_PROG_MODE: case CMD_LEAVE_PROG_MODE: + send_char(REPLY_ACK); + break; + + case CMD_EXIT_BOOTLOADER: + in_bootloader = false; + send_char(REPLY_ACK); + break; + + case CMD_PROGRAMMER_TYPE: send_char('S'); break; // serial + + case CMD_DEVICE_CODE: + send_char(123); // send only this device + send_char(0); // terminator + break; + + case CMD_SET_LED: case CMD_CLEAR_LED: case CMD_SET_TYPE: + get_char(); // discard parameter + send_char(REPLY_ACK); + break; + + case CMD_PROGRAM_ID: + send_char('X'); + send_char('B'); + send_char('o'); + send_char('o'); + send_char('t'); + send_char('+'); + send_char('+'); + break; + + case CMD_VERSION: + send_char('0' + XBOOT_VERSION_MAJOR); + send_char('0' + XBOOT_VERSION_MINOR); + break; + + case CMD_READ_SIGNATURE: + send_char(SIGNATURE_2); + send_char(SIGNATURE_1); + send_char(SIGNATURE_0); + break; + + case CMD_CRC: { + uint32_t start = 0; + uint32_t length = 0; + uint16_t crc; + + val = get_char(); + + switch (val) { + case SECTION_FLASH: length = PROGMEM_SIZE; break; + case SECTION_APPLICATION: length = APP_SECTION_SIZE; break; + case SECTION_BOOT: + start = BOOT_SECTION_START; + length = BOOT_SECTION_SIZE; + break; + + default: + send_char(REPLY_ERROR); + continue; + } + + crc = crc16_block(start, length); + + send_char((crc >> 8) & 0xff); + send_char(crc & 0xff); + break; + } + + case CMD_SYNC: break; // ESC (0x1b) to sync + + default: // otherwise, error + send_char(REPLY_ERROR); + break; + } + + // Wait for any lingering SPM instructions to finish + NVM_Wait(); + } + + // Bootloader exit + // Code here runs after the bootloader has exited, + // but before the application code has started + + // Shut down UART + uart_deinit(); + + WDT_Disable(); + + // Jump into main code + asm("jmp 0"); +} + + +uint8_t get_char() { + while (!uart_has_char()) continue; + return uart_recv_char(); +} + + +void send_char(uint8_t c) {uart_send_char_blocking(c);} +uint16_t get_word() {return (get_char() << 8) | get_char();} + + +uint8_t BlockLoad(unsigned size, uint8_t mem, uint32_t *address) { + WDT_Reset(); + + // fill up buffer + for (int i = 0; i < SPM_PAGESIZE; i++) { + char c = 0xff; + if (i < size) c = get_char(); + buffer[i] = c; + } + + switch (mem) { + case MEM_EEPROM: + EEPROM_write_block(*address, buffer, size); + *address += size; + break; + + case MEM_FLASH: + // NOTE: Flash programming, address is given in words. + SP_LoadFlashPage(buffer); + SP_EraseWriteApplicationPage(*address << 1); + *address += size >> 1; + NVM_Wait(); + break; + + case MEM_USERSIG: + SP_LoadFlashPage(buffer); + SP_EraseUserSignatureRow(); + NVM_Wait(); + SP_WriteUserSignatureRow(); + NVM_Wait(); + break; + + default: return REPLY_ERROR; + } + + return REPLY_ACK; +} + + + +void BlockRead(unsigned size, uint8_t mem, uint32_t *address) { + int offset = 0; + int size2 = size; + + switch (mem) { + case MEM_EEPROM: + EEPROM_read_block(*address, buffer, size); + (*address) += size; + break; + + case MEM_FLASH: case MEM_USERSIG: case MEM_PRODSIG: { + *address <<= 1; // Convert address to bytes temporarily + + do { + switch (mem) { + case MEM_FLASH: buffer[offset++] = SP_ReadByte(*address); break; + + case MEM_USERSIG: + buffer[offset++] = SP_ReadUserSignatureByte(*address); + break; + + case MEM_PRODSIG: + buffer[offset++] = SP_ReadCalibrationByte(*address); + break; + } + + NVM_Wait(); + + (*address)++; // Select next word in memory. + size--; // Subtract two bytes from number of bytes to read + } while (size); // Repeat until all block has been read + + *address >>= 1; // Convert address back to Flash words again. + break; + } + + default: send_char(REPLY_ERROR); break; + } + + // send bytes + for (int i = 0; i < size2; i++) + send_char(buffer[i]); +} + + +uint16_t crc16_block(uint32_t start, uint32_t length) { + uint16_t crc = 0; + + int bc = SPM_PAGESIZE; + + for (; length > 0; length--) { + if (bc == SPM_PAGESIZE) { + SP_ReadFlashPage(buffer, start); + start += SPM_PAGESIZE; + bc = 0; + } + + crc = _crc16_update(crc, buffer[bc]); + + bc++; + } + + return crc; +} diff --git a/avr/src/xboot/xboot.h b/avr/src/xboot/xboot.h new file mode 100644 index 0000000..7b327d7 --- /dev/null +++ b/avr/src/xboot/xboot.h @@ -0,0 +1,168 @@ +/************************************************************************/ +/* XBoot Extensible AVR Bootloader */ +/* */ +/* xboot.h */ +/* */ +/* Alex Forencich */ +/* */ +/* Copyright (c) 2010 Alex Forencich */ +/* */ +/* Permission is hereby granted, free of charge, to any person */ +/* obtaining a copy of this software and associated documentation */ +/* files(the "Software"), to deal in the Software without restriction, */ +/* including without limitation the rights to use, copy, modify, merge, */ +/* publish, distribute, sublicense, and/or sell copies of the Software, */ +/* and to permit persons to whom the Software is furnished to do so, */ +/* subject to the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ +/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ +/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ +/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ +/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ +/* SOFTWARE. */ +/* */ +/************************************************************************/ + +#pragma once + +#include +#include +#include + +// token pasting +#define CAT2_int(x, y) x ## y +#define CAT2(x, y) CAT2_int(x, y) +#define CAT3_int(x, y, z) x ## y ## z +#define CAT3(x, y, z) CAT3_int(x, y, z) + +// Version +#define XBOOT_VERSION_MAJOR 1 +#define XBOOT_VERSION_MINOR 7 + +// Configuration +#define ENTER_WAIT 1000 // In ms +#define UART_BAUD_RATE 115200 +#define UART_NUMBER 0 +#define UART_PORT_NAME C +#define USE_AVR1008_EEPROM +#define USE_DFLL +#define WATCHDOG_TIMEOUT WDT_PER_1KCLK_gc + +// clock config +// use 32MHz osc if makefile calls for it +#if (F_CPU == 32000000L) +// defaults to 2MHz RC oscillator +// define USE_32MHZ_RC to override +#define USE_32MHZ_RC +#endif // F_CPU + +// UART RS485 Enable Output +#define UART_EN_PORT CAT2(PORT, UART_EN_PORT_NAME) + +#if (UART_NUMBER == 0) +#define UART_RX_PIN 2 +#define UART_TX_PIN 3 +#else +#define UART_RX_PIN 6 +#define UART_TX_PIN 7 +#endif +#define UART_PORT CAT2(PORT, UART_PORT_NAME) +#define UART_DEVICE_PORT CAT2(UART_PORT_NAME, UART_NUMBER) +#define UART_DEVICE CAT2(USART, UART_DEVICE_PORT) +#define UART_DEVICE_RXC_ISR CAT3(USART, UART_DEVICE_PORT, _RXC_vect) +#define UART_DEVICE_DRE_ISR CAT3(USART, UART_DEVICE_PORT, _DRE_vect) +#define UART_DEVICE_TXC_ISR CAT3(USART, UART_DEVICE_PORT, _TXC_vect) +#define UART_RX_PIN_CTRL CAT3(UART_PORT.PIN, UART_RX_PIN, CTRL) +#define UART_TX_PIN_CTRL CAT3(UART_PORT.PIN, UART_TX_PIN, CTRL) + +// BAUD Rate Values +// Known good at 2MHz +#if (F_CPU == 2000000L) && (UART_BAUD_RATE == 19200) +#define UART_BSEL_VALUE 12 +#define UART_BSCALE_VALUE 0 +#define UART_CLK2X 1 +#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 38400) +#define UART_BSEL_VALUE 22 +#define UART_BSCALE_VALUE -2 +#define UART_CLK2X 1 +#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 57600) +#define UART_BSEL_VALUE 26 +#define UART_BSCALE_VALUE -3 +#define UART_CLK2X 1 +#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 115200) +#define UART_BSEL_VALUE 19 +#define UART_BSCALE_VALUE -4 +#define UART_CLK2X 1 + +// Known good at 32MHz +#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 19200) +#define UART_BSEL_VALUE 103 +#define UART_BSCALE_VALUE 0 +#define UART_CLK2X 0 +#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 38400) +#define UART_BSEL_VALUE 51 +#define UART_BSCALE_VALUE 0 +#define UART_CLK2X 0 +#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 57600) +#define UART_BSEL_VALUE 34 +#define UART_BSCALE_VALUE 0 +#define UART_CLK2X 0 +#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 115200) +#define UART_BSEL_VALUE 1047 +#define UART_BSCALE_VALUE -6 +#define UART_CLK2X 0 + +#else // None of the above, so calculate something +#warning Not using predefined BAUD rate, possible BAUD rate error! +#if (F_CPU == 2000000L) +#define UART_BSEL_VALUE ((F_CPU) / ((uint32_t)UART_BAUD_RATE * 8) - 1) +#define UART_BSCALE_VALUE 0 +#define UART_CLK2X 1 + +#else +#define UART_BSEL_VALUE ((F_CPU) / ((uint32_t)UART_BAUD_RATE * 16) - 1) +#define UART_BSCALE_VALUE 0 +#define UART_CLK2X 0 +#endif +#endif + +#ifndef EEPROM_PAGE_SIZE +#define EEPROM_PAGE_SIZE E2PAGESIZE +#endif + +#ifndef EEPROM_BYTE_ADDRESS_MASK +#if EEPROM_PAGE_SIZE == 32 +#define EEPROM_BYTE_ADDRESS_MASK 0x1f +#elif EEPROM_PAGE_SIZE == 16 +#define EEPROM_BYTE_ADDRESS_MASK = 0x0f +#elif EEPROM_PAGE_SIZE == 8 +#define EEPROM_BYTE_ADDRESS_MASK = 0x07 +#elif EEPROM_PAGE_SIZE == 4 +#define EEPROM_BYTE_ADDRESS_MASK = 0x03 +#else +#error Unknown EEPROM page size! Please add new byte address value! +#endif +#endif + +// Includes +#include "protocol.h" +#include "sp_driver.h" +#include "eeprom_driver.h" +#include "uart.h" +#include "watchdog.h" + +// Functions +uint8_t get_char(void); +void send_char(uint8_t c); +uint16_t get_word(void); + +uint8_t BlockLoad(unsigned size, uint8_t mem, uint32_t *address); +void BlockRead(unsigned size, uint8_t mem, uint32_t *address); + +uint16_t crc16_block(uint32_t start, uint32_t length); diff --git a/avr/tmc2660_decode.py b/avr/tmc2660_decode.py new file mode 100755 index 0000000..1b5adb3 --- /dev/null +++ b/avr/tmc2660_decode.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 + +import sys +import csv +import json + + +rdsel = 1 + + +def twos_comp(val, bits): + if val & (1 << (bits - 1)): return val - (1 << bits) + return val + + +def tmc2660_decode_response(x, rdsel = 1): + x >>= 4 # Shift right 4 bits + d = {'_hex': '0x%05x' % x} + + if rdsel == 0: + d['mstep'] = (x >> 10) & 0x3ff + + elif rdsel == 1: + d['sg'] = (x >> 10) & 0x3ff + + elif rdsel == 2: + d['sg'] = x >> 15 & 0x1f + d['se'] = (x >> 10) & 0x1f + + flags = [] + if x & (1 << 7): flags += ['stand'] + if x & (1 << 6): flags += ['open B'] + if x & (1 << 5): flags += ['open A'] + if x & (1 << 4): flags += ['short B'] + if x & (1 << 3): flags += ['short A'] + if x & (1 << 2): flags += ['temp warn'] + if x & (1 << 1): flags += ['overtemp'] + if x & (1 << 0): flags += ['stall'] + + d['flags'] = flags + + return d + + +def tmc2660_decode_cmd(x, r): + global rdsel + + addr = x >> 17 + + d = {'_hex': '0x%05x' % x} + + if addr == 0 or addr == 1: + cmd = 'DRVCTRL' + d['mstep'] = (256, 128, 64, 32, 16, 8, 4, 2, 1)[x & 0xf] + d['dedge'] = bool(x & (1 << 8)) + d['intpol'] = bool(x & (1 << 9)) + + elif addr == 4: + chm = bool(x & (1 << 14)) + + cmd = 'CHOPCONF' + d['blank'] = (16, 24, 36, 54)[(x >> 15) & 3] + d['chm'] = 'standard' if chm else 'constant' + d['random toff'] = bool(x & (1 << 13)) + + if chm: d['fast decay mode'] = ('current', 'timer')[x >> 12] + else: d['hdec'] = (16, 32, 48, 64)[(x >> 11) & 3] + + if chm: d['SWO'] = ((x >> 7) & 0xf) - 3 + else: d['HEND'] = ((x >> 7) & 0xf) - 3 + + if chm: d['fast decay'] = (((x >> 4) & 7) + ((x >> 7) & 8)) * 32 + else: d['HSTART'] = ((x >> 4) & 7) + 1 + + # TODO this isn't quite right + toff = x & 0xf + if toff == 0: d['TOFF'] = 'disabled' + else: d['TOFF'] = 12 + (32 * toff) + + elif addr == 5: + cmd = 'SMARTEN' + d['SEIMIN'] = '1/4 CS' if x & (1 << 15) else '1/2 CS' + d['SEDN'] = (32, 8, 2, 1)[(x >> 13) & 3] + d['SEMAX'] = (x >> 8) & 0xf + d['SEUP'] = (1, 2, 4, 8)[(x >> 5) & 3] + semin = x & 0xf + d['SEMIN'] = semin if semin else 'disabled' + + elif addr == 6: + cmd = 'SGCSCONF' + d['SFILT'] = bool(x & (1 << 16)) + d['SGT'] = twos_comp((x >> 8) & 0x7f, 7) + d['CS'] = ((x & 0x1f) + 1) / 32.0 + + elif addr == 7: + cmd = 'DRVCONF' + d['TST'] = bool(x & (1 << 16)) + d['SLPH'] = ('min', 'min temp', 'med temp', 'max')[(x >> 14) & 3] + d['SLPL'] = ('min', 'min', 'med', 'max')[(x >> 12) & 3] + d['DISS2G'] = bool(x & (1 << 10)) + d['TS2G'] = (3.2, 1.6, 1.2, 0.8)[(x >> 8) & 3] + d['SDOFF'] = 'SPI' if x & (1 << 7) else 'step' + d['VSENSE'] = '165mV' if x & (1 << 6) else '305mV' + rdsel = (x >> 4) & 3 + d['RDSEL'] = ('mstep', 'SG', 'SG & CS', 'Invalid')[rdsel] + + else: cmd = 'INVALID' + + return {cmd: d, 'response': tmc2660_decode_response(r, rdsel)} + + +def tmc2660_decode(path): + with open(path, newline = '') as f: + reader = csv.reader(f) + + first = True + for time, packet, mosi, miso in reader: + if first: + first = False + continue + + cmd = tmc2660_decode_cmd(int(mosi, 0), int(miso, 0)) + cmd['id'] = int(packet) + cmd['ts'] = float(time) + + print(json.dumps(cmd, sort_keys = True)) + + +if __name__ == "__main__": + for path in sys.argv[1:]: + tmc2660_decode(path) diff --git a/data_usage.py b/data_usage.py deleted file mode 100755 index 8ba92f0..0000000 --- a/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/src/axis.c b/src/axis.c deleted file mode 100644 index b01f99b..0000000 --- a/src/axis.c +++ /dev/null @@ -1,214 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - - -int motor_map[AXES] = {-1, -1, -1, -1, -1, -1}; - - -typedef struct { - float velocity_max; // max velocity in mm/min or deg/min - float feedrate_max; // max velocity in mm/min or deg/min - float travel_max; // max work envelope for soft limits - float travel_min; // min work envelope for soft limits - float jerk_max; // max jerk (Jm) in mm/min^3 divided by 1 million - float recip_jerk; // reciprocal of current jerk value - with million - float junction_dev; // aka cornering delta - float radius; // radius in mm for rotary axes - float search_velocity; // homing search velocity - float latch_velocity; // homing latch velocity - float latch_backoff; // backoff from switches prior to homing latch movement - float zero_backoff; // backoff from switches for machine zero - homing_mode_t homing_mode; - bool homed; -} axis_t; - - -axis_t axes[MOTORS] = { - { - .velocity_max = X_VELOCITY_MAX, - .feedrate_max = X_FEEDRATE_MAX, - .travel_min = X_TRAVEL_MIN, - .travel_max = X_TRAVEL_MAX, - .jerk_max = X_JERK_MAX, - .junction_dev = X_JUNCTION_DEVIATION, - .search_velocity = X_SEARCH_VELOCITY, - .latch_velocity = X_LATCH_VELOCITY, - .latch_backoff = X_LATCH_BACKOFF, - .zero_backoff = X_ZERO_BACKOFF, - .homing_mode = X_HOMING_MODE, - }, { - .velocity_max = Y_VELOCITY_MAX, - .feedrate_max = Y_FEEDRATE_MAX, - .travel_min = Y_TRAVEL_MIN, - .travel_max = Y_TRAVEL_MAX, - .jerk_max = Y_JERK_MAX, - .junction_dev = Y_JUNCTION_DEVIATION, - .search_velocity = Y_SEARCH_VELOCITY, - .latch_velocity = Y_LATCH_VELOCITY, - .latch_backoff = Y_LATCH_BACKOFF, - .zero_backoff = Y_ZERO_BACKOFF, - .homing_mode = Y_HOMING_MODE, - }, { - .velocity_max = Z_VELOCITY_MAX, - .feedrate_max = Z_FEEDRATE_MAX, - .travel_min = Z_TRAVEL_MIN, - .travel_max = Z_TRAVEL_MAX, - .jerk_max = Z_JERK_MAX, - .junction_dev = Z_JUNCTION_DEVIATION, - .search_velocity = Z_SEARCH_VELOCITY, - .latch_velocity = Z_LATCH_VELOCITY, - .latch_backoff = Z_LATCH_BACKOFF, - .zero_backoff = Z_ZERO_BACKOFF, - .homing_mode = Z_HOMING_MODE, - }, { - .velocity_max = A_VELOCITY_MAX, - .feedrate_max = A_FEEDRATE_MAX, - .travel_min = A_TRAVEL_MIN, - .travel_max = A_TRAVEL_MAX, - .jerk_max = A_JERK_MAX, - .junction_dev = A_JUNCTION_DEVIATION, - .radius = A_RADIUS, - .search_velocity = A_SEARCH_VELOCITY, - .latch_velocity = A_LATCH_VELOCITY, - .latch_backoff = A_LATCH_BACKOFF, - .zero_backoff = A_ZERO_BACKOFF, - .homing_mode = A_HOMING_MODE, - } -}; - - -bool axis_is_enabled(int axis) { - int motor = axis_get_motor(axis); - return motor != -1 && motor_is_enabled(motor); -} - - -char axis_get_char(int axis) { - return (axis < 0 || AXES <= axis) ? '?' : "XYZABCUVW"[axis]; -} - - -int axis_get_id(char axis) { - const char *axes = "XYZABCUVW"; - char *ptr = strchr(axes, axis); - return ptr == 0 ? -1 : (ptr - axes); -} - - -int axis_get_motor(int axis) {return motor_map[axis];} - - -void axis_set_motor(int axis, int motor) { - motor_map[axis] = motor; - axis_set_jerk_max(axis, axes[motor].jerk_max); // Init 1/jerk -} - - -float axis_get_vector_length(const float a[], const float b[]) { - return sqrt(square(a[AXIS_X] - b[AXIS_X]) + square(a[AXIS_Y] - b[AXIS_Y]) + - square(a[AXIS_Z] - b[AXIS_Z]) + square(a[AXIS_A] - b[AXIS_A]) + - square(a[AXIS_B] - b[AXIS_B]) + square(a[AXIS_C] - b[AXIS_C])); -} - - -#define AXIS_VAR_GET(NAME, TYPE) \ - TYPE get_##NAME(int axis) {return axes[axis].NAME;} - - -#define AXIS_VAR_SET(NAME, TYPE) \ - void set_##NAME(int axis, TYPE value) {axes[axis].NAME = value;} - - -#define AXIS_GET(NAME, TYPE, DEFAULT) \ - TYPE axis_get_##NAME(int axis) { \ - int motor = axis_get_motor(axis); \ - return motor == -1 ? DEFAULT : axes[motor].NAME; \ - } \ - AXIS_VAR_GET(NAME, TYPE) - - -#define AXIS_SET(NAME, TYPE) \ - void axis_set_##NAME(int axis, TYPE value) { \ - int motor = axis_get_motor(axis); \ - if (motor != -1) axes[motor].NAME = value; \ - } \ - AXIS_VAR_SET(NAME, TYPE) - - -AXIS_GET(velocity_max, float, 0) -AXIS_GET(feedrate_max, float, 0) -AXIS_GET(homed, bool, false) -AXIS_SET(homed, bool) -AXIS_GET(homing_mode, homing_mode_t, HOMING_DISABLED) -AXIS_SET(homing_mode, homing_mode_t) -AXIS_GET(radius, float, 0) -AXIS_GET(travel_min, float, DISABLE_SOFT_LIMIT) -AXIS_GET(travel_max, float, DISABLE_SOFT_LIMIT) -AXIS_GET(search_velocity, float, 0) -AXIS_GET(latch_velocity, float, 0) -AXIS_GET(zero_backoff, float, 0) -AXIS_GET(latch_backoff, float, 0) -AXIS_GET(junction_dev, float, 0) -AXIS_GET(recip_jerk, float, 0) - - -/* Note on jerk functions - * - * Jerk values can be rather large. Jerk values are stored in the system in - * truncated format; values are divided by 1,000,000 then multiplied before use. - * - * The axis_jerk() functions expect the jerk in divided by 1,000,000 form. - */ -AXIS_GET(jerk_max, float, 0) - - -/// Sets jerk and its reciprocal for axis -void axis_set_jerk_max(int axis, float jerk) { - axes[axis].jerk_max = jerk; - axes[axis].recip_jerk = 1 / (jerk * JERK_MULTIPLIER); -} - - -AXIS_VAR_SET(velocity_max, float) -AXIS_VAR_SET(feedrate_max, float) -AXIS_VAR_SET(radius, float) -AXIS_VAR_SET(travel_min, float) -AXIS_VAR_SET(travel_max, float) -AXIS_VAR_SET(search_velocity, float) -AXIS_VAR_SET(latch_velocity, float) -AXIS_VAR_SET(zero_backoff, float) -AXIS_VAR_SET(latch_backoff, float) -AXIS_VAR_SET(junction_dev, float) -AXIS_VAR_SET(jerk_max, float) diff --git a/src/axis.h b/src/axis.h deleted file mode 100644 index 505cca1..0000000 --- a/src/axis.h +++ /dev/null @@ -1,76 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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_DISABLED, - HOMING_STALL_MIN, - HOMING_STALL_MAX, - HOMING_SWITCH_MIN, - HOMING_SWITCH_MAX, -} homing_mode_t; - - -bool axis_is_enabled(int axis); -char axis_get_char(int axis); -int axis_get_id(char axis); -int axis_get_motor(int axis); -void axis_set_motor(int axis, int motor); -float axis_get_vector_length(const float a[], const float b[]); - -float axis_get_velocity_max(int axis); -float axis_get_feedrate_max(int axis); -float axis_get_jerk_max(int axis); -void axis_set_jerk_max(int axis, float jerk); -bool axis_get_homed(int axis); -void axis_set_homed(int axis, bool homed); -homing_mode_t axis_get_homing_mode(int axis); -void axis_set_homing_mode(int axis, homing_mode_t mode); -float axis_get_radius(int axis); -float axis_get_travel_min(int axis); -float axis_get_travel_max(int axis); -float axis_get_search_velocity(int axis); -float axis_get_latch_velocity(int axis); -float axis_get_zero_backoff(int axis); -float axis_get_latch_backoff(int axis); -float axis_get_junction_dev(int axis); -float axis_get_recip_jerk(int axis); -float axis_get_jerk_max(int axis); diff --git a/src/command.c b/src/command.c deleted file mode 100644 index 936878a..0000000 --- a/src/command.c +++ /dev/null @@ -1,335 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "homing.h" -#include "probing.h" -#include "i2c.h" -#include "plan/jog.h" -#include "plan/calibrate.h" -#include "plan/buffer.h" -#include "plan/arc.h" -#include "plan/state.h" -#include "config.h" - -#include -#include - -#include -#include -#include -#include - - -static char *_cmd = 0; - - -static void _reboot() {hw_request_hard_reset();} - - -static unsigned _parse_axis(uint8_t axis) { - switch (axis) { - case 'x': return 0; case 'y': return 1; case 'z': return 2; - case 'a': return 3; case 'b': return 4; case 'c': return 5; - case 'X': return 0; case 'Y': return 1; case 'Z': return 2; - case 'A': return 3; case 'B': return 4; case 'C': return 5; - default: return axis; - } -} - - -static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) { - switch (cmd) { - case I2C_NULL: break; - case I2C_ESTOP: estop_trigger(STAT_ESTOP_USER); break; - case I2C_CLEAR: estop_clear(); break; - case I2C_PAUSE: mp_request_hold(); break; - case I2C_OPTIONAL_PAUSE: mp_request_optional_pause(); break; - case I2C_RUN: mp_request_start(); break; - case I2C_STEP: mp_request_step(); break; - case I2C_FLUSH: mp_request_flush(); break; - case I2C_REPORT: report_request_full(); break; - case I2C_HOME: break; // TODO - case I2C_REBOOT: _reboot(); break; - case I2C_ZERO: - if (length == 0) mach_zero_all(); - else if (length == 1) mach_zero_axis(_parse_axis(*data)); - break; - } -} - - -void command_init() { - i2c_set_read_callback(command_i2c_cb); -} - - -// Command forward declarations -// (Don't be afraid, X-Macros rock!) -#define CMD(NAME, ...) \ - uint8_t command_##NAME(int, char *[]); - -#include "command.def" -#undef CMD - -// Command names & help -#define CMD(NAME, MINARGS, MAXARGS, HELP) \ - static const char pstr_##NAME[] PROGMEM = #NAME; \ - static const char NAME##_help[] PROGMEM = HELP; - -#include "command.def" -#undef CMD - -// Command table -#define CMD(NAME, MINARGS, MAXARGS, HELP) \ - {pstr_##NAME, command_##NAME, MINARGS, MAXARGS, NAME##_help}, - -static const command_t commands[] PROGMEM = { -#include "command.def" -#undef CMD - {}, // Sentinel -}; - - -int command_find(const char *match) { - for (int i = 0; ; i++) { - const char *name = pgm_read_word(&commands[i].name); - if (!name) break; - - if (strcmp_P(match, name) == 0) return i; - } - - return -1; -} - - -int command_exec(int argc, char *argv[]) { - putchar('\n'); - - int i = command_find(argv[0]); - if (i != -1) { - uint8_t min_args = pgm_read_byte(&commands[i].min_args); - uint8_t max_args = pgm_read_byte(&commands[i].max_args); - - if (argc <= min_args) return STAT_TOO_FEW_ARGUMENTS; - else if (max_args < argc - 1) return STAT_TOO_MANY_ARGUMENTS; - else { - command_cb_t cb = pgm_read_word(&commands[i].cb); - return cb(argc, argv); - } - - } else if (argc != 1) - return STAT_INVALID_OR_MALFORMED_COMMAND; - - // Get or set variable - char *value = strchr(argv[0], '='); - if (value) { - *value++ = 0; - if (vars_set(argv[0], value)) return STAT_OK; - - } else if (vars_print(argv[0])) { - putchar('\n'); - return STAT_OK; - } - - STATUS_ERROR(STAT_UNRECOGNIZED_NAME, "'%s'", argv[0]); - return STAT_UNRECOGNIZED_NAME; -} - - -int command_parser(char *cmd) { - // Parse line - char *p = cmd + 1; // Skip `$` - int argc = 0; - char *argv[MAX_ARGS] = {0}; - - if (cmd[1] == '$' && !cmd[2]) { - report_request_full(); // Full report - return STAT_OK; - } - - while (argc < MAX_ARGS && *p) { - // Skip space - while (*p && isspace(*p)) *p++ = 0; - - // Start of token - if (*p) argv[argc++] = p; - - // Find end - while (*p && !isspace(*p)) p++; - } - - // Exec command - if (argc) return command_exec(argc, argv); - - return STAT_OK; -} - - -static char *_command_next() { - if (_cmd) return _cmd; - - // Get next command - _cmd = usart_readline(); - if (!_cmd) return 0; - - // Remove leading whitespace - while (*_cmd && isspace(*_cmd)) _cmd++; - - // Remove trailing whitespace - for (size_t len = strlen(_cmd); len && isspace(_cmd[len - 1]); len--) - _cmd[len - 1] = 0; - - return _cmd; -} - - -void command_callback() { - if (!_command_next()) return; - - stat_t status = STAT_OK; - - switch (*_cmd) { - case 0: break; // Empty line - case '{': status = vars_parser(_cmd); break; - case '$': status = command_parser(_cmd); break; - case '%': break; // GCode program separator, ignore it - - default: - if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;} - else if (mp_is_flushing()) break; // Flush GCode command - else if (!mp_queue_get_room() || - mp_is_resuming() || - mach_arc_active() || - mach_is_homing() || - mach_is_probing() || - calibrate_busy() || - mp_jog_busy()) return; // Wait - - // Parse and execute GCode command - status = gc_gcode_parser(_cmd); - } - - _cmd = 0; // Command consumed - report_request(); - - if (status) status_error(status); -} - - -// Command functions -void static print_command_help(int i) { - static const char fmt[] PROGMEM = " $%-12S %S\n"; - const char *name = pgm_read_word(&commands[i].name); - const char *help = pgm_read_word(&commands[i].help); - - printf_P(fmt, name, help); -} - - -uint8_t command_help(int argc, char *argv[]) { - if (argc == 2) { - int i = command_find(argv[1]); - - if (i == -1) return STAT_UNRECOGNIZED_NAME; - else print_command_help(i); - - return STAT_OK; - } - - puts_P(PSTR("\nLine editing:\n" - " ENTER Submit current command line.\n" - " BS Backspace, delete last character.\n" - " CTRL-X Cancel current line entry.")); - - puts_P(PSTR("\nCommands:")); - for (int i = 0; ; i++) { - const char *name = pgm_read_word(&commands[i].name); - if (!name) break; - print_command_help(i); - wdt_reset(); - } - - puts_P(PSTR("\nVariables:")); - vars_print_help(); - - return STAT_OK; -} - - -uint8_t command_reboot(int argc, char *argv[]) { - _reboot(); - return 0; -} - - -uint8_t command_bootloader(int argc, char *argv[]) { - hw_request_bootloader(); - return 0; -} - - -uint8_t command_save(int argc, char *argv[]) { - vars_save(); - return 0; -} - - -uint8_t command_valid(int argc, char *argv[]) { - printf_P(vars_valid() ? PSTR("true\n") : PSTR("false\n")); - return 0; -} - - -uint8_t command_restore(int argc, char *argv[]) { - return vars_restore(); -} - - -uint8_t command_clear(int argc, char *argv[]) { - vars_clear(); - return 0; -} - - -uint8_t command_messages(int argc, char *argv[]) { - status_help(); - return 0; -} - - -uint8_t command_resume(int argc, char *argv[]) { - mp_request_resume(); - return 0; -} diff --git a/src/command.def b/src/command.def deleted file mode 100644 index 559e45d..0000000 --- a/src/command.def +++ /dev/null @@ -1,41 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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(reboot, 0, 0, "Reboot the controller") -CMD(bootloader, 0, 0, "Load bootloader") -CMD(save, 0, 0, "Save settings") -CMD(valid, 0, 0, "Print 'true' if saved settings are valid") -CMD(restore, 0, 0, "Restore settings") -CMD(clear, 0, 0, "Clear saved settings") -CMD(jog, 1, 4, "Jog") -CMD(mreset, 0, 1, "Reset motor") -CMD(calibrate, 0, 0, "Calibrate motors") -CMD(messages, 0, 0, "Dump all possible status messages") -CMD(resume, 0, 0, "Resume processing after a flush") -CMD(home, 0, 0, "Home all axes") diff --git a/src/command.h b/src/command.h deleted file mode 100644 index cecc572..0000000 --- a/src/command.h +++ /dev/null @@ -1,50 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/config.h b/src/config.h deleted file mode 100644 index 5255a0c..0000000 --- a/src/config.h +++ /dev/null @@ -1,343 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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" - -#include - - -// Pins -enum { - STALL_X_PIN = PORT_A << 3, - STALL_Y_PIN, - STALL_Z_PIN, - STALL_A_PIN, - SPIN_DIR_PIN, - SPIN_ENABLE_PIN, - ANALOG_PIN, - PROBE_PIN, - - MIN_X_PIN = PORT_B << 3, - MAX_X_PIN, - MIN_A_PIN, - MAX_A_PIN, - MIN_Y_PIN, - MAX_Y_PIN, - MIN_Z_PIN, - MAX_Z_PIN, - - SDA_PIN = PORT_C << 3, - SCL_PIN, - SERIAL_RX_PIN, - SERIAL_TX_PIN, - SERIAL_CTS_PIN, - SPI_CLK_PIN, - SPI_MISO_PIN, - SPI_MOSI_PIN, - - STEP_X_PIN = PORT_D << 3, - SPI_CS_X_PIN, - SPI_CS_A_PIN, - SPI_CS_Z_PIN, - SPIN_PWM_PIN, - SWITCH_1_PIN, - RS485_RO_PIN, - RS485_DI_PIN, - - STEP_Y_PIN = PORT_E << 3, - SPI_CS_Y_PIN, - DIR_X_PIN, - DIR_Y_PIN, - STEP_A_PIN, - SWITCH_2_PIN, - DIR_Z_PIN, - DIR_A_PIN, - - STEP_Z_PIN = PORT_F << 3, - RS485_RW_PIN, - FAULT_PIN, - ESTOP_PIN, - MOTOR_FAULT_PIN, - MOTOR_ENABLE_PIN, - NC_0_PIN, - NC_1_PIN, -}; - -#define SPI_SS_PIN SERIAL_CTS_PIN // Needed for SPI configuration - - -// Compile-time settings -#define __CLOCK_EXTERNAL_16MHZ // uses PLL to provide 32 MHz system clock -//#define __CLOCK_INTERNAL_32MHZ - - -#define AXES 6 // number of axes -#define MOTORS 4 // number of motors on the board -#define COORDS 6 // number of supported coordinate systems -#define SWITCHES 9 // number of supported limit switches -#define PWMS 2 // number of supported PWM channels - -#define DISABLE_SOFT_LIMIT -1000000 - - -// Motor settings. See motor.c -#define MOTOR_MAX_CURRENT 1.0 // 1.0 is full power -#define MOTOR_MIN_CURRENT 0.25 // 1.0 is full power -#define MOTOR_IDLE_CURRENT 0.05 // 1.0 is full power -#define MOTOR_STALL_THRESHOLD 0 // 0 -> 1 is least -> most sensitive -#define MOTOR_MICROSTEPS 32 -#define MOTOR_POWER_MODE MOTOR_POWERED_ONLY_WHEN_MOVING -#define MOTOR_IDLE_TIMEOUT 0.25 // secs, motor off after this time - -#define M1_AXIS AXIS_X -#define M1_STEP_ANGLE 1.8 -#define M1_TRAVEL_PER_REV 6.35 -#define M1_MICROSTEPS MOTOR_MICROSTEPS -#define M1_REVERSE false -#define M1_POWER_MODE MOTOR_POWER_MODE - -#define M2_AXIS AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 6.35 -#define M2_MICROSTEPS MOTOR_MICROSTEPS -#define M2_REVERSE false -#define M2_POWER_MODE MOTOR_POWER_MODE - -#define M3_AXIS AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV (25.4 / 6.0) -#define M3_MICROSTEPS MOTOR_MICROSTEPS -#define M3_REVERSE false -#define M3_POWER_MODE MOTOR_POWER_MODE - -#define M4_AXIS AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees per motor rev -#define M4_MICROSTEPS MOTOR_MICROSTEPS -#define M4_REVERSE false -#define M4_POWER_MODE MOTOR_POWER_MODE - - -// Switch settings. See switch.c -#define SWITCH_INTLVL PORT_INT0LVL_MED_gc -#define SW_LOCKOUT_TICKS 250 // ms -#define SW_DEGLITCH_TICKS 30 // ms - - -// Machine settings -//#define STEP_CORRECTION // Enable step correction -#define MAX_STEP_CORRECTION 4 // In steps per segment -#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arcs -#define JERK_MAX 5 // yes, that's km/min^3 -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal corner accel -#define JOG_JERK_MULT 4 // Jogging jerk multipler -#define JOG_MIN_VELOCITY 10 // mm/min -#define CAL_ACCELERATION 500000 // mm/min^2 - -// Axis settings -#define VELOCITY_MAX 13000 // mm/min -#define FEEDRATE_MAX VELOCITY_MAX - -#define X_VELOCITY_MAX VELOCITY_MAX // G0 max velocity in mm/min -#define X_FEEDRATE_MAX FEEDRATE_MAX // G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // minimum travel for soft limits -#define X_TRAVEL_MAX 350 // between switches or crashes -#define X_JERK_MAX JERK_MAX -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define X_SEARCH_VELOCITY 2400 // move in negative direction -#define X_LATCH_VELOCITY 100 // mm/min -#define X_LATCH_BACKOFF 5 // mm -#define X_ZERO_BACKOFF 1 // mm -#define X_HOMING_MODE HOMING_STALL_MAX - -#define Y_VELOCITY_MAX VELOCITY_MAX -#define Y_FEEDRATE_MAX FEEDRATE_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 350 -#define Y_JERK_MAX JERK_MAX -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SEARCH_VELOCITY 3000 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 5 -#define Y_ZERO_BACKOFF 1 -#define Y_HOMING_MODE HOMING_STALL_MAX - -#define Z_VELOCITY_MAX 2000 // VELOCITY_MAX -#define Z_FEEDRATE_MAX FEEDRATE_MAX -#define Z_TRAVEL_MIN 0 -#define Z_TRAVEL_MAX 75 -#define Z_JERK_MAX JERK_MAX -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SEARCH_VELOCITY 400 -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 5 -#define Z_ZERO_BACKOFF 1 -#define Z_HOMING_MODE HOMING_STALL_MAX - -// A values are chosen to make the A motor react the same as X for testing -// set to the same speed as X axis -#define A_VELOCITY_MAX (X_VELOCITY_MAX / M1_TRAVEL_PER_REV * 360) -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 -#define A_TRAVEL_MAX -1 // same value means infinite -#define A_JERK_MAX (X_JERK_MAX * 360 / M1_TRAVEL_PER_REV) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV / 2 / M_PI) -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_HOMING_MODE HOMING_DISABLED - - -// Spindle settings -#define SPINDLE_TYPE SPINDLE_TYPE_HUANYANG -#define SPINDLE_PWM_FREQUENCY 100 // in Hz -#define SPINDLE_MIN_RPM 1000 -#define SPINDLE_MAX_RPM 24000 -#define SPINDLE_MIN_DUTY 0.05 -#define SPINDLE_MAX_DUTY 0.99 -#define SPINDLE_REVERSE false - - -// Gcode defaults -#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE PLANE_XY // See machine.h -#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58, G59 -#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS -#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE -#define GCODE_DEFAULT_ARC_DISTANCE_MODE INCREMENTAL_MODE - - -// Motor ISRs -#define STALL_ISR_vect PORTA_INT1_vect -#define FAULT_ISR_vect PORTF_INT1_vect - - -/* Interrupt usage: - * - * HI Stepper timers (set in stepper.h) - * LO Segment execution SW interrupt (set in stepper.h) - * MED GPIO1 switch port (set in gpio.h) - * MED Serial RX (set in usart.c) - * MED Serial TX (set in usart.c) (* see note) - * LO Real time clock interrupt (set in xmega_rtc.h) - * - * (*) The TX cannot run at LO level or exception reports and other prints - * called from a LO interrupt (as in prep_line()) will kill the system - * in a permanent loop call in usart_putc() (usart.c). - */ - -// Timer assignments - see specific modules for details -// TCC1 free -#define TIMER_STEP TCC0 // Step timer (see stepper.h) -#define TIMER_PWM TCD1 // PWM timer (see pwm_spindle.c) - -#define M1_TIMER TCD0 -#define M2_TIMER TCE0 -#define M3_TIMER TCF0 -#define M4_TIMER TCE1 - -#define M1_DMA_CH DMA.CH0 -#define M2_DMA_CH DMA.CH1 -#define M3_DMA_CH DMA.CH2 -#define M4_DMA_CH DMA.CH3 - -#define M1_DMA_TRIGGER DMA_CH_TRIGSRC_TCD0_CCA_gc -#define M2_DMA_TRIGGER DMA_CH_TRIGSRC_TCE0_CCA_gc -#define M3_DMA_TRIGGER DMA_CH_TRIGSRC_TCF0_CCA_gc -#define M4_DMA_TRIGGER DMA_CH_TRIGSRC_TCE1_CCA_gc - - -// Timer setup for stepper and dwells -#define STEP_TIMER_DISABLE 0 -#define STEP_TIMER_ENABLE TC_CLKSEL_DIV4_gc -#define STEP_TIMER_DIV 4 -#define STEP_TIMER_FREQ (F_CPU / STEP_TIMER_DIV) -#define STEP_TIMER_POLL (STEP_TIMER_FREQ * 0.001) -#define STEP_TIMER_WGMODE TC_WGMODE_NORMAL_gc // count to TOP & rollover -#define STEP_TIMER_ISR TCC0_OVF_vect -#define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc - -#define MAX_SEGMENT_TIME ((float)0xffff / 60.0 / STEP_TIMER_FREQ) -#define NOM_SEGMENT_USEC 5000.0 // nominal segment time -#define MIN_SEGMENT_USEC 2500.0 // minimum segment time - - -// Huanyang settings -#define HUANYANG_PORT USARTD1 -#define HUANYANG_DRE_vect USARTD1_DRE_vect -#define HUANYANG_TXC_vect USARTD1_TXC_vect -#define HUANYANG_RXC_vect USARTD1_RXC_vect -#define HUANYANG_TIMEOUT 50 // ms. response timeout -#define HUANYANG_RETRIES 4 // Number of retries before failure -#define HUANYANG_ID 1 // Default ID - - -// Serial settings -#define SERIAL_BAUD USART_BAUD_115200 -#define SERIAL_PORT USARTC0 -#define SERIAL_DRE_vect USARTC0_DRE_vect -#define SERIAL_RXC_vect USARTC0_RXC_vect - - -// Input -#define INPUT_BUFFER_LEN 255 // text buffer size (255 max) - - -// Arc -#define ARC_RADIUS_ERROR_MAX 1.0 // max mm diff between start and end radius -#define ARC_RADIUS_ERROR_MIN 0.005 // min mm where 1% rule applies -#define ARC_RADIUS_TOLERANCE 0.001 // 0.1% radius variance test - - -// Planner -/// Should be at least the number of buffers required to support optimal -/// planning in the case of very short lines or arc segments. Suggest no less -/// than 12. Maximum is 255 with out also changing the type of mb.space. Must -/// leave headroom for stack. -#define PLANNER_BUFFER_POOL_SIZE 32 - -/// Buffers to reserve in planner before processing new input line -#define PLANNER_BUFFER_HEADROOM 4 - -/// Minimum number of filled buffers before timeout until execution starts -#define PLANNER_EXEC_MIN_FILL 4 - -/// Delay before executing new buffers unless PLANNER_EXEC_MIN_FILL is met -/// This gives the planner a chance to make a good plan before execution starts -#define PLANNER_EXEC_DELAY 250 // In ms - - -// I2C -#define I2C_DEV TWIC -#define I2C_ISR TWIC_TWIS_vect -#define I2C_ADDR 0x2b -#define I2C_MAX_DATA 8 diff --git a/src/coolant.c b/src/coolant.c deleted file mode 100644 index 29cac81..0000000 --- a/src/coolant.c +++ /dev/null @@ -1,55 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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) { - if (x) OUTCLR_PIN(SWITCH_1_PIN); - else OUTSET_PIN(SWITCH_1_PIN); -} - - -void coolant_set_flood(bool x) { - if (x) OUTCLR_PIN(SWITCH_2_PIN); - else OUTSET_PIN(SWITCH_2_PIN); -} - - -bool coolant_get_mist() {return OUT_PIN(SWITCH_1_PIN);} -bool coolant_get_flood() {return OUT_PIN(SWITCH_2_PIN);} diff --git a/src/coolant.h b/src/coolant.h deleted file mode 100644 index f29166f..0000000 --- a/src/coolant.h +++ /dev/null @@ -1,37 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/cpp_magic.h b/src/cpp_magic.h deleted file mode 100644 index c7abc59..0000000 --- a/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/src/drv8711.c b/src/drv8711.c deleted file mode 100644 index f55d811..0000000 --- a/src/drv8711.c +++ /dev/null @@ -1,401 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "motor.h" - -#include -#include - -#include -#include -#include - - -#define DRIVERS MOTORS -#define COMMANDS 10 - - -#define DRV8711_WORD_BYTE_PTR(WORD, LOW) \ - (((uint8_t *)&(WORD)) + ((LOW) ? 0 : 1)) - - -bool motor_fault = false; - - -typedef struct { - uint8_t status; - - bool active; - float idle_current; - float drive_current; - float stall_threshold; - float power; - - uint8_t mode; // microstepping mode - stall_callback_t stall_cb; - - uint8_t cs_pin; - uint8_t stall_pin; -} drv8711_driver_t; - - -static drv8711_driver_t drivers[DRIVERS] = { - {.cs_pin = SPI_CS_X_PIN, .stall_pin = STALL_X_PIN}, - {.cs_pin = SPI_CS_Y_PIN, .stall_pin = STALL_Y_PIN}, - {.cs_pin = SPI_CS_Z_PIN, .stall_pin = STALL_Z_PIN}, - {.cs_pin = SPI_CS_A_PIN, .stall_pin = STALL_A_PIN}, -}; - - -typedef struct { - uint8_t *read; - bool callback; - uint8_t disable_cs_pin; - - uint8_t cmd; - uint8_t driver; - bool low_byte; - - uint8_t ncmds; - uint16_t commands[DRIVERS][COMMANDS]; - uint16_t responses[DRIVERS]; -} spi_t; - -static spi_t spi = {0}; - - -static void _driver_check_status(int driver) { - uint8_t status = drivers[driver].status; - uint8_t mflags = 0; - - if (status & DRV8711_STATUS_OTS_bm) mflags |= MOTOR_FLAG_OVER_TEMP_bm; - if (status & DRV8711_STATUS_AOCP_bm) mflags |= MOTOR_FLAG_OVER_CURRENT_bm; - if (status & DRV8711_STATUS_BOCP_bm) mflags |= MOTOR_FLAG_OVER_CURRENT_bm; - if (status & DRV8711_STATUS_APDF_bm) mflags |= MOTOR_FLAG_DRIVER_FAULT_bm; - if (status & DRV8711_STATUS_BPDF_bm) mflags |= MOTOR_FLAG_DRIVER_FAULT_bm; - if (status & DRV8711_STATUS_UVLO_bm) mflags |= MOTOR_FLAG_UNDER_VOLTAGE_bm; - if (status & DRV8711_STATUS_STD_bm) mflags |= MOTOR_FLAG_STALLED_bm; - if (status & DRV8711_STATUS_STDLAT_bm) mflags |= MOTOR_FLAG_STALLED_bm; - - //if (mflags) motor_error_callback(driver, mflags); TODO -} - - -static float _driver_get_current(int driver) { - drv8711_driver_t *drv = &drivers[driver]; - -#if 1 - if (!drv->active) return drv->idle_current; - return - MOTOR_MIN_CURRENT + (drv->drive_current - MOTOR_MIN_CURRENT) * drv->power; - -#else - return drv->active ? drv->drive_current : drv->idle_current; -#endif -} - - -static uint8_t _spi_next_command(uint8_t cmd) { - // Process status responses - for (int driver = 0; driver < DRIVERS; driver++) { - uint16_t command = spi.commands[driver][cmd]; - - if (DRV8711_CMD_IS_READ(command) && - DRV8711_CMD_ADDR(command) == DRV8711_STATUS_REG) { - drivers[driver].status = spi.responses[driver]; - _driver_check_status(driver); - } - } - - // Next command - if (++cmd == spi.ncmds) { - cmd = 0; // Wrap around - - for (int driver = 0; driver < DRIVERS; driver++) - motor_driver_callback(driver); - } - - // Prep next command - for (int driver = 0; driver < DRIVERS; driver++) { - uint16_t *command = &spi.commands[driver][cmd]; - - switch (DRV8711_CMD_ADDR(*command)) { - case DRV8711_STATUS_REG: - if (!DRV8711_CMD_IS_READ(*command)) - *command = (*command & 0xf000) | (0x0fff & ~(drivers[driver].status)); - break; - - case DRV8711_TORQUE_REG: // Update motor current setting - *command = (*command & 0xff00) | - (uint8_t)round(0xff * _driver_get_current(driver)); - break; - - case DRV8711_CTRL_REG: // Set microsteps - *command = (*command & 0xff87) | (drivers[driver].mode << 3); - break; - - default: break; - } - } - - return cmd; -} - - -static void _spi_send() { - // Flush any status errors (TODO check for errors) - uint8_t x = SPIC.STATUS; - x = x; - - // Disable CS - if (spi.disable_cs_pin) { - OUTCLR_PIN(spi.disable_cs_pin); // Set low (inactive) - _delay_us(1); - spi.disable_cs_pin = 0; - } - - // Schedule next CS disable or enable next CS now - if (spi.low_byte) spi.disable_cs_pin = drivers[spi.driver].cs_pin; - else { - OUTSET_PIN(drivers[spi.driver].cs_pin); // Set high (active) - _delay_us(1); - } - - // Read - if (spi.read) { - *spi.read = SPIC.DATA; - spi.read = 0; - } - - // Callback, passing current command index, and get next command index - if (spi.callback) { - spi.cmd = _spi_next_command(spi.cmd); - spi.callback = false; - } - - // Write byte and prep next read - SPIC.DATA = - *DRV8711_WORD_BYTE_PTR(spi.commands[spi.driver][spi.cmd], spi.low_byte); - spi.read = DRV8711_WORD_BYTE_PTR(spi.responses[spi.driver], spi.low_byte); - - // Check if WORD complete, go to next driver & check if command finished - if (spi.low_byte && ++spi.driver == DRIVERS) { - spi.driver = 0; // Wrap around - spi.callback = true; // Call back after last byte is read - } - - // Next byte - spi.low_byte = !spi.low_byte; -} - - -static void _init_spi_commands() { - // Setup SPI command sequence - for (int driver = 0; driver < DRIVERS; driver++) { - uint16_t *commands = spi.commands[driver]; - spi.ncmds = 0; - - // Set OFF - commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_OFF_REG, 12); - - // Set BLANK - commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_BLANK_REG, 0x80); - - // Set DECAY - commands[spi.ncmds++] = - DRV8711_WRITE(DRV8711_DECAY_REG, DRV8711_DECAY_DECMOD_AUTO_OPT | 6); - - // Set STALL - commands[spi.ncmds++] = - DRV8711_WRITE(DRV8711_STALL_REG, - DRV8711_STALL_SDCNT_2 | DRV8711_STALL_VDIV_8 | 200); - - // Set DRIVE - commands[spi.ncmds++] = - DRV8711_WRITE(DRV8711_DRIVE_REG, DRV8711_DRIVE_IDRIVEP_50 | - DRV8711_DRIVE_IDRIVEN_100 | DRV8711_DRIVE_TDRIVEP_500 | - DRV8711_DRIVE_TDRIVEN_500 | DRV8711_DRIVE_OCPDEG_2 | - DRV8711_DRIVE_OCPTH_500); - - // Set TORQUE - commands[spi.ncmds++] = - DRV8711_WRITE(DRV8711_TORQUE_REG, DRV8711_TORQUE_SMPLTH_50); - - // Set CTRL enable motor & set ISENSE gain - commands[spi.ncmds++] = - DRV8711_WRITE(DRV8711_CTRL_REG, DRV8711_CTRL_ENBL_bm | - DRV8711_CTRL_ISGAIN_10 | DRV8711_CTRL_DTIME_850); - - // Read STATUS - commands[spi.ncmds++] = DRV8711_READ(DRV8711_STATUS_REG); - - // Clear STATUS - commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_STATUS_REG, 0); - } - - if (COMMANDS < spi.ncmds) - STATUS_ERROR(STAT_INTERNAL_ERROR, - "SPI command buffer overflow increase COMMANDS in %s", - __FILE__); - - _spi_send(); // Kick it off -} - - -ISR(SPIC_INT_vect) {_spi_send();} - - -ISR(STALL_ISR_vect) { - for (int i = 0; i < DRIVERS; i++) { - drv8711_driver_t *driver = &drivers[i]; - if (driver->stall_cb) driver->stall_cb(i); - } -} - - -ISR(FAULT_ISR_vect) {motor_fault = !IN_PIN(MOTOR_FAULT_PIN);} // TODO - - -void drv8711_init() { - // Configure drivers - for (int i = 0; i < DRIVERS; i++) { - drivers[i].idle_current = MOTOR_IDLE_CURRENT; - drivers[i].drive_current = MOTOR_MAX_CURRENT; - drivers[i].stall_threshold = MOTOR_STALL_THRESHOLD; - - drv8711_disable(i); - } - - // Setup pins - // Must set the SS pin either in/high or any/output for master mode to work - // Note, this pin is also used by the USART as the CTS line - DIRSET_PIN(SPI_SS_PIN); // Output - OUTSET_PIN(SPI_CLK_PIN); // High - DIRSET_PIN(SPI_CLK_PIN); // Output - DIRCLR_PIN(SPI_MISO_PIN); // Input - OUTSET_PIN(SPI_MOSI_PIN); // High - DIRSET_PIN(SPI_MOSI_PIN); // Output - - for (int i = 0; i < DRIVERS; i++) { - uint8_t cs_pin = drivers[i].cs_pin; - uint8_t stall_pin = drivers[i].stall_pin; - - OUTSET_PIN(cs_pin); // High - DIRSET_PIN(cs_pin); // Output - DIRCLR_PIN(stall_pin); // Input - - // Stall interrupt - PINCTRL_PIN(stall_pin) = PORT_ISC_FALLING_gc; - PORT(stall_pin)->INT1MASK |= BM(stall_pin); - PORT(stall_pin)->INTCTRL |= PORT_INT1LVL_HI_gc; - } - - // Fault interrupt - DIRCLR_PIN(MOTOR_FAULT_PIN); - PINCTRL_PIN(MOTOR_FAULT_PIN) = PORT_ISC_RISING_gc; - PORT(MOTOR_FAULT_PIN)->INT1MASK |= BM(MOTOR_FAULT_PIN); - PORT(MOTOR_FAULT_PIN)->INTCTRL |= PORT_INT1LVL_HI_gc; - - // Configure SPI - PR.PRPC &= ~PR_SPI_bm; // Disable power reduction - SPIC.CTRL = SPI_ENABLE_bm | SPI_MASTER_bm | SPI_MODE_0_gc | - SPI_PRESCALER_DIV16_gc; // enable, big endian, master, mode, clock div - PORT(SPI_CLK_PIN)->REMAP = PORT_SPI_bm; // Swap SCK and MOSI - SPIC.INTCTRL = SPI_INTLVL_LO_gc; // interupt level - - _init_spi_commands(); -} - - -void drv8711_enable(int driver) { - if (driver < 0 || DRIVERS <= driver) return; - drivers[driver].active = true; -} - - -void drv8711_disable(int driver) { - if (driver < 0 || DRIVERS <= driver) return; - drivers[driver].active = false; -} - - -void drv8711_set_power(int driver, float power) { - if (driver < 0 || DRIVERS <= driver) return; - drivers[driver].power = power < 0 ? 0 : (1 < power ? 1 : power); -} - - -void drv8711_set_microsteps(int driver, uint16_t msteps) { - if (driver < 0 || DRIVERS <= driver) return; - switch (msteps) { - case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256: - break; - default: return; // Invalid - } - - drivers[driver].mode = round(logf(msteps) / logf(2)); -} - - -void drv8711_set_stall_callback(int driver, stall_callback_t cb) { - drivers[driver].stall_cb = cb; -} - - -float get_drive_power(int driver) { - if (driver < 0 || DRIVERS <= driver) return 0; - return drivers[driver].drive_current; -} - - -void set_drive_power(int driver, float value) { - if (driver < 0 || DRIVERS <= driver || value < 0 || 1 < value) return; - drivers[driver].drive_current = value; -} - - -float get_idle_power(int driver) { - if (driver < 0 || DRIVERS <= driver) return 0; - return drivers[driver].idle_current; -} - - -void set_idle_power(int driver, float value) { - if (driver < 0 || DRIVERS <= driver || value < 0 || 1 < value) return; - drivers[driver].idle_current = value; -} - - -float get_current_power(int driver) { - if (driver < 0 || DRIVERS <= driver) return 0; - return _driver_get_current(driver); -} - - -bool get_motor_fault() {return motor_fault;} diff --git a/src/drv8711.h b/src/drv8711.h deleted file mode 100644 index 187c88a..0000000 --- a/src/drv8711.h +++ /dev/null @@ -1,169 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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, -}; - - -#define DRV8711_READ(ADDR) ((1 << 15) | ((ADDR) << 12)) -#define DRV8711_WRITE(ADDR, DATA) (((ADDR) << 12) | ((DATA) & 0xfff)) -#define DRV8711_CMD_ADDR(CMD) (((CMD) >> 12) & 7) -#define DRV8711_CMD_IS_READ(CMD) ((1 << 15) & (CMD)) - - -void drv8711_init(); -void drv8711_enable(int driver); -void drv8711_disable(int driver); -void drv8711_set_power(int driver, float power); -void drv8711_set_microsteps(int driver, uint16_t msteps); -void drv8711_set_stall_callback(int driver, stall_callback_t cb); diff --git a/src/estop.c b/src/estop.c deleted file mode 100644 index 5f533ae..0000000 --- a/src/estop.c +++ /dev/null @@ -1,147 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "homing.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 - if (estop.triggered) OUTSET_PIN(FAULT_PIN); // High - else OUTCLR_PIN(FAULT_PIN); // Low - DIRSET_PIN(FAULT_PIN); // Output -} - - -bool estop_triggered() { - return estop.triggered || switch_is_active(SW_ESTOP); -} - - -void estop_trigger(stat_t reason) { - if (estop.triggered) return; - estop.triggered = true; - - // Hard stop the motors and the spindle - st_shutdown(); - spindle_estop(); - - // Set machine state - mp_state_estop(); - - // Set axes not homed - mach_set_not_homed(); - - // Save reason - _set_reason(reason); - - report_request(); -} - - -void estop_clear() { - // Check if estop switch is set - if (switch_is_active(SW_ESTOP)) { - if (_get_reason() != STAT_ESTOP_SWITCH) _set_reason(STAT_ESTOP_SWITCH); - return; // Can't clear while estop switch is still active - } - - // Clear fault signal - OUTCLR_PIN(FAULT_PIN); // Low - - estop.triggered = false; - - // Clear reason - _set_reason(STAT_OK); - - // Reboot - // Note, hardware.c waits until any spindle stop command has been delivered - hw_request_hard_reset(); -} - - -bool get_estop() { - return estop_triggered(); -} - - -void set_estop(bool value) { - if (value == estop_triggered()) return; - if (value) estop_trigger(STAT_ESTOP_USER); - else estop_clear(); -} - - -PGM_P get_estop_reason() { - return status_to_pgmstr(_get_reason()); -} diff --git a/src/estop.h b/src/estop.h deleted file mode 100644 index 55fdec4..0000000 --- a/src/estop.h +++ /dev/null @@ -1,38 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/gcode_parser.c b/src/gcode_parser.c deleted file mode 100644 index 0803507..0000000 --- a/src/gcode_parser.c +++ /dev/null @@ -1,569 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "machine.h" -#include "plan/arc.h" -#include "probing.h" -#include "homing.h" -#include "axis.h" -#include "util.h" - -#include -#include -#include -#include -#include - - -parser_t parser = {{0}}; - - -#define SET_MODAL(m, parm, val) \ - {parser.gn.parm = val; parser.gf.parm = true; modals[m] += 1; break;} -#define SET_NON_MODAL(parm, val) \ - {parser.gn.parm = val; parser.gf.parm = true; break;} -#define EXEC_FUNC(f, parm) if (parser.gf.parm) f(parser.gn.parm) - - -static uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block - - -/* Normalize a block (line) of gcode in place - * - * Normalization functions: - * - convert all letters to upper case - * - remove white space, control and other invalid characters - * - remove (erroneous) leading zeros that might be taken to mean Octal - * - identify and return start of comments and messages - * - signal if a block-delete character (/) was encountered in the first space - * - * So this: " g1 x100 Y100 f400" becomes this: "G1X100Y100F400" - * - * Comment and message handling: - * - Comments field start with a '(' char or alternately a semicolon ';' - * - Comments and messages are not normalized - they are left alone - * - The 'MSG' specifier in comment can have mixed case but cannot cannot - * have embedded white spaces - * - Normalization returns true if there was a message to display, false - * otherwise - * - Comments always terminate the block - i.e. leading or embedded comments - * are not supported - * - Valid cases (examples) Notes: - * G0X10 - command only - no comment - * (comment text) - There is no command on this line - * G0X10 (comment text) - * G0X10 (comment text - It's OK to drop the trailing paren - * G0X10 ;comment text - It's OK to drop the trailing paren - * - * - Invalid cases (examples) Notes: - * G0X10 comment text - Comment with no separator - * N10 (comment) G0X10 - embedded comment. G0X10 will be ignored - * (comment) G0X10 - leading comment. G0X10 will be ignored - * G0X10 # comment - invalid separator - * - * Returns: - * - com points to comment string or to 0 if no comment - * - msg points to message string or to 0 if no comment - * - block_delete_flag is set true if block delete encountered, false otherwise - */ - -static void _normalize_gcode_block(char *str, char **com, char **msg, - uint8_t *block_delete_flag) { - char *rd = str; // read pointer - char *wr = str; // write pointer - - // mark block deletes - *block_delete_flag = *rd == '/'; - - // normalize the command block & find the comment (if any) - for (; *wr; rd++) - if (!*rd) *wr = 0; - else if (*rd == '(' || *rd == ';') {*wr = 0; *com = rd + 1;} - else if (isalnum(*rd) || strchr("-.", *rd)) // all valid characters - *wr++ = toupper(*rd); - - // Perform Octal stripping - remove invalid leading zeros in number strings - rd = str; - while (*rd) { - if (*rd == '.') break; // don't strip past a decimal point - if (!isdigit(*rd) && *(rd + 1) == '0' && isdigit(*(rd + 2))) { - wr = rd + 1; - while (*wr) {*wr = *(wr + 1); wr++;} // copy forward w/overwrite - continue; - } - - rd++; - } - - // process comments and messages - if (**com) { - rd = *com; - while (isspace(*rd)) rd++; // skip any leading spaces before "msg" - - if (tolower(*rd) == 'm' && tolower(*(rd + 1)) == 's' && - tolower(*(rd + 2)) == 'g') - *msg = rd + 3; - - for (; *rd; rd++) - // 0 terminate on trailing parenthesis, if any - if (*rd == ')') *rd = 0; - } -} - - -/* Get gcode word consisting of a letter and a value - * - * This function requires the Gcode string to be normalized. - * Normalization must remove any leading zeros or they will be converted to - * octal. G0X... is not interpreted as hexadecimal. This is trapped. - */ -static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) { - if (!**pstr) return STAT_COMPLETE; // no more words to process - - // get letter part - if (!isupper(**pstr)) return STAT_INVALID_OR_MALFORMED_COMMAND; - *letter = **pstr; - (*pstr)++; - - // X-axis-becomes-a-hexadecimal-number get-value case, e.g. G0X100 --> G255 - if (**pstr == '0' && *(*pstr + 1) == 'X') { - *value = 0; - (*pstr)++; - return STAT_OK; // pointer points to X - } - - // get-value general case - char *end; - *value = strtod(*pstr, &end); - // more robust test then checking for value == 0 - if (end == *pstr) return STAT_GCODE_COMMAND_UNSUPPORTED; - *pstr = end; // pointer points to next character after the word - - return STAT_OK; -} - - -/// Isolate the decimal point value as an integer -static uint8_t _point(float value) {return value * 10 - trunc(value) * 10;} - - -#if 0 -static bool _axis_changed() { - for (int axis = 0; axis < AXES; axis++) - if (parser.gf.target[axis]) return true; - return false; -} -#endif - - -/// Check for some gross Gcode block semantic violations -static stat_t _validate_gcode_block() { - // Check for modal group violations. From NIST, section 3.4 "It - // is an error to put a G-code from group 1 and a G-code from - // group 0 on the same line if both of them use axis words. If an - // axis word-using G-code from group 1 is implicitly in effect on - // a line (by having been activated on an earlier line), and a - // group 0 G-code that uses axis words appears on the line, the - // activity of the group 1 G-code is suspended for that line. The - // axis word-using G-codes from group 0 are G10, G28, G30, and G92" - - if (modals[MODAL_GROUP_G0] && modals[MODAL_GROUP_G1]) - return STAT_MODAL_GROUP_VIOLATION; - -#if 0 // This check fails for arcs which may have offsets but no axis word - // look for commands that require an axis word to be present - if (modals[MODAL_GROUP_G0] || modals[MODAL_GROUP_G1]) - if (!_axis_changed()) return STAT_GCODE_AXIS_IS_MISSING; -#endif - - return STAT_OK; -} - - -/* Execute parsed block - * - * Conditionally (based on whether a flag is set in gf) call the - * machining functions in order of execution as per RS274NGC_3 table 8 - * (below, with modifications): - * - * 0. record the line number - * 1. comment (includes message) [handled during block normalization] - * 2. set feed rate mode (G93, G94 - inverse time or per minute) - * 3. set feed rate (F) - * 3a. set feed override rate (M50) - * 4. set spindle speed (S) - * 4a. set spindle override rate (M51) - * 5. select tool (T) - * 6. change tool (M6) - * 7. spindle on or off (M3, M4, M5) - * 8. coolant on or off (M7, M8, M9) - * 9. enable or disable overrides (M48, M49) - * 10. dwell (G4) - * 11. set active plane (G17, G18, G19) - * 12. set length units (G20, G21) - * 13. cutter radius compensation on or off (G40, G41, G42) - * 14. cutter length compensation on or off (G43, G49) - * 15. coordinate system selection (G54, G55, G56, G57, G58, G59) - * 16. set path control mode (G61, G61.1, G64) - * 17. set distance mode (G90, G91, G90.1, G91.1) - * 18. set retract mode (G98, G99) - * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30) - * 19b. update system data (G10) - * 19c. set axis offsets (G92, G92.1, G92.2, G92.3) - * 20. perform motion (G0 to G3, G80-G89) as modified (possibly) by G53 - * 21. stop and end (M0, M1, M2, M30, M60) - * - * Values in gn are in original units and should not be unit converted prior - * to calling the machine functions (which does the unit conversions) - */ -static stat_t _execute_gcode_block() { - stat_t status = STAT_OK; - - mach_set_line(parser.gn.line); - EXEC_FUNC(mach_set_feed_mode, feed_mode); - EXEC_FUNC(mach_set_feed_rate, feed_rate); - EXEC_FUNC(mach_feed_override_enable, feed_override_enable); - EXEC_FUNC(mach_set_spindle_speed, spindle_speed); - EXEC_FUNC(mach_spindle_override_enable, spindle_override_enable); - EXEC_FUNC(mach_select_tool, tool); - EXEC_FUNC(mach_change_tool, tool_change); - EXEC_FUNC(mach_set_spindle_mode, spindle_mode); - EXEC_FUNC(mach_mist_coolant_control, mist_coolant); - EXEC_FUNC(mach_flood_coolant_control, flood_coolant); - EXEC_FUNC(mach_override_enables, override_enables); - - if (parser.gn.next_action == NEXT_ACTION_DWELL) // G4 - dwell - RITORNO(mach_dwell(parser.gn.parameter)); - - EXEC_FUNC(mach_set_plane, plane); - EXEC_FUNC(mach_set_units, units); - //--> cutter radius compensation goes here - //--> cutter length compensation goes here - EXEC_FUNC(mach_set_coord_system, coord_system); - EXEC_FUNC(mach_set_path_mode, path_mode); - EXEC_FUNC(mach_set_distance_mode, distance_mode); - EXEC_FUNC(mach_set_arc_distance_mode, arc_distance_mode); - //--> set retract mode goes here - - switch (parser.gn.next_action) { - case NEXT_ACTION_SET_G28_POSITION: // G28.1 - mach_set_g28_position(); - break; - case NEXT_ACTION_GOTO_G28_POSITION: // G28 - status = mach_goto_g28_position(parser.gn.target, parser.gf.target); - break; - case NEXT_ACTION_SET_G30_POSITION: // G30.1 - mach_set_g30_position(); - break; - case NEXT_ACTION_GOTO_G30_POSITION: // G30 - status = mach_goto_g30_position(parser.gn.target, parser.gf.target); - break; - case NEXT_ACTION_SEARCH_HOME: // G28.2 - mach_homing_cycle_start(); - break; - case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: // G28.3 - mach_set_absolute_origin(parser.gn.target, parser.gf.target); - break; - case NEXT_ACTION_HOMING_NO_SET: // G28.4 - mach_homing_cycle_start_no_set(); - break; - case NEXT_ACTION_STRAIGHT_PROBE: // G38.2 - status = mach_probe(parser.gn.target, parser.gf.target); - break; - case NEXT_ACTION_SET_COORD_DATA: - mach_set_coord_offsets(parser.gn.parameter, parser.gn.target, - parser.gf.target); - break; - case NEXT_ACTION_SET_ORIGIN_OFFSETS: - mach_set_origin_offsets(parser.gn.target, parser.gf.target); - break; - case NEXT_ACTION_RESET_ORIGIN_OFFSETS: - mach_reset_origin_offsets(); - break; - case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: - mach_suspend_origin_offsets(); - break; - case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: - mach_resume_origin_offsets(); - break; - case NEXT_ACTION_DWELL: break; // Handled above - - case NEXT_ACTION_DEFAULT: - // apply override setting to gm struct - mach_set_absolute_mode(parser.gn.absolute_mode); - - switch (parser.gn.motion_mode) { - case MOTION_MODE_CANCEL_MOTION_MODE: - mach_set_motion_mode(parser.gn.motion_mode); - break; - case MOTION_MODE_RAPID: - status = mach_rapid(parser.gn.target, parser.gf.target); - break; - case MOTION_MODE_FEED: - status = mach_feed(parser.gn.target, parser.gf.target); - break; - case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: - // gf.radius sets radius mode if radius was collected in gn - status = mach_arc_feed(parser.gn.target, parser.gf.target, - parser.gn.arc_offset, parser.gf.arc_offset, - parser.gn.arc_radius, parser.gf.arc_radius, - parser.gn.parameter, parser.gf.parameter, - modals[MODAL_GROUP_G1], parser.gn.motion_mode); - break; - default: break; // Should not get here - } - } - // un-set absolute override once the move is planned - mach_set_absolute_mode(false); - - // do the program stops and ends : M0, M1, M2, M30, M60 - if (parser.gf.program_flow) - switch (parser.gn.program_flow) { - case PROGRAM_STOP: mach_program_stop(); break; - case PROGRAM_OPTIONAL_STOP: mach_optional_program_stop(); break; - case PROGRAM_PALLET_CHANGE_STOP: mach_pallet_change_stop(); break; - case PROGRAM_END: mach_program_end(); break; - } - - return status; -} - - -/* Parses one line of 0 terminated G-Code. - * - * All the parser does is load the state values in gn (next model - * state) and set flags in gf (model state flags). The execute - * routine applies them. The buffer is assumed to contain only - * uppercase characters and signed floats (no whitespace). - * - * A number of implicit things happen when the gn struct is zeroed: - * - inverse feed rate mode is canceled - set back to units_per_minute mode - */ -static stat_t _parse_gcode_block(char *buf) { - char *pstr = buf; // persistent pointer for parsing words - char letter; // parsed letter, eg.g. G or X or Y - float value = 0; // value parsed from letter (e.g. 2 for G2) - stat_t status = STAT_OK; - - // set initial state for new move - memset(modals, 0, sizeof(modals)); // clear all parser values - memset(&parser.gf, 0, sizeof(gcode_flags_t)); // clear all next-state flags - memset(&parser.gn, 0, sizeof(gcode_state_t)); // clear all next-state values - - // get motion mode from previous block - parser.gn.motion_mode = mach_get_motion_mode(); - - // extract commands and parameters - while ((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) { - switch (letter) { - case 'G': - switch ((uint8_t)value) { - case 0: - SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_RAPID); - case 1: - SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_FEED); - case 2: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC); - case 3: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC); - case 4: SET_NON_MODAL(next_action, NEXT_ACTION_DWELL); - case 10: - SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA); - case 17: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XY); - case 18: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XZ); - case 19: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_YZ); - case 20: SET_MODAL(MODAL_GROUP_G6, units, INCHES); - case 21: SET_MODAL(MODAL_GROUP_G6, units, MILLIMETERS); - case 28: - switch (_point(value)) { - case 0: - SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G28_POSITION); - case 1: - SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G28_POSITION); - case 2: SET_NON_MODAL(next_action, NEXT_ACTION_SEARCH_HOME); - case 3: SET_NON_MODAL(next_action, NEXT_ACTION_SET_ABSOLUTE_ORIGIN); - case 4: SET_NON_MODAL(next_action, NEXT_ACTION_HOMING_NO_SET); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 30: - switch (_point(value)) { - case 0: - SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G30_POSITION); - case 1: - SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G30_POSITION); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 38: - switch (_point(value)) { - case 2: SET_NON_MODAL(next_action, NEXT_ACTION_STRAIGHT_PROBE); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 40: break; // ignore cancel cutter radius compensation - case 49: break; // ignore cancel tool length offset comp. - case 53: SET_NON_MODAL(absolute_mode, true); - case 54: SET_MODAL(MODAL_GROUP_G12, coord_system, G54); - case 55: SET_MODAL(MODAL_GROUP_G12, coord_system, G55); - case 56: SET_MODAL(MODAL_GROUP_G12, coord_system, G56); - case 57: SET_MODAL(MODAL_GROUP_G12, coord_system, G57); - case 58: SET_MODAL(MODAL_GROUP_G12, coord_system, G58); - case 59: SET_MODAL(MODAL_GROUP_G12, coord_system, G59); - case 61: - switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_PATH); - case 1: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_STOP); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 64: SET_MODAL(MODAL_GROUP_G13,path_mode, PATH_CONTINUOUS); - case 80: SET_MODAL(MODAL_GROUP_G1, motion_mode, - MOTION_MODE_CANCEL_MOTION_MODE); - // case 90: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); - // case 91: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); - case 90: - switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); - case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, ABSOLUTE_MODE); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 91: - switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); - case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, INCREMENTAL_MODE); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 92: - switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G0, next_action, - NEXT_ACTION_SET_ORIGIN_OFFSETS); - case 1: SET_NON_MODAL(next_action, NEXT_ACTION_RESET_ORIGIN_OFFSETS); - case 2: SET_NON_MODAL(next_action, NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS); - case 3: SET_NON_MODAL(next_action, NEXT_ACTION_RESUME_ORIGIN_OFFSETS); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 93: SET_MODAL(MODAL_GROUP_G5, feed_mode, INVERSE_TIME_MODE); - case 94: SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_MINUTE_MODE); - // case 95: - // SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_REVOLUTION_MODE); - // case 96: // Spindle Constant Surface Speed (not currently supported) - case 97: break; // Spindle RPM mode (only mode curently supported) - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 'M': - switch ((uint8_t)value) { - case 0: - SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_STOP); - case 1: - SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_OPTIONAL_STOP); - case 60: - SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_PALLET_CHANGE_STOP); - case 2: case 30: - SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_END); - case 3: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_CW); - case 4: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_CCW); - case 5: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_OFF); - case 6: SET_NON_MODAL(tool_change, true); - case 7: SET_MODAL(MODAL_GROUP_M8, mist_coolant, true); - case 8: SET_MODAL(MODAL_GROUP_M8, flood_coolant, true); - case 9: SET_MODAL(MODAL_GROUP_M8, flood_coolant, false); // Also mist - case 48: SET_MODAL(MODAL_GROUP_M9, override_enables, true); - case 49: SET_MODAL(MODAL_GROUP_M9, override_enables, false); - case 50: SET_MODAL(MODAL_GROUP_M9, feed_override_enable, true); - case 51: SET_MODAL(MODAL_GROUP_M9, spindle_override_enable, true); - default: status = STAT_MCODE_COMMAND_UNSUPPORTED; - } - break; - - case 'T': SET_NON_MODAL(tool, (uint8_t)trunc(value)); - case 'F': SET_NON_MODAL(feed_rate, value); - // used for dwell time, G10 coord select, rotations - case 'P': SET_NON_MODAL(parameter, value); - case 'S': SET_NON_MODAL(spindle_speed, value); - case 'X': SET_NON_MODAL(target[AXIS_X], value); - case 'Y': SET_NON_MODAL(target[AXIS_Y], value); - case 'Z': SET_NON_MODAL(target[AXIS_Z], value); - case 'A': SET_NON_MODAL(target[AXIS_A], value); - case 'B': SET_NON_MODAL(target[AXIS_B], value); - case 'C': SET_NON_MODAL(target[AXIS_C], value); - // case 'U': SET_NON_MODAL(target[AXIS_U], value); // reserved - // case 'V': SET_NON_MODAL(target[AXIS_V], value); // reserved - // case 'W': SET_NON_MODAL(target[AXIS_W], value); // reserved - case 'I': SET_NON_MODAL(arc_offset[0], value); - case 'J': SET_NON_MODAL(arc_offset[1], value); - case 'K': SET_NON_MODAL(arc_offset[2], value); - case 'R': SET_NON_MODAL(arc_radius, value); - case 'N': SET_NON_MODAL(line, (uint32_t)value); // line number - case 'L': break; // not used for anything - case 0: break; - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - - if (status != STAT_OK) break; - } - - if (status != STAT_OK && status != STAT_COMPLETE) return status; - RITORNO(_validate_gcode_block()); - - return _execute_gcode_block(); // if successful execute the block -} - - -/// Parse a block (line) of gcode -/// Top level of gcode parser. Normalizes block and looks for special cases -stat_t gc_gcode_parser(char *block) { - char *str = block; // gcode command or 0 string - char none = 0; - char *com = &none; // gcode comment or 0 string - char *msg = &none; // gcode message or 0 string - uint8_t block_delete_flag; - - _normalize_gcode_block(str, &com, &msg, &block_delete_flag); - - // Block delete omits the line if a / char is present in the first space - // For now this is unconditional and will always delete - if (block_delete_flag) return STAT_NOOP; - - // queue a "(MSG" response - if (*msg) mach_message(msg); // queue the message - - return _parse_gcode_block(block); -} diff --git a/src/gcode_parser.h b/src/gcode_parser.h deleted file mode 100644 index 4d729a0..0000000 --- a/src/gcode_parser.h +++ /dev/null @@ -1,67 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 struct { - gcode_state_t gn; // gcode input values - gcode_flags_t gf; // gcode input flags -} parser_t; - - -extern parser_t parser; - - -stat_t gc_gcode_parser(char *block); diff --git a/src/gcode_state.c b/src/gcode_state.c deleted file mode 100644 index c368b29..0000000 --- a/src/gcode_state.c +++ /dev/null @@ -1,97 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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" - - -PGM_P gs_get_units_pgmstr(units_t mode) { - switch (mode) { - case INCHES: return PSTR("IN"); - case MILLIMETERS: return PSTR("MM"); - case DEGREES: return PSTR("DEG"); - } - - return PSTR("INVALID"); -} - - -PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode) { - switch (mode) { - case INVERSE_TIME_MODE: return PSTR("INVERSE TIME"); - case UNITS_PER_MINUTE_MODE: return PSTR("PER MIN"); - case UNITS_PER_REVOLUTION_MODE: return PSTR("PER REV"); - } - - return PSTR("INVALID"); -} - - -PGM_P gs_get_plane_pgmstr(plane_t plane) { - switch (plane) { - case PLANE_XY: return PSTR("XY"); - case PLANE_XZ: return PSTR("XZ"); - case PLANE_YZ: return PSTR("YZ"); - } - - return PSTR("INVALID"); -} - - -PGM_P gs_get_coord_system_pgmstr(coord_system_t cs) { - switch (cs) { - case ABSOLUTE_COORDS: return PSTR("ABS"); - case G54: return PSTR("G54"); - case G55: return PSTR("G55"); - case G56: return PSTR("G56"); - case G57: return PSTR("G57"); - case G58: return PSTR("G58"); - case G59: return PSTR("G59"); - } - - return PSTR("INVALID"); -} - - -PGM_P gs_get_path_mode_pgmstr(path_mode_t mode) { - switch (mode) { - case PATH_EXACT_PATH: return PSTR("EXACT PATH"); - case PATH_EXACT_STOP: return PSTR("EXACT STOP"); - case PATH_CONTINUOUS: return PSTR("CONTINUOUS"); - } - - return PSTR("INVALID"); -} - - -PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode) { - switch (mode) { - case ABSOLUTE_MODE: return PSTR("ABSOLUTE"); - case INCREMENTAL_MODE: return PSTR("INCREMENTAL"); - } - - return PSTR("INVALID"); -} diff --git a/src/gcode_state.def b/src/gcode_state.def deleted file mode 100644 index aa4d042..0000000 --- a/src/gcode_state.def +++ /dev/null @@ -1,69 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/gcode_state.h b/src/gcode_state.h deleted file mode 100644 index ff3525b..0000000 --- a/src/gcode_state.h +++ /dev/null @@ -1,204 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - - -/* The difference between next_action_t and motion_mode_t is that - * next_action_t is used by the current block, and may carry non-modal - * commands, whereas motion_mode_t persists across blocks as G modal group 1 - */ - -/// these are in order to optimized CASE statement -typedef enum { - NEXT_ACTION_DEFAULT, // Must be zero (invokes motion modes) - NEXT_ACTION_SEARCH_HOME, // G28.2 homing cycle - NEXT_ACTION_SET_ABSOLUTE_ORIGIN, // G28.3 origin set - NEXT_ACTION_HOMING_NO_SET, // G28.4 homing cycle no coord setting - NEXT_ACTION_SET_G28_POSITION, // G28.1 set position in abs coordinates - NEXT_ACTION_GOTO_G28_POSITION, // G28 go to machine position - NEXT_ACTION_SET_G30_POSITION, // G30.1 - NEXT_ACTION_GOTO_G30_POSITION, // G30 - NEXT_ACTION_SET_COORD_DATA, // G10 - NEXT_ACTION_SET_ORIGIN_OFFSETS, // G92 - NEXT_ACTION_RESET_ORIGIN_OFFSETS, // G92.1 - NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS, // G92.2 - NEXT_ACTION_RESUME_ORIGIN_OFFSETS, // G92.3 - NEXT_ACTION_DWELL, // G4 - NEXT_ACTION_STRAIGHT_PROBE, // G38.2 -} next_action_t; - - -typedef enum { // G Modal Group 1 - MOTION_MODE_RAPID, // G0 - rapid - MOTION_MODE_FEED, // G1 - straight feed - MOTION_MODE_CW_ARC, // G2 - clockwise arc feed - MOTION_MODE_CCW_ARC, // G3 - counter-clockwise arc feed - MOTION_MODE_CANCEL_MOTION_MODE, // G80 - MOTION_MODE_STRAIGHT_PROBE, // G38.2 - MOTION_MODE_CANNED_CYCLE_81, // G81 - drilling - MOTION_MODE_CANNED_CYCLE_82, // G82 - drilling with dwell - MOTION_MODE_CANNED_CYCLE_83, // G83 - peck drilling - MOTION_MODE_CANNED_CYCLE_84, // G84 - right hand tapping - MOTION_MODE_CANNED_CYCLE_85, // G85 - boring, no dwell, feed out - MOTION_MODE_CANNED_CYCLE_86, // G86 - boring, spindle stop, rapid out - MOTION_MODE_CANNED_CYCLE_87, // G87 - back boring - MOTION_MODE_CANNED_CYCLE_88, // G88 - boring, spindle stop, manual out - MOTION_MODE_CANNED_CYCLE_89, // G89 - boring, dwell, feed out -} motion_mode_t; - - -typedef enum { // plane - translates to: - // axis_0 axis_1 axis_2 - PLANE_XY, // G17 X Y Z - PLANE_XZ, // G18 X Z Y - PLANE_YZ, // G19 Y Z X -} plane_t; - - -typedef enum { - INCHES, // G20 - MILLIMETERS, // G21 - DEGREES, // ABC axes (this value used for displays only) -} units_t; - - -typedef enum { - ABSOLUTE_COORDS, // machine coordinate system - G54, G55, G56, G57, G58, G59, -} coord_system_t; - - -/// G Modal Group 13 -typedef enum { - PATH_EXACT_PATH, // G61 hits corners but stops only if needed - PATH_EXACT_STOP, // G61.1 stops at all corners - PATH_CONTINUOUS, // G64 and typically the default mode -} path_mode_t; - - -typedef enum { - ABSOLUTE_MODE, // G90 - INCREMENTAL_MODE, // G91 -} distance_mode_t; - - -typedef enum { - INVERSE_TIME_MODE, // G93 - UNITS_PER_MINUTE_MODE, // G94 - UNITS_PER_REVOLUTION_MODE, // G95 (unimplemented) -} feed_mode_t; - - -typedef enum { - ORIGIN_OFFSET_SET, // G92 - set origin offsets - ORIGIN_OFFSET_CANCEL, // G92.1 - zero out origin offsets - ORIGIN_OFFSET_SUSPEND, // G92.2 - do not apply offsets, but preserve values - ORIGIN_OFFSET_RESUME, // G92.3 - resume application of the suspended offsets -} origin_offset_t; - - -typedef enum { - PROGRAM_STOP, - PROGRAM_OPTIONAL_STOP, - PROGRAM_PALLET_CHANGE_STOP, - PROGRAM_END, -} program_flow_t; - - -/// spindle state settings -typedef enum { - SPINDLE_OFF, - SPINDLE_CW, - SPINDLE_CCW, -} spindle_mode_t; - - -/// mist and flood coolant states -typedef enum { - COOLANT_OFF, // all coolant off - COOLANT_ON, // request coolant on or indicate both coolants are on - COOLANT_MIST, // indicates mist coolant on - COOLANT_FLOOD, // indicates flood coolant on -} coolant_state_t; - - -/// used for spindle and arc dir -typedef enum { - DIRECTION_CW, - DIRECTION_CCW, -} direction_t; - - -/* Gcode model - * - * - mach.gm is the core Gcode model state. It keeps the internal gcode - * state model in normalized, canonical form. All values are unit - * converted (to mm) and in the machine coordinate system - * (absolute coordinate system). Gm is owned by the machine layer and - * should be accessed only through mach_*() routines. - * - * - parser.gn is used by the gcode parser and is re-initialized for - * each gcode block. It accepts data in the new gcode block in the - * formats present in the block (pre-normalized forms). During - * initialization some state elements are necessarily restored - * from gm. - * - * - parser.gf is used by the gcode parser to hold flags for any data that has - * changed in gn during the parse. - */ - - -/// Gcode model state -typedef struct { -#define MEMBER(TYPE, VAR) TYPE VAR; -#include "gcode_state.def" -#undef MEMBER -} gcode_state_t; - - -typedef struct { -#define MEMBER(TYPE, VAR) bool VAR; -#include "gcode_state.def" -#undef MEMBER -} gcode_flags_t; - - -PGM_P gs_get_units_pgmstr(units_t mode); -PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode); -PGM_P gs_get_plane_pgmstr(plane_t plane); -PGM_P gs_get_coord_system_pgmstr(coord_system_t cs); -PGM_P gs_get_path_mode_pgmstr(path_mode_t mode); -PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode); diff --git a/src/hardware.c b/src/hardware.c deleted file mode 100644 index 65c5c8f..0000000 --- a/src/hardware.c +++ /dev/null @@ -1,196 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 -#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] - - -/// This routine is lifted and modified from Boston Android and from -/// http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659 -static void _init_clock() { -#if defined(__CLOCK_EXTERNAL_8MHZ) // external 8 Mhx Xtal w/ 4x PLL = 32 Mhz - // 2-9 MHz crystal; 0.4-16 MHz XTAL w/ 16K CLK startup - OSC.XOSCCTRL = OSC_FRQRANGE_2TO9_gc | OSC_XOSCSEL_XTAL_16KCLK_gc; - OSC.CTRL = OSC_XOSCEN_bm; // enable external crystal oscillator - while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready - - OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 4; // PLL source, 4x (32 MHz sys clock) - OSC.CTRL = OSC_PLLEN_bm | OSC_XOSCEN_bm; // Enable PLL & External Oscillator - while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready - - CCP = CCP_IOREG_gc; - CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock - - OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock - -#elif defined(__CLOCK_EXTERNAL_16MHZ) // external 16Mhz Xtal w/ 2x PLL = 32 Mhz - // 12-16 MHz crystal; 0.4-16 MHz XTAL w/ 16K CLK startup - OSC.XOSCCTRL = OSC_FRQRANGE_12TO16_gc | OSC_XOSCSEL_XTAL_16KCLK_gc; - OSC.CTRL = OSC_XOSCEN_bm; // enable external crystal oscillator - while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready - - OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 2; // PLL source, 2x (32 MHz sys clock) - OSC.CTRL = OSC_PLLEN_bm | OSC_XOSCEN_bm; // Enable PLL & External Oscillator - while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready - - CCP = CCP_IOREG_gc; - CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL clock - - OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock - -#elif defined(__CLOCK_INTERNAL_32MHZ) // 32 MHz internal clock - OSC.CTRL = OSC_RC32MEN_bm; // enable internal 32MHz oscillator - while (!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for oscillator ready - - CCP = CCP_IOREG_gc; // Security Signature to modify clk - CLK.CTRL = CLK_SCLKSEL_RC32M_gc; // select sysclock 32MHz osc - -#else -#error No clock defined -#endif -} - - -static void _load_hw_id_byte(int i, register8_t *reg) { - NVM.CMD = NVM_CMD_READ_CALIB_ROW_gc; - uint8_t byte = pgm_read_byte(reg); - NVM.CMD = NVM_CMD_NO_OPERATION_gc; - - hw.id[i] = HEXNIB(byte >> 4); - hw.id[i + 1] = HEXNIB(byte); -} - - -static void _read_hw_id() { - int i = 0; - _load_hw_id_byte(i, &PROD_SIGS.LOTNUM5); i += 2; - _load_hw_id_byte(i, &PROD_SIGS.LOTNUM4); i += 2; - _load_hw_id_byte(i, &PROD_SIGS.LOTNUM3); i += 2; - _load_hw_id_byte(i, &PROD_SIGS.LOTNUM2); i += 2; - _load_hw_id_byte(i, &PROD_SIGS.LOTNUM1); i += 2; - _load_hw_id_byte(i, &PROD_SIGS.LOTNUM0); i += 2; - hw.id[i++] = '-'; - _load_hw_id_byte(i, &PROD_SIGS.WAFNUM); i += 2; - hw.id[i++] = '-'; - _load_hw_id_byte(i, &PROD_SIGS.COORDX1); i += 2; - _load_hw_id_byte(i, &PROD_SIGS.COORDX0); i += 2; - hw.id[i++] = '-'; - _load_hw_id_byte(i, &PROD_SIGS.COORDY1); i += 2; - _load_hw_id_byte(i, &PROD_SIGS.COORDY0); i += 2; - hw.id[i] = 0; -} - - -/// Lowest level hardware init -void hardware_init() { - _init_clock(); // set system clock - rtc_init(); // real time counter - _read_hw_id(); - - // Round-robin, interrupts in application section, all interupts levels - CCP = CCP_IOREG_gc; - PMIC.CTRL = - PMIC_RREN_bm | PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm; -} - - -void hw_request_hard_reset() {hw.hard_reset = true;} - - -/// Hard reset using watchdog timer -/// software hard reset using the watchdog timer -void hw_hard_reset() { - usart_flush(); - cli(); - CCP = CCP_IOREG_gc; - RST.CTRL = RST_SWRST_bm; -} - - -/// Controller's rest handler -void hw_reset_handler() { - if (hw.hard_reset) { - while (huanyang_stopping() || !usart_tx_empty() || !eeprom_is_ready()) - continue; - - hw_hard_reset(); - } - - if (hw.bootloader) { - // TODO enable bootloader interrupt vectors and jump to BOOT_SECTION_START - hw.bootloader = false; - } -} - - -/// Executes a software reset using CCPWrite -void hw_request_bootloader() {hw.bootloader = true;} - - -uint8_t hw_disable_watchdog() { - uint8_t state = WDT.CTRL; - wdt_disable(); - return state; -} - - -void hw_restore_watchdog(uint8_t state) { - cli(); - CCP = CCP_IOREG_gc; - WDT.CTRL = state | WDT_CEN_bm; - sei(); -} - - -const char *get_hw_id() { - return hw.id; -} diff --git a/src/hardware.h b/src/hardware.h deleted file mode 100644 index 4287779..0000000 --- a/src/hardware.h +++ /dev/null @@ -1,45 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/home.c b/src/home.c deleted file mode 100644 index a8b8986..0000000 --- a/src/home.c +++ /dev/null @@ -1,176 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "home.h" - -#include "plan/runtime.h" -#include "plan/line.h" -#include "plan/state.h" -#include "plan/exec.h" -#include "axis.h" -#include "motor.h" -#include "util.h" -#include "config.h" - -#include -#include - - -typedef enum { - STATE_INIT, - STATE_HOMING, - STATE_STALLED, - STATE_BACKOFF, - STATE_DONE, -} home_state_t; - - -typedef struct { - bool homed[AXES]; - unsigned axis; - home_state_t state; - float velocity; - uint16_t microsteps; -} home_t; - -static home_t home; - - -void _stall_callback(int motor) { - if (home.velocity == mp_runtime_get_velocity()) { - mp_exec_abort(); - home.state = STATE_STALLED; - } -} - - -void _move_axis(float travel) { - float target[AXES]; - float *position = mp_runtime_get_position(); - copy_vector(target, position); - target[home.axis] += travel; - mp_aline(target, false, false, false, home.velocity, 1, -1); -} - - -void home_callback() { - if (mp_get_cycle() != CYCLE_HOMING || !mp_is_quiescent() || - !mp_queue_is_empty()) return; - - while (true) { - int motor = axis_get_motor(home.axis); - homing_mode_t mode = axis_get_homing_mode(home.axis); - int direction = - (mode == HOMING_STALL_MIN || mode == HOMING_SWITCH_MIN) ? -1 : 1; - float min = axis_get_travel_min(home.axis); - float max = axis_get_travel_max(home.axis); - - switch (home.state) { - case STATE_INIT: { - if (motor == -1 || mode == HOMING_DISABLED) { - home.state = STATE_DONE; - break; - } - - STATUS_INFO("Homing %c axis", axis_get_char(home.axis)); - - // Set axis not homed - home.homed[home.axis] = false; - - // Determine homing type: switch or stall - - // Configure driver, set stall callback and compute homing velocity - home.microsteps = motor_get_microsteps(motor); - motor_set_microsteps(motor, 8); - motor_set_stall_callback(motor, _stall_callback); - //home.velocity = axis_get_stall_homing_velocity(home.axis); - home.velocity = axis_get_search_velocity(home.axis); - - // Move in home direction - float travel = max - min; // TODO consider ramping distance - _move_axis(travel * direction); - - home.state = STATE_HOMING; - return; - } - - case STATE_HOMING: - case STATE_STALLED: - // Restore motor driver config - motor_set_microsteps(motor, home.microsteps); - motor_set_stall_callback(motor, 0); - - if (home.state == STATE_HOMING) { - STATUS_ERROR(STAT_HOMING_CYCLE_FAILED, "Failed to find %c axis home", - axis_get_char(home.axis)); - mp_set_cycle(CYCLE_MACHINING); - - } else { - STATUS_INFO("Backing off %c axis", axis_get_char(home.axis)); - mach_set_axis_position(home.axis, direction < 0 ? min : max); - _move_axis(axis_get_zero_backoff(home.axis) * direction * -1); - home.state = STATE_BACKOFF; - } - return; - - case STATE_BACKOFF: - STATUS_INFO("Homed %c axis", axis_get_char(home.axis)); - - // Set axis position & homed - mach_set_axis_position(home.axis, min); - home.homed[home.axis] = true; - home.state = STATE_DONE; - break; - - case STATE_DONE: - if (home.axis == AXIS_X) { - // Done - mp_set_cycle(CYCLE_MACHINING); - return; - } - - // Next axis - home.axis--; - home.state = STATE_INIT; - break; - } - } -} - - -uint8_t command_home(int argc, char *argv[]) { - if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY) - return 0; - - // Init - memset(&home, 0, sizeof(home)); - home.axis = AXIS_Z; - - mp_set_cycle(CYCLE_HOMING); - - return 0; -} diff --git a/src/home.h b/src/home.h deleted file mode 100644 index dd6f286..0000000 --- a/src/home.h +++ /dev/null @@ -1,31 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - -void home_init(); -void home_callback(); diff --git a/src/homing.c b/src/homing.c deleted file mode 100644 index fda66c9..0000000 --- a/src/homing.c +++ /dev/null @@ -1,408 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "homing.h" - -#include "axis.h" -#include "machine.h" -#include "switch.h" -#include "gcode_parser.h" -#include "report.h" - -#include "plan/planner.h" -#include "plan/runtime.h" -#include "plan/state.h" - - -typedef void (*homing_func_t)(int8_t axis); - -static void _homing_axis_start(int8_t axis); - - -typedef enum { - HOMING_NOT_HOMED, // machine is not homed - HOMING_HOMED, // machine is homed - HOMING_WAITING, // machine waiting to be homed -} homing_state_t; - - -/// persistent homing runtime variables -typedef struct { - homing_state_t state; // homing cycle sub-state machine - bool homed[AXES]; // individual axis homing flags - - // controls for homing cycle - int8_t axis; // axis currently being homed - - /// homing switch for current axis (index into switch flag table) - int8_t homing_switch; - int8_t limit_switch; // limit switch for current axis or -1 if none - - uint8_t homing_closed; // 0=open, 1=closed - uint8_t limit_closed; // 0=open, 1=closed - /// G28.4 flag. true = set coords to zero at the end of homing cycle - uint8_t set_coordinates; - homing_func_t func; // binding for callback function state machine - - // per-axis parameters - /// set to 1 for positive (max), -1 for negative (to min); - float direction; - float search_travel; // signed distance to travel in search - float search_velocity; // search speed as positive number - float latch_velocity; // latch speed as positive number - /// max distance to back off switch during latch phase - float latch_backoff; - /// distance to back off switch before setting zero - float zero_backoff; - /// maximum distance of switch clearing backoffs before erring out - float max_clear_backoff; - - // state saved from gcode model - uint8_t saved_units; // G20,G21 global setting - uint8_t saved_coord_system; // G54 - G59 setting - uint8_t saved_distance_mode; // G90,G91 global setting - uint8_t saved_feed_mode; // G93,G94 global setting - float saved_feed_rate; // F setting - float saved_jerk; // saved and restored for each axis homed -} homing_t; - - -static homing_t hm = {0}; - - -// G28.2 homing cycle - -/* Homing works from a G28.2 according to the following writeup: - * - * https://github.com/synthetos - * /TinyG/wiki/Homing-and-Limits-Description-and-Operation - * - * How does this work? - * - * Homing is invoked using a G28.2 command with 1 or more axes specified in the - * command: e.g. g28.2 x0 y0 z0 (FYI: the number after each axis is irrelevant) - * - * Homing is always run in the following order - for each enabled axis: - * Z,X,Y,A Note: B and C cannot be homed - * - * At the start of a homing cycle those switches configured for homing - * (or for homing and limits) are treated as homing switches (they are modal). - * - * After initialization the following sequence is run for each axis to be homed: - * - * 0. If a homing or limit switch is closed on invocation, clear the switch - * 1. Drive towards the homing switch at search velocity until switch is hit - * 2. Drive away from the homing switch at latch velocity until switch opens - * 3. Back off switch by the zero backoff distance and set zero for that axis - * - * Homing works as a state machine that is driven by registering a callback - * function at hm.func() for the next state to be run. Once the axis is - * initialized each callback basically does two things (1) start the move - * for the current function, and (2) register the next state with hm.func(). - * When a move is started it will either be interrupted if the homing switch - * changes state, This will cause the move to stop with a feedhold. The other - * thing that can happen is the move will run to its full length if no switch - * change is detected (hit or open), - * - * Once all moves for an axis are complete the next axis in the sequence is - * homed. - * - * When a homing cycle is initiated the homing state is set to HOMING_NOT_HOMED - * When homing completes successfully this is set to HOMING_HOMED, otherwise it - * remains HOMING_NOT_HOMED. - * - * Notes: - * - * 1. When coding a cycle (like this one) you get to perform one queued - * move per entry into the continuation, then you must exit. - * - * 2. When coding a cycle (like this one) you must wait until - * the last move has actually been queued (or has finished) before - * declaring the cycle to be done. Otherwise there is a nasty race - * condition that will accept the next command before the position of - * the final move has been recorded in the Gcode model. - */ - - -/*** Return next axis in sequence based on axis in arg - * - * Accepts "axis" arg as the current axis; or -1 to retrieve the first axis - * Returns next axis based on "axis" argument and if that axis is flagged for - * homing in the gf struct - * Returns -1 when all axes have been processed - * Returns -2 if no axes are specified (Gcode calling error) - * Homes Z first, then the rest in sequence - * - * Isolating this function facilitates implementing more complex and - * user-specified axis homing orders - */ -static int8_t _get_next_axis(int8_t axis) { - switch (axis) { - case -1: if (parser.gf.target[AXIS_Z]) return AXIS_Z; - case AXIS_Z: if (parser.gf.target[AXIS_X]) return AXIS_X; - case AXIS_X: if (parser.gf.target[AXIS_Y]) return AXIS_Y; - case AXIS_Y: if (parser.gf.target[AXIS_A]) return AXIS_A; - case AXIS_A: if (parser.gf.target[AXIS_B]) return AXIS_B; - case AXIS_B: if (parser.gf.target[AXIS_C]) return AXIS_C; - } - - return axis == -1 ? -2 : -1; // error or done -} - - -/// Helper to finalize homing, third part of return to home -static void _homing_finalize_exit() { - mp_flush_planner(); // should be stopped, but in case of switch closure - - // Restore saved machine state - mach_set_coord_system(hm.saved_coord_system); - mach_set_units(hm.saved_units); - mach_set_distance_mode(hm.saved_distance_mode); - mach_set_feed_mode(hm.saved_feed_mode); - mach_set_feed_rate(hm.saved_feed_rate); - mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); - - mp_set_cycle(CYCLE_MACHINING); // Default cycle -} - - -static void _homing_error_exit(stat_t status) { - _homing_finalize_exit(); - status_error(status); -} - - -/// Execute moves -static void _homing_axis_move(int8_t axis, float target, float velocity) { - float vect[AXES] = {0}; - bool flags[AXES] = {0}; - - vect[axis] = target; - flags[axis] = true; - mach_set_feed_rate(velocity); - mp_flush_planner(); // don't use mp_request_flush() here - - stat_t status = mach_feed(vect, flags); - if (status) _homing_error_exit(status); -} - - -/// End homing cycle in progress -static void _homing_abort(int8_t axis) { - axis_set_jerk_max(axis, hm.saved_jerk); // restore the max jerk value - - // homing state remains HOMING_NOT_HOMED - _homing_error_exit(STAT_HOMING_CYCLE_FAILED); - - report_request(); -} - - -/// set zero and finish up -static void _homing_axis_set_zero(int8_t axis) { - if (hm.set_coordinates) { - mach_set_axis_position(axis, 0); - hm.homed[axis] = true; - - } else // do not set axis if in G28.4 cycle - mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); - - axis_set_jerk_max(axis, hm.saved_jerk); // restore the max jerk value - - hm.func = _homing_axis_start; -} - - -/// backoff to zero position -static void _homing_axis_zero_backoff(int8_t axis) { - _homing_axis_move(axis, hm.zero_backoff, hm.search_velocity); - hm.func = _homing_axis_set_zero; -} - - -/// Slow reverse until switch opens again -static void _homing_axis_latch(int8_t axis) { - // verify assumption that we arrived here because of homing switch closure - // rather than user-initiated feedhold or other disruption - if (!switch_is_active(hm.homing_switch)) hm.func = _homing_abort; - - else { - _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity); - hm.func = _homing_axis_zero_backoff; - } -} - - -/// Fast search for switch, closes switch -static void _homing_axis_search(int8_t axis) { - // use the homing jerk for search onward - _homing_axis_move(axis, hm.search_travel, hm.search_velocity); - hm.func = _homing_axis_latch; -} - - -/// Initiate a clear to move off a switch that is thrown at the start -static void _homing_axis_clear(int8_t axis) { - // Handle an initial switch closure by backing off the closed switch - // NOTE: Relies on independent switches per axis (not shared) - - if (switch_is_active(hm.homing_switch)) - _homing_axis_move(axis, hm.latch_backoff, hm.search_velocity); - - else if (switch_is_active(hm.limit_switch)) - _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity); - - hm.func = _homing_axis_search; -} - - -/// Get next axis, initialize variables, call the clear -static void _homing_axis_start(int8_t axis) { - // get the first or next axis - if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error - if (axis == -1) { // -1 is done - hm.state = HOMING_HOMED; - _homing_finalize_exit(); - return; - - } else if (axis == -2) // -2 is error - return _homing_error_exit(STAT_HOMING_ERROR_BAD_OR_NO_AXIS); - } - - // clear the homed flag for axis to move w/o triggering soft limits - hm.homed[axis] = false; - - // trap axis mis-configurations - if (fp_ZERO(axis_get_search_velocity(axis))) - return _homing_error_exit(STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY); - if (fp_ZERO(axis_get_latch_velocity(axis))) - return _homing_error_exit(STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY); - if (axis_get_latch_backoff(axis) < 0) - return _homing_error_exit(STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF); - - // calculate and test travel distance - float travel_distance = - fabs(axis_get_travel_max(axis) - axis_get_travel_min(axis)) + - axis_get_latch_backoff(axis); - if (fp_ZERO(travel_distance)) - return _homing_error_exit(STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL); - - hm.axis = axis; // persist the axis - // search velocity is always positive - hm.search_velocity = fabs(axis_get_search_velocity(axis)); - // latch velocity is always positive - hm.latch_velocity = fabs(axis_get_latch_velocity(axis)); - - // determine which switch to use - bool min_enabled = switch_is_enabled(MIN_SWITCH(axis)); - bool max_enabled = switch_is_enabled(MAX_SWITCH(axis)); - - if (min_enabled) { - // setup parameters homing to the minimum switch - hm.homing_switch = MIN_SWITCH(axis); // min is the homing switch - hm.limit_switch = MAX_SWITCH(axis); // max would be limit switch - hm.search_travel = -travel_distance; // in negative direction - hm.latch_backoff = axis_get_latch_backoff(axis); // in positive direction - hm.zero_backoff = axis_get_zero_backoff(axis); - - } else if (max_enabled) { - // setup parameters for positive travel (homing to the maximum switch) - hm.homing_switch = MAX_SWITCH(axis); // max is homing switch - hm.limit_switch = MIN_SWITCH(axis); // min would be limit switch - hm.search_travel = travel_distance; // in positive direction - hm.latch_backoff = -axis_get_latch_backoff(axis); // in negative direction - hm.zero_backoff = -axis_get_zero_backoff(axis); - - } else { - // if homing is disabled for the axis then skip to the next axis - hm.limit_switch = -1; // disable the limit switch parameter - hm.func = _homing_axis_start; - return; - } - - hm.saved_jerk = axis_get_jerk_max(axis); // save the max jerk value - hm.func = _homing_axis_clear; // start the clear -} - - -bool mach_is_homing() { - return mp_get_cycle() == CYCLE_HOMING; -} - - -void mach_set_not_homed() { - for (int axis = 0; axis < AXES; axis++) - mach_set_homed(axis, false); -} - - -bool mach_get_homed(int axis) { - return hm.homed[axis]; -} - - -void mach_set_homed(int axis, bool homed) { - // TODO save homed to EEPROM - hm.homed[axis] = homed; -} - - -/// G28.2 homing cycle using limit switches -void mach_homing_cycle_start() { - // save relevant non-axis parameters from Gcode model - hm.saved_units = mach_get_units(); - hm.saved_coord_system = mach_get_coord_system(); - hm.saved_distance_mode = mach_get_distance_mode(); - hm.saved_feed_mode = mach_get_feed_mode(); - hm.saved_feed_rate = mach_get_feed_rate(); - - // set working values - mach_set_units(MILLIMETERS); - mach_set_distance_mode(INCREMENTAL_MODE); - mach_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates - mach_set_feed_mode(UNITS_PER_MINUTE_MODE); - hm.set_coordinates = true; - - hm.axis = -1; // set to retrieve initial axis - hm.func = _homing_axis_start; // bind initial processing function - mp_set_cycle(CYCLE_HOMING); - hm.state = HOMING_NOT_HOMED; -} - - -void mach_homing_cycle_start_no_set() { - mach_homing_cycle_start(); - hm.set_coordinates = false; // don't update position variables at end of cycle -} - - -/// Main loop callback for running the homing cycle -void mach_homing_callback() { - if (mp_get_cycle() != CYCLE_HOMING || mp_get_state() != STATE_READY) return; - hm.func(hm.axis); // execute the current homing move -} diff --git a/src/homing.h b/src/homing.h deleted file mode 100644 index f4798f1..0000000 --- a/src/homing.h +++ /dev/null @@ -1,39 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - - -bool mach_is_homing(); -void mach_set_not_homed(); -bool mach_get_homed(int axis); -void mach_set_homed(int axis, bool homed); -void mach_homing_cycle_start(); -void mach_homing_cycle_start_no_set(); -void mach_homing_callback(); diff --git a/src/huanyang.c b/src/huanyang.c deleted file mode 100644 index 1abe98b..0000000 --- a/src/huanyang.c +++ /dev/null @@ -1,543 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 -#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_DC_VOLTAGE, - HUANYANG_AC_VOLTAGE, - HUANYANG_CONT, - HUANYANG_TEMPERATURE, -}; - - -enum { - HUANYANG_FORWARD = 1, - HUANYANG_STOP = 8, - HUANYANG_REVERSE = 17, -}; - - -enum { - HUANYANG_RUN = 1 << 0, - HUANYANG_JOG = 1 << 1, - HUANYANG_COMMAND_RF = 1 << 2, - HUANYANG_RUNNING = 1 << 3, - HUANYANG_JOGGING = 1 << 4, - HUANYANG_RUNNING_RF = 1 << 5, - HUANYANG_BRACKING = 1 << 6, - HUANYANG_TRACK_START = 1 << 7, -}; - - -typedef bool (*next_command_cb_t)(int index); - - -typedef struct { - uint8_t id; - bool debug; - - next_command_cb_t next_command_cb; - uint8_t command_index; - uint8_t current_offset; - uint8_t command[8]; - uint8_t command_length; - uint8_t response_length; - uint8_t response[10]; - uint32_t last; - uint8_t retry; - - bool connected; - bool changed; - bool estop; - spindle_mode_t mode; - float speed; - - float actual_freq; - float actual_current; - uint16_t actual_rpm; - uint16_t dc_voltage; - uint16_t ac_voltage; - uint16_t temperature; - - float max_freq; - float min_freq; - uint16_t rated_rpm; - - uint8_t status; -} huanyang_t; - - -static huanyang_t ha = {0}; - - -#define CTRL_STATUS_RESPONSE(R) ((uint16_t)R[4] << 8 | R[5]) -#define FUNC_RESPONSE(R) (R[2] == 2 ? R[4] : ((uint16_t)R[4] << 8 | R[5])) - - -static uint16_t _crc16(const uint8_t *buffer, unsigned length) { - uint16_t crc = 0xffff; - - for (unsigned i = 0; i < length; i++) - crc = _crc16_update(crc, buffer[i]); - - return crc; -} - - -static void _set_baud(uint16_t bsel, uint8_t bscale) { - HUANYANG_PORT.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8)); - HUANYANG_PORT.BAUDCTRLA = bsel; -} - - -static void _set_write(bool x) { - if (x) OUTSET_PIN(RS485_RW_PIN); // High - else OUTCLR_PIN(RS485_RW_PIN); // Low -} - - -static void _set_dre_interrupt(bool enable) { - if (enable) HUANYANG_PORT.CTRLA |= USART_DREINTLVL_MED_gc; - else HUANYANG_PORT.CTRLA &= ~USART_DREINTLVL_MED_gc; -} - - -static void _set_txc_interrupt(bool enable) { - if (enable) HUANYANG_PORT.CTRLA |= USART_TXCINTLVL_MED_gc; - else HUANYANG_PORT.CTRLA &= ~USART_TXCINTLVL_MED_gc; -} - - -static void _set_rxc_interrupt(bool enable) { - if (enable) HUANYANG_PORT.CTRLA |= USART_RXCINTLVL_MED_gc; - else HUANYANG_PORT.CTRLA &= ~USART_RXCINTLVL_MED_gc; -} - - -static void _set_command1(int code, uint8_t arg0) { - ha.command_length = 4; - ha.command[1] = code; - ha.command[2] = 1; - ha.command[3] = arg0; -} - - -static void _set_command2(int code, uint8_t arg0, uint8_t arg1) { - ha.command_length = 5; - ha.command[1] = code; - ha.command[2] = 2; - ha.command[3] = arg0; - ha.command[4] = arg1; -} - - -static void _set_command3(int code, uint8_t arg0, uint8_t arg1, uint8_t arg2) { - ha.command_length = 6; - ha.command[1] = code; - ha.command[2] = 3; - ha.command[3] = arg0; - ha.command[4] = arg1; - ha.command[5] = arg2; -} - - -static int _response_length(int code) { - switch (code) { - case HUANYANG_FUNC_READ: return 8; - case HUANYANG_FUNC_WRITE: return 8; - case HUANYANG_CTRL_WRITE: return 6; - case HUANYANG_CTRL_READ: return 8; - case HUANYANG_FREQ_WRITE: return 7; - default: return -1; - } -} - - -static bool _update(int index) { - // Read response - switch (index) { - case 1: ha.status = ha.response[5]; break; - case 2: ha.max_freq = FUNC_RESPONSE(ha.response) * 0.01; break; - case 3: ha.min_freq = FUNC_RESPONSE(ha.response) * 0.01; break; - case 4: ha.rated_rpm = FUNC_RESPONSE(ha.response); break; - default: break; - } - - // Setup next command - uint8_t var; - switch (index) { - case 0: { // Update mode - uint8_t state; - switch (ha.mode) { - case SPINDLE_CW: state = HUANYANG_FORWARD; break; - case SPINDLE_CCW: state = HUANYANG_REVERSE; break; - default: state = HUANYANG_STOP; break; - } - - _set_command1(HUANYANG_CTRL_WRITE, state); - - return true; - } - - case 1: var = HUANYANG_MAX_FREQ; break; - case 2: var = HUANYANG_MIN_FREQ; break; - case 3: var = HUANYANG_RATED_RPM; break; - - case 4: { // Update freqency - // Compute frequency in Hz - float freq = fabs(ha.speed * 50 / ha.rated_rpm); - - // Clamp frequency - if (ha.max_freq < freq) freq = ha.max_freq; - if (freq < ha.min_freq) freq = ha.min_freq; - - // Frequency write command - uint16_t f = freq * 100; - _set_command2(HUANYANG_FREQ_WRITE, f >> 8, f); - - return true; - } - - default: - report_request(); - return false; - } - - _set_command3(HUANYANG_FUNC_READ, var, 0, 0); - - return true; -} - - -static bool _query_status(int index) { - // Read response - switch (index) { - case 1: ha.actual_freq = CTRL_STATUS_RESPONSE(ha.response) * 0.01; break; - case 2: ha.actual_current = CTRL_STATUS_RESPONSE(ha.response) * 0.1; break; - case 3: ha.actual_rpm = CTRL_STATUS_RESPONSE(ha.response); break; - case 4: ha.dc_voltage = CTRL_STATUS_RESPONSE(ha.response); break; - case 5: ha.ac_voltage = CTRL_STATUS_RESPONSE(ha.response); break; - case 6: ha.temperature = CTRL_STATUS_RESPONSE(ha.response); break; - default: break; - } - - // Setup next command - uint8_t var; - switch (index) { - case 0: var = HUANYANG_ACTUAL_FREQ; break; - case 1: var = HUANYANG_ACTUAL_CURRENT; break; - case 2: var = HUANYANG_ACTUAL_RPM; break; - case 3: var = HUANYANG_DC_VOLTAGE; break; - case 4: var = HUANYANG_AC_VOLTAGE; break; - case 5: var = HUANYANG_TEMPERATURE; break; - default: - report_request(); - return false; - } - - _set_command1(HUANYANG_CTRL_READ, var); - - return true; -} - - -static void _next_command(); - - -static void _next_state() { - if (ha.changed) { - ha.next_command_cb = _update; - ha.changed = false; - - } else ha.next_command_cb = _query_status; - - _next_command(); -} - - -static bool _check_response() { - // Check CRC - uint16_t computed = _crc16(ha.response, ha.response_length - 2); - uint16_t expected = (ha.response[ha.response_length - 1] << 8) | - ha.response[ha.response_length - 2]; - - if (computed != expected) { - STATUS_WARNING("huanyang: invalid CRC, expected=0x%04u got=0x%04u", - expected, computed); - return false; - } - - // Check return function code matches sent - if (ha.command[1] != ha.response[1]) { - STATUS_WARNING("huanyang: invalid function code, expected=%u got=%u", - ha.command[2], ha.response[2]); - return false; - } - - return true; -} - - -static void _next_command() { - if (ha.next_command_cb && ha.next_command_cb(ha.command_index++)) { - ha.response_length = _response_length(ha.command[1]); - - ha.command[0] = ha.id; - - uint16_t crc = _crc16(ha.command, ha.command_length); - ha.command[ha.command_length++] = crc; - ha.command[ha.command_length++] = crc >> 8; - - _set_dre_interrupt(true); - - return; - } - - ha.command_index = 0; - _next_state(); -} - - -static void _retry_command() { - ha.last = 0; - ha.current_offset = 0; - ha.retry++; - - _set_write(true); // RS485 write mode - - _set_txc_interrupt(false); - _set_rxc_interrupt(false); - _set_dre_interrupt(true); - - if (ha.debug) STATUS_DEBUG("huanyang: retry %d", ha.retry); -} - - -// Data register empty interrupt -ISR(HUANYANG_DRE_vect) { - HUANYANG_PORT.DATA = ha.command[ha.current_offset++]; - - if (ha.current_offset == ha.command_length) { - _set_dre_interrupt(false); - _set_txc_interrupt(true); - ha.current_offset = 0; - } -} - - -/// Transmit complete interrupt -ISR(HUANYANG_TXC_vect) { - _set_txc_interrupt(false); - _set_rxc_interrupt(true); - _set_write(false); // RS485 read mode - ha.last = rtc_get_time(); // Set timeout timer -} - - -// Data received interrupt -ISR(HUANYANG_RXC_vect) { - ha.response[ha.current_offset++] = USARTD1.DATA; - - if (ha.current_offset == ha.response_length) { - _set_rxc_interrupt(false); - ha.current_offset = 0; - _set_write(true); // RS485 write mode - - if (_check_response()) { - ha.last = 0; // Clear timeout timer - ha.retry = 0; // Reset retry counter - ha.connected = true; - - _next_command(); - } - } -} - - -void huanyang_init() { - PR.PRPD &= ~PR_USART1_bm; // Disable power reduction - - DIRCLR_PIN(RS485_RO_PIN); // Input - OUTSET_PIN(RS485_DI_PIN); // High - DIRSET_PIN(RS485_DI_PIN); // Output - OUTSET_PIN(RS485_RW_PIN); // High - DIRSET_PIN(RS485_RW_PIN); // Output - - _set_baud(3325, 0b1101); // 9600 @ 32MHz with 2x USART - - // No parity, 8 data bits, 1 stop bit - USARTD1.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | - USART_CHSIZE_8BIT_gc; - - // Configure receiver and transmitter - USARTD1.CTRLB = USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm; - - ha.id = HUANYANG_ID; - huanyang_reset(); -} - - -void huanyang_set(spindle_mode_t mode, float speed) { - if ((ha.mode != mode || ha.speed != speed) && !ha.estop) { - if (ha.debug) STATUS_DEBUG("huanyang: mode=%d, speed=%0.2f", mode, speed); - - ha.mode = mode; - ha.speed = speed; - ha.changed = true; - } -} - - -void huanyang_reset() { - _set_dre_interrupt(false); - _set_txc_interrupt(false); - _set_rxc_interrupt(false); - _set_write(true); // RS485 write mode - - // Flush USART - uint8_t x = USARTD1.DATA; - x = USARTD1.STATUS; - x = x; - - // Save settings - uint8_t id = ha.id; - spindle_mode_t mode = ha.mode; - float speed = ha.speed; - bool debug = ha.debug; - - // Clear state - memset(&ha, 0, sizeof(ha)); - - // Restore settings - ha.id = id; - ha.mode = mode; - ha.speed = speed; - ha.debug = debug; - ha.changed = true; - - _next_state(); -} - - -void huanyang_rtc_callback() { - if (ha.last && rtc_expired(ha.last + HUANYANG_TIMEOUT)) { - if (ha.retry < HUANYANG_RETRIES) _retry_command(); - else { - if (ha.debug) STATUS_DEBUG("huanyang: timedout"); - - if (ha.debug && ha.current_offset) { - const uint8_t buf_len = 8 * 2 + 1; - char sent[buf_len]; - char received[buf_len]; - - uint8_t i; - for (i = 0; i < ha.command_length; i++) - sprintf(sent + i * 2, "%02x", ha.command[i]); - sent[i * 2] = 0; - - for (i = 0; i < ha.current_offset; i++) - sprintf(received + i * 2, "%02x", ha.response[i]); - received[i * 2] = 0; - - STATUS_DEBUG("huanyang: sent 0x%s received 0x%s expected %u bytes", - sent, received, ha.response_length); - } - - huanyang_reset(); - } - } -} - - -void huanyang_estop() { - huanyang_set(SPINDLE_OFF, 0); - huanyang_reset(); - ha.estop = true; -} - - -bool huanyang_stopping() { - return ha.estop && (ha.changed || ha.next_command_cb == _update); -} - - -uint8_t get_huanyang_id() {return ha.id;} -void set_huanyang_id(uint8_t value) {ha.id = value;} -bool get_huanyang_debug() {return ha.debug;} -void set_huanyang_debug(bool value) {ha.debug = value;} -bool get_huanyang_connected() {return ha.connected;} -float get_huanyang_freq() {return ha.actual_freq;} -float get_huanyang_current() {return ha.actual_current;} -uint16_t get_huanyang_rpm() {return ha.actual_rpm;} -uint16_t get_huanyang_dcv() {return ha.dc_voltage;} -uint16_t get_huanyang_acv() {return ha.ac_voltage;} -uint16_t get_huanyang_temp() {return ha.temperature;} -float get_huanyang_max_freq() {return ha.max_freq;} -float get_huanyang_min_freq() {return ha.min_freq;} -uint16_t get_huanyang_rated_rpm() {return ha.rated_rpm;} -float get_huanyang_status() {return ha.status;} diff --git a/src/huanyang.h b/src/huanyang.h deleted file mode 100644 index 46efeac..0000000 --- a/src/huanyang.h +++ /dev/null @@ -1,38 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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_estop(); -bool huanyang_stopping(); diff --git a/src/i2c.c b/src/i2c.c deleted file mode 100644 index 0ec26b9..0000000 --- a/src/i2c.c +++ /dev/null @@ -1,128 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2013 Alden S. Hart Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/i2c.h b/src/i2c.h deleted file mode 100644 index 7ded350..0000000 --- a/src/i2c.h +++ /dev/null @@ -1,58 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2013 Alden S. Hart Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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_HOME, - I2C_REBOOT, - I2C_ZERO, -} i2c_cmd_t; - - -typedef void (*i2c_read_cb_t)(i2c_cmd_t cmd, uint8_t *data, uint8_t length); -typedef uint8_t (*i2c_write_cb_t)(uint8_t offset, bool *done); - - -void i2c_init(); -void i2c_set_read_callback(i2c_read_cb_t cb); -void i2c_set_write_callback(i2c_write_cb_t cb); diff --git a/src/machine.c b/src/machine.c deleted file mode 100644 index 49c6349..0000000 --- a/src/machine.c +++ /dev/null @@ -1,855 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "homing.h" - -#include "plan/planner.h" -#include "plan/runtime.h" -#include "plan/dwell.h" -#include "plan/arc.h" -#include "plan/line.h" -#include "plan/state.h" - - -typedef struct { // struct to manage mach globals and cycles - float offset[COORDS + 1][AXES]; // coordinate systems & offsets G53-G59 - float origin_offset[AXES]; // G92 offsets - bool origin_offset_enable; // G92 offsets enabled / disabled - - float position[AXES]; // model position - float g28_position[AXES]; // stored machine position for G28 - float g30_position[AXES]; // stored machine position for G30 - - gcode_state_t gm; // core gcode model state -} machine_t; - - -static machine_t mach = { - // Offsets - .offset = { - {}, // ABSOLUTE_COORDS - - {0, 0, 0, 0, 0, 0}, // G54 - {X_TRAVEL_MAX / 2, Y_TRAVEL_MAX / 2, 0, 0, 0, 0}, // G55 - {0, 0, 0, 0, 0, 0}, // G56 - {0, 0, 0, 0, 0, 0}, // G57 - {0, 0, 0, 0, 0, 0}, // G58 - {0, 0, 0, 0, 0, 0}, // G59 - }, - - // State - .gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE}, -}; - - -// Machine State functions -uint32_t mach_get_line() {return mach.gm.line;} -float mach_get_feed_rate() {return mach.gm.feed_rate;} -feed_mode_t mach_get_feed_mode() {return mach.gm.feed_mode;} - - -bool mach_is_inverse_time_mode() { - return mach.gm.feed_mode == INVERSE_TIME_MODE; -} - - -float mach_get_feed_override() { - return mach.gm.feed_override_enable ? mach.gm.feed_override : 1; -} - - -float mach_get_spindle_override() { - return mach.gm.spindle_override_enable ? mach.gm.spindle_override : 1; -} - - -motion_mode_t mach_get_motion_mode() {return mach.gm.motion_mode;} -bool mach_is_rapid() {return mach.gm.motion_mode == MOTION_MODE_RAPID;} -plane_t mach_get_plane() {return mach.gm.plane;} -units_t mach_get_units() {return mach.gm.units;} -coord_system_t mach_get_coord_system() {return mach.gm.coord_system;} -bool mach_get_absolute_mode() {return mach.gm.absolute_mode;} -path_mode_t mach_get_path_mode() {return mach.gm.path_mode;} -bool mach_is_exact_stop() {return mach.gm.path_mode == PATH_EXACT_STOP;} -distance_mode_t mach_get_distance_mode() {return mach.gm.distance_mode;} -distance_mode_t mach_get_arc_distance_mode() {return mach.gm.arc_distance_mode;} - - -void mach_set_motion_mode(motion_mode_t motion_mode) { - mach.gm.motion_mode = motion_mode; -} - - -/// Spindle speed callback from planner queue -static stat_t _exec_spindle_speed(mp_buffer_t *bf) { - spindle_set_speed(bf->value); - return STAT_NOOP; // No move queued -} - - -/// Queue the S parameter to the planner buffer -void mach_set_spindle_speed(float speed) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->value = speed * mach_get_spindle_override(); - mp_queue_push_nonstop(_exec_spindle_speed, mach_get_line()); -} - - -/// execute the spindle command (called from planner) -static stat_t _exec_spindle_mode(mp_buffer_t *bf) { - spindle_set_mode(bf->value); - return STAT_NOOP; // No move queued -} - - -/// Queue the spindle command to the planner buffer -void mach_set_spindle_mode(spindle_mode_t mode) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->value = mode; - mp_queue_push_nonstop(_exec_spindle_mode, mach_get_line()); -} - - -void mach_set_absolute_mode(bool absolute_mode) { - mach.gm.absolute_mode = absolute_mode; -} - - -void mach_set_line(uint32_t line) {mach.gm.line = line;} - - -/* Coordinate systems and offsets - * - * Functions to get, set and report coordinate systems and work offsets - * These functions are not part of the NIST defined functions - * - * Notes on Coordinate System and Offset functions - * - * All positional information in the machine is kept as - * absolute coords and in canonical units (mm). The offsets are only - * used to translate in and out of canonical form during - * interpretation and response. - * - * Managing the coordinate systems & offsets is somewhat complicated. - * The following affect offsets: - * - coordinate system selected. 1-9 correspond to G54-G59 - * - absolute override: forces current move to be interpreted in machine - * coordinates: G53 (system 0) - * - G92 offsets are added "on top of" the coord system offsets -- - * if origin_offset_enable - * - G28 and G30 moves; these are run in absolute coordinates - * - * The offsets themselves are considered static, are kept in mach, and are - * supposed to be persistent. - * - * To reduce complexity and data load the following is done: - * - Full data for coordinates/offsets is only accessible by the - * machine, not the downstream - * - Resolved set of coord and G92 offsets, with per-move exceptions can - * be captured as "work_offsets" - * - The core gcode context (gm) only knows about the active coord system - * and the work offsets - */ - -/* Return the currently active coordinate offset for an axis - * - * Takes G5x, G92 and absolute override into account to return the - * active offset for this move - * - * This function is typically used to evaluate and set offsets. - */ -float mach_get_active_coord_offset(uint8_t axis) { - // no offset in absolute override mode - if (mach.gm.absolute_mode) return 0; - float offset = mach.offset[mach.gm.coord_system][axis]; - - if (mach.origin_offset_enable) - offset += mach.origin_offset[axis]; // includes G5x and G92 components - - return offset; -} - - -static stat_t _exec_update_work_offsets(mp_buffer_t *bf) { - mp_runtime_set_work_offsets(bf->target); - return STAT_NOOP; // No move queued -} - - -// Capture coord offsets from the model into absolute values -void mach_update_work_offsets() { - static float work_offset[AXES] = {0}; - bool same = true; - - for (int axis = 0; axis < AXES; axis++) { - float offset = mach_get_active_coord_offset(axis); - - if (offset != work_offset[axis]) { - work_offset[axis] = offset; - same = false; - } - } - - if (!same) { - mp_buffer_t *bf = mp_queue_get_tail(); - copy_vector(bf->target, work_offset); - mp_queue_push_nonstop(_exec_update_work_offsets, mach_get_line()); - } -} - - -const float *mach_get_position() {return mach.position;} - - -void mach_set_position(const float position[]) { - copy_vector(mach.position, position); -} - - -/*** Get position of axis in absolute coordinates - * - * NOTE: Machine position is always returned in mm mode. No units conversion - * is performed. - */ -float mach_get_axis_position(uint8_t axis) {return mach.position[axis];} - - -/* Critical helpers - * - * Core functions supporting the machining functions - * These functions are not part of the NIST defined functions - */ - - - -/*** Calculate target vector - * - * This is a core routine. It handles: - * - conversion of linear units to internal canonical form (mm) - * - conversion of relative mode to absolute (internal canonical form) - * - translation of work coordinates to machine coordinates (internal - * canonical form) - * - application of axis radius mode - * - * Target coordinates are provided in @param values. - * Axes that need processing are signaled in @param flags. - */ -void mach_calc_target(float target[], const float values[], - const bool flags[]) { - for (int axis = 0; axis < AXES; axis++) { - target[axis] = mach.position[axis]; - if (!flags[axis] || !axis_is_enabled(axis)) continue; - - target[axis] = mach.gm.distance_mode == ABSOLUTE_MODE ? - mach_get_active_coord_offset(axis) : mach.position[axis]; - - float radius = axis_get_radius(axis); - if (radius) // Handle radius mode if radius is non-zero - target[axis] += TO_MM(values[axis]) * 360 / (2 * M_PI * radius); - // For ABC axes no mm conversion - it's already in degrees - else if (AXIS_Z < axis) target[axis] += values[axis]; - else target[axis] += TO_MM(values[axis]); - } -} - - -/*** Return error code if soft limit is exceeded - * - * Must be called with target properly set in GM struct. Best done - * after mach_calc_target(). - * - * Tests for soft limit for any homed axis if min and max are - * different values. You can set min and max to 0,0 to disable soft - * limits for an axis. Also will not test a min or a max if the value - * is < -1000000 (negative one million). This allows a single end to - * be tested w/the other disabled, should that requirement ever arise. - */ -stat_t mach_test_soft_limits(float target[]) { - for (int axis = 0; axis < AXES; axis++) { - if (!axis_get_homed(axis)) continue; // don't test axes that arent homed - - float min = axis_get_travel_min(axis); - float max = axis_get_travel_max(axis); - - // min == max means no soft limits - if (fp_EQ(min, max)) continue; - - if ((min > DISABLE_SOFT_LIMIT && target[axis] < min) || - (max > DISABLE_SOFT_LIMIT && target[axis] > max)) - return STAT_SOFT_LIMIT_EXCEEDED; - } - - return STAT_OK; -} - - -/* machining functions - * Values are passed in pre-unit_converted state (from gn structure) - * All operations occur on gm (current model state) - * - * These are organized by section number (x.x.x) in the order they are - * found in NIST RS274 NGCv3 - */ - -// Initialization and Termination (4.3.2) - -void machine_init() { - // Set gcode defaults - mach_set_units(GCODE_DEFAULT_UNITS); - mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM); - mach_set_plane(GCODE_DEFAULT_PLANE); - mach_set_path_mode(GCODE_DEFAULT_PATH_CONTROL); - mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE); - mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE); - mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // always the default - - // Sub-system inits - spindle_init(); - coolant_init(); -} - - -// Representation (4.3.3) -// -// Affect the Gcode model only (asynchronous) -// These functions assume input validation occurred upstream. - -/// G17, G18, G19 select axis plane -void mach_set_plane(plane_t plane) {mach.gm.plane = plane;} - - -/// G20, G21 -void mach_set_units(units_t mode) {mach.gm.units = mode;} - - -/// G90, G91 -void mach_set_distance_mode(distance_mode_t mode) { - mach.gm.distance_mode = mode; -} - - -/// G90.1, G91.1 -void mach_set_arc_distance_mode(distance_mode_t mode) { - mach.gm.arc_distance_mode = mode; -} - - -/* G10 L2 Pn, delayed persistence - * - * This function applies the offset to the GM model. - */ -void mach_set_coord_offsets(coord_system_t coord_system, float offset[], - bool flags[]) { - if (coord_system < G54 || G59 < coord_system) return; - - for (int axis = 0; axis < AXES; axis++) - if (flags[axis]) - mach.offset[coord_system][axis] = TO_MM(offset[axis]); -} - - -/// G54-G59 -void mach_set_coord_system(coord_system_t coord_system) { - mach.gm.coord_system = coord_system; -} - - -/* Set the position of a single axis in the model, planner and runtime - * - * This command sets an axis/axes to a position provided as an argument. - * This is useful for setting origins for homing, probing, and other operations. - * - * !!!!! DO NOT CALL THIS FUNCTION WHILE IN A MACHINING CYCLE !!!!! - * - * More specifically, do not call this function if there are any moves - * in the planner or if the runtime is moving. The system must be - * quiescent or you will introduce positional errors. This is true - * because the planned / running moves have a different reference - * frame than the one you are now going to set. These functions should - * only be called during initialization sequences and during cycles - * (such as homing cycles) when you know there are no more moves in - * the planner and that all motion has stopped. - */ -void mach_set_axis_position(unsigned axis, float position) { - //if (!mp_is_quiescent()) ALARM(STAT_MACH_NOT_QUIESCENT); - if (AXES <= axis) return; - - mach.position[axis] = position; - mp_set_axis_position(axis, position); - mp_runtime_set_axis_position(axis, position); - mp_runtime_set_steps_from_position(); -} - - -stat_t mach_zero_all() { - for (unsigned axis = 0; axis < AXES; axis++) { - stat_t status = mach_zero_axis(axis); - if (status != STAT_OK) return status; - } - - return STAT_OK; -} - - -stat_t mach_zero_axis(unsigned axis) { - if (!mp_is_quiescent()) return STAT_MACH_NOT_QUIESCENT; - if (AXES <= axis) return STAT_INVALID_AXIS; - - mach_set_axis_position(axis, 0); - - return STAT_OK; -} - - -// G28.3 functions and support -static stat_t _exec_absolute_origin(mp_buffer_t *bf) { - const float *origin = bf->target; - const float *flags = bf->unit; - - for (int axis = 0; axis < AXES; axis++) - if (flags[axis]) { - mp_runtime_set_axis_position(axis, origin[axis]); - mach_set_homed(axis, true); // G28.3 is not considered homed until here - } - - mp_runtime_set_steps_from_position(); - - return STAT_NOOP; // No move queued -} - - -/* G28.3 - model, planner and queue to runtime - * - * Takes a vector of origins (presumably 0's, but not necessarily) and - * applies them to all axes where the corresponding position in the - * flags vector is true (1). - * - * This is a 2 step process. The model and planner contexts are set - * immediately, the runtime command is queued and synchronized with - * the planner queue. This includes the runtime position and the step - * recording done by the encoders. At that point any axis that is set - * is also marked as homed. - */ -void mach_set_absolute_origin(float origin[], bool flags[]) { - float value[AXES]; - - for (int axis = 0; axis < AXES; axis++) - if (flags[axis]) { - value[axis] = TO_MM(origin[axis]); - mach.position[axis] = value[axis]; // set model position - mp_set_axis_position(axis, value[axis]); // set mm position - } - - mp_buffer_t *bf = mp_queue_get_tail(); - copy_vector(bf->target, origin); - copy_vector(bf->unit, flags); - mp_queue_push_nonstop(_exec_absolute_origin, mach_get_line()); -} - - -/* G92's behave according to NIST 3.5.18 & LinuxCNC G92 - * http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G92-G92.1-G92.2-G92.3 - */ - -/// G92 -void mach_set_origin_offsets(float offset[], bool flags[]) { - // set offsets in the Gcode model extended context - mach.origin_offset_enable = true; - for (int axis = 0; axis < AXES; axis++) - if (flags[axis]) - mach.origin_offset[axis] = mach.position[axis] - - mach.offset[mach.gm.coord_system][axis] - TO_MM(offset[axis]); -} - - -/// G92.1 -void mach_reset_origin_offsets() { - mach.origin_offset_enable = false; - for (int axis = 0; axis < AXES; axis++) - mach.origin_offset[axis] = 0; -} - - -/// G92.2 -void mach_suspend_origin_offsets() { - mach.origin_offset_enable = false; -} - - -/// G92.3 -void mach_resume_origin_offsets() { - mach.origin_offset_enable = true; -} - - -stat_t mach_plan_line(float target[]) { - return mp_aline(target, mach_is_rapid(), mach_is_inverse_time_mode(), - mach_is_exact_stop(), mach.gm.feed_rate, - mach_get_feed_override(), mach_get_line()); -} - - -// Free Space Motion (4.3.4) -static stat_t _feed(float values[], bool flags[]) { - float target[AXES]; - mach_calc_target(target, values, flags); - - // test soft limits - stat_t status = mach_test_soft_limits(target); - if (status != STAT_OK) return ALARM(status); - - // prep and plan the move - mach_update_work_offsets(); // update resolved offsets - mach_plan_line(target); - copy_vector(mach.position, target); // update model position - - return status; -} - - -/// G0 linear rapid -stat_t mach_rapid(float values[], bool flags[]) { - mach.gm.motion_mode = MOTION_MODE_RAPID; - return _feed(values, flags); -} - - -/// G28.1 -void mach_set_g28_position() {copy_vector(mach.g28_position, mach.position);} - - -/// G28 -stat_t mach_goto_g28_position(float target[], bool flags[]) { - mach_set_absolute_mode(true); - - // move through intermediate point, or skip - mach_rapid(target, flags); - - // execute actual stored move - bool f[] = {true, true, true, true, true, true}; - return mach_rapid(mach.g28_position, f); -} - - -/// G30.1 -void mach_set_g30_position() {copy_vector(mach.g30_position, mach.position);} - - -/// G30 -stat_t mach_goto_g30_position(float target[], bool flags[]) { - mach_set_absolute_mode(true); - - // move through intermediate point, or skip - mach_rapid(target, flags); - - // execute actual stored move - bool f[] = {true, true, true, true, true, true}; - return mach_rapid(mach.g30_position, f); -} - - -// Machining Attributes (4.3.5) - -/// F parameter -/// Normalize feed rate to mm/min or to minutes if in inverse time mode -void mach_set_feed_rate(float feed_rate) { - if (mach.gm.feed_mode == INVERSE_TIME_MODE) - // normalize to minutes (active for this gcode block only) - mach.gm.feed_rate = feed_rate ? 1 / feed_rate : 0; // Avoid div by zero - - else mach.gm.feed_rate = TO_MM(feed_rate); -} - - -/// G93, G94 -void mach_set_feed_mode(feed_mode_t mode) { - if (mach.gm.feed_mode == mode) return; - mach.gm.feed_rate = 0; // Force setting feed rate after changing modes - mach.gm.feed_mode = mode; -} - - -/// G61, G61.1, G64 -void mach_set_path_mode(path_mode_t mode) { - mach.gm.path_mode = mode; -} - - -// Machining Functions (4.3.6) See arc.c - -/// G4, P parameter (seconds) -stat_t mach_dwell(float seconds) { - return mp_dwell(seconds, mach_get_line()); -} - - -/// G1 -stat_t mach_feed(float values[], bool flags[]) { - // trap zero feed rate condition - if (fp_ZERO(mach.gm.feed_rate) || - (mach.gm.feed_mode == INVERSE_TIME_MODE && !parser.gf.feed_rate)) - return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - - mach.gm.motion_mode = MOTION_MODE_FEED; - return _feed(values, flags); -} - - -// Spindle Functions (4.3.7) see spindle.c, spindle.h - -// Tool Functions (4.3.8) - -/// T parameter -void mach_select_tool(uint8_t tool) {mach.gm.tool = tool;} - - -static stat_t _exec_change_tool(mp_buffer_t *bf) { - mp_runtime_set_tool(bf->value); - mp_set_hold_reason(HOLD_REASON_TOOL_CHANGE); - mp_state_holding(); - return STAT_NOOP; // No move queued -} - - -/// M6 -void mach_change_tool(bool x) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->value = mach.gm.tool; - mp_queue_push(_exec_change_tool, mach_get_line()); -} - - -// Miscellaneous Functions (4.3.9) -static stat_t _exec_mist_coolant(mp_buffer_t *bf) { - coolant_set_mist(bf->value); - return STAT_NOOP; // No move queued -} - - -/// M7 -void mach_mist_coolant_control(bool mist_coolant) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->value = mist_coolant; - mp_queue_push_nonstop(_exec_mist_coolant, mach_get_line()); -} - - -static stat_t _exec_flood_coolant(mp_buffer_t *bf) { - coolant_set_flood(bf->value); - if (!bf->value) coolant_set_mist(false); // M9 special function - return STAT_NOOP; // No move queued -} - - -/// M8, M9 -void mach_flood_coolant_control(bool flood_coolant) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->value = flood_coolant; - mp_queue_push_nonstop(_exec_flood_coolant, mach_get_line()); -} - - -/* Override enables are kind of a mess in Gcode. This is an attempt to sort - * them out. See - * http://www.linuxcnc.org/docs/2.4/html/gcode_main.html#sec:M50:-Feed-Override - */ - -/// M48, M49 -void mach_override_enables(bool flag) { - mach.gm.feed_override_enable = flag; - mach.gm.spindle_override_enable = flag; -} - - -/// M50 -void mach_feed_override_enable(bool flag) { - if (parser.gf.parameter && fp_ZERO(parser.gn.parameter)) - mach.gm.feed_override_enable = false; - else { - if (parser.gf.parameter) mach.gm.feed_override = parser.gf.parameter; - mach.gm.feed_override_enable = true; - } -} - - -/// M51 -void mach_spindle_override_enable(bool flag) { - if (parser.gf.parameter && fp_ZERO(parser.gn.parameter)) - mach.gm.spindle_override_enable = false; - else { - if (parser.gf.parameter) mach.gm.spindle_override = parser.gf.parameter; - mach.gm.spindle_override_enable = true; - } -} - - -void mach_message(const char *message) { - status_message_P(0, STAT_LEVEL_INFO, STAT_OK, PSTR("%s"), message); -} - - -/* Program Functions (4.3.10) - * - * This group implements stop, start, end, and hold. - * It is extended beyond the NIST spec to handle various situations. - * - * mach_program_stop and mach_optional_program_stop are synchronous Gcode - * commands that are received through the interpreter. They cause all motion - * to stop at the end of the current command, including spindle motion. - * - * Note that the stop occurs at the end of the immediately preceding command - * (i.e. the stop is queued behind the last command). - * - * mach_program_end is a stop that also resets the machine to initial state - */ - - -static stat_t _exec_program_stop(mp_buffer_t *bf) { - // Machine should be stopped at this point. Go into hold so that a start is - // needed before executing further instructions. - mp_set_hold_reason(HOLD_REASON_PROGRAM_PAUSE); - mp_state_holding(); - return STAT_NOOP; // No move queued -} - - -/// M0 Queue a program stop -void mach_program_stop() { - mp_queue_push(_exec_program_stop, mach_get_line()); -} - - -static stat_t _exec_optional_program_stop(mp_buffer_t *bf) { - mp_state_optional_pause(); // Pause here if it was requested by the user - return STAT_NOOP; // No move queued -} - - -/// M1 -void mach_optional_program_stop() { - mp_queue_push(_exec_optional_program_stop, mach_get_line()); -} - - -static stat_t _exec_pallet_change_stop(mp_buffer_t *bf) { - // Emit pallet change signal - mp_set_hold_reason(HOLD_REASON_PALLET_CHANGE); - mp_state_holding(); - return STAT_NOOP; // No move queued -} - - -/// M60 -void mach_pallet_change_stop() { - mp_queue_push(_exec_pallet_change_stop, mach_get_line()); -} - - -/*** mach_program_end() implements M2 and M30. End behaviors are defined by - * NIST 3.6.1 are: - * - * 1. Axis offsets are set to zero (like G92.2) and origin offsets are set - * to the default (like G54) - * 2. Selected plane is set to PLANE_XY (like G17) - * 3. Distance mode is set to MODE_ABSOLUTE (like G90) - * 4. Feed rate mode is set to UNITS_PER_MINUTE (like G94) - * 5. Feed and speed overrides are set to ON (like M48) - * 6. Cutter compensation is turned off (like G40) - * 7. The spindle is stopped (like M5) - * 8. The current motion mode is set to G_1 (like G1) - * 9. Coolant is turned off (like M9) - * - * mach_program_end() implements things slightly differently: - * - * 1. Axis offsets are set to G92.1 CANCEL offsets - * (instead of using G92.2 SUSPEND Offsets) - * Set default coordinate system - * 2. Selected plane is set to default plane - * 3. Distance mode is set to MODE_ABSOLUTE (like G90) - * 4. Feed rate mode is set to UNITS_PER_MINUTE (like G94) - * 5. Not implemented - * 6. Not implemented - * 7. The spindle is stopped (like M5) - * 8. Motion mode is canceled like G80 (not set to G1) - * 9. Coolant is turned off (like M9) - * + Default INCHES or MM units mode is restored - */ - - -/// M2, M30 -void mach_program_end() { - mach_reset_origin_offsets(); // G92.1 - we do G91.1 instead of G92.2 - mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM); - mach_set_plane(GCODE_DEFAULT_PLANE); - mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE); - mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE); - mach_set_spindle_mode(SPINDLE_OFF); // M5 - mach_flood_coolant_control(false); // M9 - mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // G94 - mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); -} diff --git a/src/machine.h b/src/machine.h deleted file mode 100644 index c6f5497..0000000 --- a/src/machine.h +++ /dev/null @@ -1,136 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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" - - -#define TO_MM(a) (mach_get_units() == INCHES ? (a) * MM_PER_INCH : a) - -// Model state getters and setters -uint32_t mach_get_line(); -float mach_get_feed_rate(); -bool mach_is_inverse_time_mode(); -feed_mode_t mach_get_feed_mode(); -float mach_get_feed_override(); -float mach_get_spindle_override(); -motion_mode_t mach_get_motion_mode(); -bool mach_is_rapid(); -plane_t mach_get_plane(); -units_t mach_get_units(); -coord_system_t mach_get_coord_system(); -bool mach_get_absolute_mode(); -path_mode_t mach_get_path_mode(); -bool mach_is_exact_stop(); -distance_mode_t mach_get_distance_mode(); -distance_mode_t mach_get_arc_distance_mode(); - -void mach_set_motion_mode(motion_mode_t motion_mode); -void mach_set_spindle_mode(spindle_mode_t spindle_mode); -void mach_set_spindle_speed(float speed); -void mach_set_absolute_mode(bool absolute_mode); -void mach_set_line(uint32_t line); - -// Coordinate systems and offsets -float mach_get_active_coord_offset(uint8_t axis); -void mach_update_work_offsets(); -const float *mach_get_position(); -void mach_set_position(const float position[]); -float mach_get_axis_position(uint8_t axis); - -// Critical helpers -void mach_calc_target(float target[], const float values[], const bool flags[]); -stat_t mach_test_soft_limits(float target[]); - -// machining functions defined by NIST [organized by NIST Gcode doc] - -// Initialization and termination (4.3.2) -void machine_init(); - -// Representation (4.3.3) -void mach_set_plane(plane_t plane); -void mach_set_units(units_t mode); -void mach_set_distance_mode(distance_mode_t mode); -void mach_set_arc_distance_mode(distance_mode_t mode); -void mach_set_coord_offsets(coord_system_t coord_system, float offset[], - bool flags[]); - -void mach_set_axis_position(unsigned axis, float position); -void mach_set_absolute_origin(float origin[], bool flags[]); - -stat_t mach_zero_all(); -stat_t mach_zero_axis(unsigned axis); - -void mach_set_coord_system(coord_system_t coord_system); -void mach_set_origin_offsets(float offset[], bool flags[]); -void mach_reset_origin_offsets(); -void mach_suspend_origin_offsets(); -void mach_resume_origin_offsets(); - -// Free Space Motion (4.3.4) -stat_t mach_plan_line(float target[]); -stat_t mach_rapid(float target[], bool flags[]); -void mach_set_g28_position(); -stat_t mach_goto_g28_position(float target[], bool flags[]); -void mach_set_g30_position(); -stat_t mach_goto_g30_position(float target[], bool flags[]); - -// Machining Attributes (4.3.5) -void mach_set_feed_rate(float feed_rate); -void mach_set_feed_mode(feed_mode_t mode); -void mach_set_path_mode(path_mode_t mode); - -// Machining Functions (4.3.6) see arc.h -stat_t mach_feed(float target[], bool flags[]); -stat_t mach_dwell(float seconds); - -// Spindle Functions (4.3.7) see spindle.h - -// Tool Functions (4.3.8) -void mach_select_tool(uint8_t tool); -void mach_change_tool(bool x); - -// Miscellaneous Functions (4.3.9) -void mach_mist_coolant_control(bool mist_coolant); -void mach_flood_coolant_control(bool flood_coolant); - -void mach_override_enables(bool flag); -void mach_feed_override_enable(bool flag); -void mach_spindle_override_enable(bool flag); - -void mach_message(const char *message); - -// Program Functions (4.3.10) -void mach_program_stop(); -void mach_optional_program_stop(); -void mach_pallet_change_stop(); -void mach_program_end(); diff --git a/src/main.c b/src/main.c deleted file mode 100644 index 973ebbd..0000000 --- a/src/main.c +++ /dev/null @@ -1,102 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2013 - 2015 Robert Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "probing.h" -#include "homing.h" -#include "home.h" -#include "i2c.h" - -#include "plan/planner.h" -#include "plan/arc.h" -#include "plan/state.h" - -#include -#include - -#include -#include - - -int main() { - //wdt_enable(WDTO_250MS); TODO - - // Init - cli(); // disable interrupts - - hardware_init(); // hardware setup - must be first - usart_init(); // serial port - i2c_init(); // i2c port - drv8711_init(); // motor drivers - stepper_init(); // steppers - motor_init(); // motors - switch_init(); // switches - mp_init(); // motion planning - machine_init(); // gcode machine - vars_init(); // configuration variables - estop_init(); // emergency stop handler - command_init(); - - sei(); // enable interrupts - - // Splash - fprintf_P(stdout, PSTR("\n{\"firmware\": \"Buildbotics AVR\", " - "\"version\": \"" VERSION "\"}\n")); - - // Nominal segment time cannot be longer than maximum - if (MAX_SEGMENT_TIME < NOM_SEGMENT_TIME) ALARM(STAT_INTERNAL_ERROR); - - // Main loop - while (true) { - hw_reset_handler(); // handle hard reset requests - if (!estop_triggered()) { - mp_state_callback(); - mach_arc_callback(); // arc generation runs - home_callback(); - //mach_homing_callback(); // G28.2 continuation - mach_probe_callback(); // G38.2 continuation - } - command_callback(); // process next command - report_callback(); // report changes - wdt_reset(); - } - - return 0; -} diff --git a/src/messages.def b/src/messages.def deleted file mode 100644 index c3f8ed6..0000000 --- a/src/messages.def +++ /dev/null @@ -1,114 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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") - -// Errors and warnings -STAT_MSG(MINIMUM_LENGTH_MOVE, "Move less than minimum length") -STAT_MSG(MINIMUM_TIME_MOVE, "Move less than minimum time") -STAT_MSG(MAXIMUM_TIME_MOVE, "Move greater than maximum time") -STAT_MSG(MACHINE_ALARMED, "Machine alarmed - Command not processed") -STAT_MSG(LIMIT_SWITCH_HIT, "Limit switch hit - Shutdown occurred") -STAT_MSG(SOFT_LIMIT_EXCEEDED, "Soft limit exceeded") -STAT_MSG(INVALID_AXIS, "Invalid axis") -STAT_MSG(EXPECTED_MOVE, "A move was expected but none was queued") - -// Homing -STAT_MSG(HOMING_CYCLE_FAILED, "Homing cycle failed") -STAT_MSG(HOMING_ERROR_BAD_OR_NO_AXIS, "Homing Error - Bad or no axis specified") -STAT_MSG(HOMING_ERROR_AXIS_MISCONFIGURED, "Homing Error - Axis misconfigured") -STAT_MSG(HOMING_ERROR_ZERO_SEARCH_VELOCITY, - "Homing Error - Search velocity is zero") -STAT_MSG(HOMING_ERROR_ZERO_LATCH_VELOCITY, - "Homing Error - Latch velocity is zero") -STAT_MSG(HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL, - "Homing Error - Travel min & max are the same") -STAT_MSG(HOMING_ERROR_NEGATIVE_LATCH_BACKOFF, - "Homing Error - Negative latch backoff") -STAT_MSG(HOMING_ERROR_SWITCH_MISCONFIGURATION, - "Homing Error - Homing switches misconfigured") - -// Probing -STAT_MSG(PROBE_INVALID_DEST, "Probing error - invalid destination") -STAT_MSG(MOVE_DURING_PROBE, "Probing error - move during probe") - -// End of stats marker -STAT_MSG(MAX, "") diff --git a/src/motor.c b/src/motor.c deleted file mode 100644 index 43221ae..0000000 --- a/src/motor.c +++ /dev/null @@ -1,624 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "plan/runtime.h" -#include "plan/calibrate.h" - -#include -#include - -#include -#include -#include -#include - - -typedef enum { - MOTOR_IDLE, // motor stopped and may be partially energized - MOTOR_ENERGIZING, - MOTOR_ACTIVE -} motor_power_state_t; - - -typedef struct { - // Config - uint8_t axis; // map motor to axis - uint16_t microsteps; // microsteps per full step - bool reverse; - motor_power_mode_t power_mode; - float step_angle; // degrees per whole step - float travel_rev; // mm or deg of travel per motor revolution - uint8_t step_pin; - uint8_t dir_pin; - TC0_t *timer; - DMA_CH_t *dma; - uint8_t dma_trigger; - - // Runtime state - motor_power_state_t power_state; // state machine for managing motor power - uint32_t timeout; - motor_flags_t flags; - int32_t encoder; - int32_t commanded; - int32_t steps; // Currently used by the x-axis only - uint8_t last_clock; - bool wasEnabled; - int32_t error; - bool last_negative; // Last step sign - bool reading; - - // Move prep - uint8_t timer_clock; // clock divisor setting or zero for off - uint16_t timer_period; // clock period counter - bool negative; // step sign - direction_t direction; // travel direction corrected for polarity - int32_t position; - bool round_up; // toggle rounding direction - float power; -} motor_t; - - -static motor_t motors[MOTORS] = { - { - .axis = M1_AXIS, - .step_angle = M1_STEP_ANGLE, - .travel_rev = M1_TRAVEL_PER_REV, - .microsteps = M1_MICROSTEPS, - .reverse = M1_REVERSE, - .power_mode = M1_POWER_MODE, - .step_pin = STEP_X_PIN, - .dir_pin = DIR_X_PIN, - .timer = &M1_TIMER, - .dma = &M1_DMA_CH, - .dma_trigger = M1_DMA_TRIGGER, - }, { - .axis = M2_AXIS, - .step_angle = M2_STEP_ANGLE, - .travel_rev = M2_TRAVEL_PER_REV, - .microsteps = M2_MICROSTEPS, - .reverse = M2_REVERSE, - .power_mode = M2_POWER_MODE, - .step_pin = STEP_Y_PIN, - .dir_pin = DIR_Y_PIN, - .timer = &M2_TIMER, - .dma = &M2_DMA_CH, - .dma_trigger = M2_DMA_TRIGGER, - }, { - .axis = M3_AXIS, - .step_angle = M3_STEP_ANGLE, - .travel_rev = M3_TRAVEL_PER_REV, - .microsteps = M3_MICROSTEPS, - .reverse = M3_REVERSE, - .power_mode = M3_POWER_MODE, - .step_pin = STEP_Z_PIN, - .dir_pin = DIR_Z_PIN, - .timer = &M3_TIMER, - .dma = &M3_DMA_CH, - .dma_trigger = M3_DMA_TRIGGER, - }, { - .axis = M4_AXIS, - .step_angle = M4_STEP_ANGLE, - .travel_rev = M4_TRAVEL_PER_REV, - .microsteps = M4_MICROSTEPS, - .reverse = M4_REVERSE, - .power_mode = M4_POWER_MODE, - .step_pin = STEP_A_PIN, - .dir_pin = DIR_A_PIN, - .timer = (TC0_t *)&M4_TIMER, - .dma = &M4_DMA_CH, - .dma_trigger = M4_DMA_TRIGGER, - } -}; - - -static uint8_t _dummy; - - -void motor_init() { - // Enable DMA - DMA.CTRL = DMA_RESET_bm; - DMA.CTRL = DMA_ENABLE_bm; - DMA.INTFLAGS = 0xff; // clear all interrupts - - // Motor enable - OUTSET_PIN(MOTOR_ENABLE_PIN); // Low (disabled) - DIRSET_PIN(MOTOR_ENABLE_PIN); // Output - - for (int motor = 0; motor < MOTORS; motor++) { - motor_t *m = &motors[motor]; - - axis_set_motor(m->axis, motor); - - // IO pins - DIRSET_PIN(m->step_pin); // Output - DIRSET_PIN(m->dir_pin); // Output - - // Setup motor timer - m->timer->CTRLB = TC_WGMODE_FRQ_gc | TC1_CCAEN_bm; - - // Setup DMA channel as timer event counter - m->dma->ADDRCTRL = DMA_CH_SRCDIR_FIXED_gc | DMA_CH_DESTDIR_FIXED_gc; - m->dma->TRIGSRC = m->dma_trigger; - m->dma->REPCNT = 0; - - // Note, the DMA transfer must read CCA to clear the trigger - m->dma->SRCADDR0 = (((uintptr_t)&m->timer->CCA) >> 0) & 0xff; - m->dma->SRCADDR1 = (((uintptr_t)&m->timer->CCA) >> 8) & 0xff; - m->dma->SRCADDR2 = 0; - - m->dma->DESTADDR0 = (((uintptr_t)&_dummy) >> 0) & 0xff; - m->dma->DESTADDR1 = (((uintptr_t)&_dummy) >> 8) & 0xff; - m->dma->DESTADDR2 = 0; - - m->dma->CTRLB = 0; - m->dma->CTRLA = - DMA_CH_REPEAT_bm | DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc; - m->dma->CTRLA |= DMA_CH_ENABLE_bm; - - drv8711_set_microsteps(motor, m->microsteps); - } -} - - -void motor_enable(int motor, bool enable) { - if (enable) OUTSET_PIN(MOTOR_ENABLE_PIN); // Active high - else { - OUTCLR_PIN(MOTOR_ENABLE_PIN); - motors[motor].power_state = MOTOR_IDLE; - } -} - - -bool motor_is_enabled(int motor) { - return motors[motor].power_mode != MOTOR_DISABLED; -} - - -int motor_get_axis(int motor) {return motors[motor].axis;} - - -void motor_set_stall_callback(int motor, stall_callback_t cb) { - drv8711_set_stall_callback(motor, cb); -} - - -/// @return computed homing velocity -float motor_get_stall_homing_velocity(int motor) { - // Compute velocity: - // velocity = travel_rev * step_angle * 60 / (SMPLTH * mstep * 360 * 2) - // SMPLTH = 50us = 0.00005s - // mstep = 8 - return motors[motor].travel_rev * motors[motor].step_angle * 1667 / - motors[motor].microsteps; -} - - -float motor_get_steps_per_unit(int motor) { - return 360 * motors[motor].microsteps / motors[motor].travel_rev / - motors[motor].step_angle; -} - - -float motor_get_units_per_step(int motor) { - return motors[motor].travel_rev * motors[motor].step_angle / - motors[motor].microsteps / 360; -} - - -float _get_max_velocity(int motor) { - return - axis_get_velocity_max(motors[motor].axis) * motor_get_steps_per_unit(motor); -} - - -uint16_t motor_get_microsteps(int motor) {return motors[motor].microsteps;} - - -void motor_set_microsteps(int motor, uint16_t microsteps) { - switch (microsteps) { - case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256: - break; - default: return; - } - - motors[motor].microsteps = microsteps; - drv8711_set_microsteps(motor, microsteps); -} - - -int32_t motor_get_encoder(int motor) {return motors[motor].encoder;} - - -void motor_set_encoder(int motor, float encoder) { - //if (st_is_busy()) ALARM(STAT_INTERNAL_ERROR); TODO - - motor_t *m = &motors[motor]; - m->encoder = m->position = m->commanded = round(encoder); - m->error = 0; -} - - -int32_t motor_get_error(int motor) { - motor_t *m = &motors[motor]; - - m->reading = true; - int32_t error = motors[motor].error; - m->reading = false; - - return error; -} - - -int32_t motor_get_position(int motor) {return motors[motor].position;} - - -/// returns true if motor is in an error state -bool motor_error(int m) { - uint8_t flags = motors[m].flags; - if (calibrate_busy()) flags &= ~MOTOR_FLAG_STALLED_bm; - return flags & MOTOR_FLAG_ERROR_bm; -} - - -bool motor_stalled(int m) { - return motors[m].flags & MOTOR_FLAG_STALLED_bm; -} - - -void motor_reset(int m) { - motors[m].flags = 0; -} - - -/// Remove power from a motor -static void _deenergize(int m) { - if (motors[m].power_state == MOTOR_ACTIVE) { - motors[m].power_state = MOTOR_IDLE; - drv8711_disable(m); - } -} - - -/// Apply power to a motor -static void _energize(int m) { - if (motors[m].power_state == MOTOR_IDLE && !motor_error(m)) { - motors[m].power_state = MOTOR_ENERGIZING; - drv8711_enable(m); - - motor_driver_callback(m); // TODO Shouldn't call this directly - } - - // Reset timeout, regardless - motors[m].timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000; -} - - -bool motor_energizing() { - for (int m = 0; m < MOTORS; m++) - if (motors[m].power_state == MOTOR_ENERGIZING) - return true; - - return false; -} - - -void motor_driver_callback(int motor) { - motor_t *m = &motors[motor]; - - if (m->power_state == MOTOR_IDLE) m->flags &= ~MOTOR_FLAG_ENABLED_bm; - else if (!estop_triggered()) { - m->power_state = MOTOR_ACTIVE; - m->flags |= MOTOR_FLAG_ENABLED_bm; - motor_enable(motor, true); - } - - report_request(); -} - - -/// Callback to manage motor power sequencing and power-down timing. -stat_t motor_rtc_callback() { // called by controller - for (int motor = 0; motor < MOTORS; motor++) - // Deenergize motor if disabled or timedout - if (motors[motor].power_mode == MOTOR_DISABLED || - rtc_expired(motors[motor].timeout)) _deenergize(motor); - - return STAT_OK; -} - - -void motor_error_callback(int motor, motor_flags_t errors) { - motor_t *m = &motors[motor]; - - if (m->power_state != MOTOR_ACTIVE) return; - - m->flags |= errors & MOTOR_FLAG_ERROR_bm; - report_request(); - - if (motor_error(motor)) { - if (m->flags & MOTOR_FLAG_STALLED_bm) ALARM(STAT_MOTOR_STALLED); - if (m->flags & MOTOR_FLAG_OVER_TEMP_bm) ALARM(STAT_MOTOR_OVER_TEMP); - if (m->flags & MOTOR_FLAG_OVER_CURRENT_bm) ALARM(STAT_MOTOR_OVER_CURRENT); - if (m->flags & MOTOR_FLAG_DRIVER_FAULT_bm) ALARM(STAT_MOTOR_DRIVER_FAULT); - if (m->flags & MOTOR_FLAG_UNDER_VOLTAGE_bm) ALARM(STAT_MOTOR_UNDER_VOLTAGE); - } -} - - -void motor_load_move(int motor) { - motor_t *m = &motors[motor]; - - // Get actual step count from DMA channel - uint16_t steps = 0xffff - m->dma->TRFCNT; - m->dma->TRFCNT = 0xffff; // Reset DMA channel counter - m->dma->CTRLB = DMA_CH_CHBUSY_bm | DMA_CH_CHPEND_bm; - m->dma->CTRLA |= DMA_CH_ENABLE_bm; - - // Adjust clock count - if (m->last_clock) { - uint32_t count = m->timer->CNT; - int8_t freq_change = m->last_clock - m->timer_clock; - - count <<= freq_change; // Adjust count - - if (m->timer_period < count) count -= m->timer_period; - if (m->timer_period < count) count -= m->timer_period; - if (m->timer_period < count) count = m->timer_period / 2; - - m->timer->CNT = count; - - } else m->timer->CNT = m->timer_period / 2; - - // Set or zero runtime clock and period - m->timer->CCA = m->timer_period; // Set frequency - m->timer->CTRLA = m->timer_clock; // Start or stop - m->last_clock = m->timer_clock; - m->timer_clock = 0; // Clear clock - - // Set direction - if (m->direction == DIRECTION_CW) OUTCLR_PIN(m->dir_pin); - else OUTSET_PIN(m->dir_pin); - - // Accumulate encoder - if (!m->wasEnabled) steps = 0; - - m->encoder += m->last_negative ? -(int32_t)steps : steps; - m->last_negative = m->negative; - - // Compute error - if (!m->reading) m->error = m->commanded - m->encoder; - m->commanded = m->position; - - // Set power - drv8711_set_power(motor, m->power); -} - - -void motor_end_move(int motor) { - motor_t *m = &motors[motor]; - m->wasEnabled = m->dma->CTRLA & DMA_CH_ENABLE_bm; - m->dma->CTRLA &= ~DMA_CH_ENABLE_bm; - m->timer->CTRLA = 0; // Stop clock -} - - -stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error, - float time) { - motor_t *m = &motors[motor]; - - // Validate input - if (motor < 0 || MOTORS < motor) return ALARM(STAT_INTERNAL_ERROR); - if (clocks < 0) return ALARM(STAT_INTERNAL_ERROR); - if (isinf(target)) return ALARM(STAT_MOVE_TARGET_INFINITE); - if (isnan(target)) return ALARM(STAT_MOVE_TARGET_NAN); - - // Compute motor timer clock and period. Rounding is performed to eliminate - // a negative bias in the uint32_t conversion that results in long-term - // negative drift. - int32_t travel = round(target) - m->position + error; - uint32_t ticks_per_step = travel ? labs(clocks / 2 / travel) : 0; - m->position = round(target); - - // Setup the direction, compensating for polarity. - m->negative = travel < 0; - if (m->negative) m->direction = DIRECTION_CCW ^ m->reverse; - else m->direction = DIRECTION_CW ^ m->reverse; - - // Find the clock rate that will fit the required number of steps - if (ticks_per_step <= 0xffff) m->timer_clock = TC_CLKSEL_DIV1_gc; - else if (ticks_per_step <= 0x1ffff) m->timer_clock = TC_CLKSEL_DIV2_gc; - else if (ticks_per_step <= 0x3ffff) m->timer_clock = TC_CLKSEL_DIV4_gc; - else if (ticks_per_step <= 0x7ffff) m->timer_clock = TC_CLKSEL_DIV8_gc; - else m->timer_clock = 0; // Clock off, too slow - - // Note, we rely on the fact that TC_CLKSEL_DIV1_gc through TC_CLKSEL_DIV8_gc - // equal 1, 2, 3 & 4 respectively. - m->timer_period = ticks_per_step >> (m->timer_clock - 1); - - // Round up if DIV4 or DIV8 and the error is high enough - if (0xffff < ticks_per_step && m->timer_period < 0xffff) { - int8_t step_error = ticks_per_step & ((1 << (m->timer_clock - 1)) - 1); - int8_t half_error = 1 << (m->timer_clock - 2); - - if (step_error == half_error) { - if (m->round_up) m->timer_period++; - m->round_up = !m->round_up; - - } else if (half_error < step_error) m->timer_period++; - } - - if (!m->timer_period) m->timer_clock = 0; - if (!m->timer_clock) m->timer_period = 0; - - // Power motor - switch (m->power_mode) { - case MOTOR_DISABLED: break; - - case MOTOR_POWERED_ONLY_WHEN_MOVING: - if (!m->timer_clock) break; // Not moving - // Fall through - - case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: - _energize(motor); // TODO is ~5ms enough time to enable the motor? - break; - - default: break; // Shouldn't get here - } - - // Compute power from axis velocity - m->power = travel / (_get_max_velocity(motor) * time); - - return STAT_OK; -} - - -// Var callbacks -char get_axis_name(int motor) {return axis_get_char(motors[motor].axis);} - - -void set_axis_name(int motor, char axis) { - int id = axis_get_id(axis); - if (id != -1) motors[motor].axis = id; -} - - -float get_step_angle(int motor) {return motors[motor].step_angle;} -void set_step_angle(int motor, float value) {motors[motor].step_angle = value;} -float get_travel(int motor) {return motors[motor].travel_rev;} -void set_travel(int motor, float value) {motors[motor].travel_rev = value;} -uint16_t get_microstep(int motor) {return motors[motor].microsteps;} - - -void set_microstep(int motor, uint16_t value) { - if (motor < 0 || MOTORS <= motor) return; - motor_set_microsteps(motor, value); -} - - -bool get_reverse(int motor) { - if (motor < 0 || MOTORS <= motor) return 0; - return motors[motor].reverse; -} - - -void set_reverse(int motor, bool value) {motors[motor].reverse = value;} -uint8_t get_motor_axis(int motor) {return motors[motor].axis;} - - -void set_motor_axis(int motor, uint8_t value) { - if (MOTORS <= motor || AXES <= value || value == motors[motor].axis) return; - axis_set_motor(motors[motor].axis, -1); - motors[motor].axis = value; - axis_set_motor(value, motor); -} - - -uint8_t get_power_mode(int motor) {return motors[motor].power_mode;} - - -void set_power_mode(int motor, uint8_t value) { - if (value < MOTOR_POWER_MODE_MAX_VALUE) - motors[motor].power_mode = value; -} - - -uint8_t get_status_flags(int motor) {return motors[motor].flags;} - - -void print_status_flags(uint8_t flags) { - bool first = true; - - putchar('"'); - - if (MOTOR_FLAG_ENABLED_bm & flags) { - printf_P(PSTR("enable")); - first = false; - } - - if (MOTOR_FLAG_STALLED_bm & flags) { - if (!first) printf_P(PSTR(", ")); - printf_P(PSTR("stall")); - first = false; - } - - if (MOTOR_FLAG_OVER_TEMP_bm & flags) { - if (!first) printf_P(PSTR(", ")); - printf_P(PSTR("over temp")); - first = false; - } - - if (MOTOR_FLAG_OVER_CURRENT_bm & flags) { - if (!first) printf_P(PSTR(", ")); - printf_P(PSTR("over current")); - first = false; - } - - if (MOTOR_FLAG_DRIVER_FAULT_bm & flags) { - if (!first) printf_P(PSTR(", ")); - printf_P(PSTR("fault")); - first = false; - } - - if (MOTOR_FLAG_UNDER_VOLTAGE_bm & flags) { - if (!first) printf_P(PSTR(", ")); - printf_P(PSTR("uvlo")); - first = false; - } - - putchar('"'); -} - - -uint8_t get_status_strings(int m) {return get_status_flags(m);} - - -// Command callback -void command_mreset(int argc, char *argv[]) { - if (argc == 1) - for (int m = 0; m < MOTORS; m++) - motor_reset(m); - - else { - int m = atoi(argv[1]); - if (m < MOTORS) motor_reset(m); - } - - report_request(); -} diff --git a/src/motor.h b/src/motor.h deleted file mode 100644 index 3575afc..0000000 --- a/src/motor.h +++ /dev/null @@ -1,93 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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_FLAG_ENABLED_bm = 1 << 0, - MOTOR_FLAG_STALLED_bm = 1 << 1, - MOTOR_FLAG_OVER_TEMP_bm = 1 << 2, - MOTOR_FLAG_OVER_CURRENT_bm = 1 << 3, - MOTOR_FLAG_DRIVER_FAULT_bm = 1 << 4, - MOTOR_FLAG_UNDER_VOLTAGE_bm = 1 << 5, - MOTOR_FLAG_ERROR_bm = (//MOTOR_FLAG_STALLED_bm | - MOTOR_FLAG_OVER_TEMP_bm | - MOTOR_FLAG_OVER_CURRENT_bm | - MOTOR_FLAG_DRIVER_FAULT_bm | - MOTOR_FLAG_UNDER_VOLTAGE_bm) -} motor_flags_t; - - -typedef enum { - MOTOR_DISABLED, // motor enable is deactivated - MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON - MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, - // de-powered out of cycle - MOTOR_POWERED_ONLY_WHEN_MOVING, // idles shortly after stopped, even in cycle - MOTOR_POWER_MODE_MAX_VALUE // for input range checking -} motor_power_mode_t; - - -typedef void (*stall_callback_t)(int motor); - - -void motor_init(); -void motor_enable(int motor, bool enable); - -bool motor_is_enabled(int motor); -int motor_get_axis(int motor); -void motor_set_stall_callback(int motor, stall_callback_t cb); -float motor_get_stall_homing_velocity(int motor); -float motor_get_steps_per_unit(int motor); -float motor_get_units_per_step(int motor); -uint16_t motor_get_microsteps(int motor); -void motor_set_microsteps(int motor, uint16_t microsteps); -int32_t motor_get_encoder(int motor); -void motor_set_encoder(int motor, float encoder); -int32_t motor_get_error(int motor); -int32_t motor_get_position(int motor); - -bool motor_error(int motor); -bool motor_stalled(int motor); -void motor_reset(int motor); - -bool motor_energizing(); - -void motor_driver_callback(int motor); -stat_t motor_rtc_callback(); -void motor_error_callback(int motor, motor_flags_t errors); - -void motor_load_move(int motor); -void motor_end_move(int motor); -stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error, - float time); diff --git a/src/pins.c b/src/pins.c deleted file mode 100644 index 99f8a55..0000000 --- a/src/pins.c +++ /dev/null @@ -1,31 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/pins.h b/src/pins.h deleted file mode 100644 index c972333..0000000 --- a/src/pins.h +++ /dev/null @@ -1,47 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - - -enum {PORT_A = 1, PORT_B, PORT_C, PORT_D, PORT_E, PORT_F}; - -extern PORT_t *pin_ports[]; - -#define PORT(PIN) pin_ports[(PIN >> 3) - 1] -#define BM(PIN) (1 << (PIN & 7)) - -#define DIRSET_PIN(PIN) PORT(PIN)->DIRSET = BM(PIN) -#define DIRCLR_PIN(PIN) PORT(PIN)->DIRCLR = BM(PIN) -#define OUTCLR_PIN(PIN) PORT(PIN)->OUTCLR = BM(PIN) -#define OUTSET_PIN(PIN) PORT(PIN)->OUTSET = BM(PIN) -#define OUTTGL_PIN(PIN) PORT(PIN)->OUTTGL = BM(PIN) -#define OUT_PIN(PIN) (!!(PORT(PIN)->OUT & BM(PIN))) -#define IN_PIN(PIN) (!!(PORT(PIN)->IN & BM(PIN))) -#define PINCTRL_PIN(PIN) ((&PORT(PIN)->PIN0CTRL)[PIN & 7]) diff --git a/src/plan/arc.c b/src/plan/arc.c deleted file mode 100644 index 957a617..0000000 --- a/src/plan/arc.c +++ /dev/null @@ -1,507 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - - -typedef struct { - bool running; - - float target[AXES]; // XYZABC where the move should go - float position[AXES]; // end point of the current segment - - float theta; // total angle specified by arc - float radius; // Raw R value, or computed via offsets - - uint8_t plane_axis_0; // arc plane axis 0 - e.g. X for G17 - uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17 - uint8_t linear_axis; // linear axis (normal to plane) - - uint32_t segments; // count of running segments - float segment_theta; // angular motion per segment - float segment_linear_travel; // linear motion per segment - float center_0; // center at axis 0 (e.g. X for G17) - float center_1; // center at axis 1 (e.g. Y for G17) -} arc_t; - -arc_t arc = {0}; - - -/*** Returns a naive estimate of arc execution time to inform segment - * calculation. The arc time is computed not to exceed the time taken - * in the slowest dimension in the arc plane or in linear - * travel. Maximum feed rates are compared in each dimension, but the - * comparison assumes that the arc will have at least one segment - * where the unit vector is 1 in that dimension. This is not true for - * any arbitrary arc, with the result that the time returned may be - * less than optimal. - */ -static float _estimate_arc_time(float length, float linear_travel, - float planar_travel) { - // Determine move time at requested feed rate - // Inverse feed rate is normalized to minutes - float time = mach_is_inverse_time_mode() ? - mach_get_feed_rate() : length / mach_get_feed_rate(); - - - // Apply feed override - time /= mach_get_feed_override(); - - // Downgrade the time if there is a rate-limiting axis - return max4(time, planar_travel / axis_get_feedrate_max(arc.plane_axis_0), - planar_travel / axis_get_feedrate_max(arc.plane_axis_1), - fabs(linear_travel) / axis_get_feedrate_max(arc.linear_axis)); -} - - -/*** Compute arc center (offset) from radius. - * - * Needs to calculate the center of the circle that has the designated radius - * and passes through both the current position and the target position - * - * This method calculates the following set of equations where: - * - * [x,y] is the vector from current to target position, - * d == magnitude of that vector, - * h == hypotenuse of the triangle formed by the radius of the circle, - * the distance to the center of the travel vector. - * - * A vector perpendicular to the travel vector [-y,x] is scaled to the length - * of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] - * to form the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the - * center of the arc. - * - * d^2 == x^2 + y^2 - * h^2 == r^2 - (d/2)^2 - * i == x/2 - y/d*h - * j == y/2 + x/d*h - * O <- [i,j] - * - | - * r - | - * - | - * - | h - * - | - * [0,0] -> C -----------------+--------------- T <- [x,y] - * | <------ d/2 ---->| - * - * C - Current position - * T - Target position - * O - center of circle that pass through both C and T - * d - distance from C to T - * r - designated radius - * h - distance from center of CT to O - * - * Expanding the equations: - * - * d -> sqrt(x^2 + y^2) - * h -> sqrt(4 * r^2 - x^2 - y^2)/2 - * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - * - * Which can be written: - * - * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - * - * Which we for size and speed reasons optimize to: - * - * h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) - * i = (x - (y * h_x2_div_d))/2 - * j = (y + (x * h_x2_div_d))/2 - * - * Computing clockwise vs counter-clockwise motion - * - * The counter clockwise circle lies to the left of the target direction. - * When offset is positive the left hand circle will be generated - - * when it is negative the right hand circle is generated. - * - * T <-- Target position - * ^ - * Clockwise circles with | Clockwise circles with - * this center will have | this center will have - * > 180 deg of angular travel | < 180 deg of angular travel, - * \ | which is a good thing! - * \ | / - * center of arc when -> x <----- | -----> x <- center of arc when - * h_x2_div_d is positive | h_x2_div_d is negative - * | - * C <-- Current position - * - * - * Assumes arc singleton has been pre-loaded with target and position. - * Parts of this routine were originally sourced from the grbl project. - */ -static stat_t _compute_arc_offsets_from_radius(float offset[], bool clockwise) { - // Calculate the change in position along each selected axis - float x = - arc.target[arc.plane_axis_0] - mach_get_axis_position(arc.plane_axis_0); - float y = - arc.target[arc.plane_axis_1] - mach_get_axis_position(arc.plane_axis_1); - - // *** From Forrest Green - Other Machine Co, 3/27/14 - // If the distance between endpoints is greater than the arc diameter, disc - // will be negative indicating that the arc is offset into the complex plane - // beyond the reach of any real CNC. However, numerical errors can flip the - // sign of disc as it approaches zero (which happens as the arc angle - // approaches 180 degrees). To avoid mishandling these arcs we use the - // closest real solution (which will be 0 when disc <= 0). This risks - // obscuring g-code errors where the radius is actually too small (they will - // be treated as half circles), but ensures that all valid arcs end up - // reasonably close to their intended paths regardless of any numerical - // issues. - float disc = 4 * square(arc.radius) - (square(x) + square(y)); - - float h_x2_div_d = disc > 0 ? -sqrt(disc) / hypotf(x, y) : 0; - - // Invert sign of h_x2_div_d if circle is counter clockwise (see header notes) - if (!clockwise) h_x2_div_d = -h_x2_div_d; - - // Negative R is g-code-alese for "I want a circle with more than 180 degrees - // of travel" (go figure!), even though it is advised against ever generating - // such circles in a single line of g-code. By inverting the sign of - // h_x2_div_d the center of the circles is placed on the opposite side of - // the line of travel and thus we get the unadvisably long arcs as prescribed. - if (arc.radius < 0) h_x2_div_d = -h_x2_div_d; - - // Complete the operation by calculating the actual center of the arc - offset[arc.plane_axis_0] = (x - y * h_x2_div_d) / 2; - offset[arc.plane_axis_1] = (y + x * h_x2_div_d) / 2; - offset[arc.linear_axis] = 0; - - return STAT_OK; -} - - -/* Compute arc from I and J (arc center point) - * - * The theta calculation sets up an clockwise or counterclockwise arc - * from the current position to the target position around the center - * designated by the offset vector. All theta-values measured in - * radians of deviance from the positive y-axis. - * - * | <- theta == 0 - * * * * - * * * - * * * - * * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2) - * * / - * C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4)) - * - * Parts of this routine were originally sourced from the grbl project. - */ -static stat_t _compute_arc(bool radius_f, const float position[], - float offset[], float rotations, bool clockwise, - bool full_circle) { - // Compute radius. A non-zero radius value indicates a radius arc - if (radius_f) _compute_arc_offsets_from_radius(offset, clockwise); - else // compute start radius - arc.radius = hypotf(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]); - - // Test arc specification for correctness according to: - // http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc - // "It is an error if: when the arc is projected on the selected - // plane, the distance from the current point to the center differs - // from the distance from the end point to the center by more than - // (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)." - - // Compute end radius from the center of circle (offsets) to target endpoint - float end_0 = arc.target[arc.plane_axis_0] - - position[arc.plane_axis_0] - offset[arc.plane_axis_0]; - - float end_1 = arc.target[arc.plane_axis_1] - - position[arc.plane_axis_1] - offset[arc.plane_axis_1]; - - // end radius - start radius - float err = fabs(hypotf(end_0, end_1) - arc.radius); - - if (err > ARC_RADIUS_ERROR_MAX || - (err < ARC_RADIUS_ERROR_MIN && err > arc.radius * ARC_RADIUS_TOLERANCE)) - return STAT_ARC_SPECIFICATION_ERROR; - - // Calculate the theta (angle) of the current point (position) - // arc.theta is angular starting point for the arc (also needed later for - // calculating center point) - arc.theta = atan2(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]); - - // g18_correction is used to invert G18 XZ plane arcs for proper CW - // orientation - float g18_correction = mach_get_plane() == PLANE_XZ ? -1 : 1; - float angular_travel = 0; - - if (full_circle) { - // angular travel always starts as zero for full circles - angular_travel = 0; - - // handle the valid case of a full circle arc w/ P=0 - if (fp_ZERO(rotations)) rotations = 1.0; - - } else { - float theta_end = atan2(end_0, end_1); - - // Compute the angular travel - if (fp_EQ(theta_end, arc.theta)) - // very large radii arcs can have zero angular travel (thanks PartKam) - angular_travel = 0; - - else { - // make the difference positive so we have clockwise travel - if (theta_end < arc.theta) theta_end += 2 * M_PI * g18_correction; - - // compute positive angular travel - angular_travel = theta_end - arc.theta; - - // reverse travel direction if it's CCW arc - if (!clockwise) angular_travel -= 2 * M_PI * g18_correction; - } - } - - // Add in travel for rotations - if (clockwise) angular_travel += 2 * M_PI * rotations * g18_correction; - else angular_travel -= 2 * M_PI * rotations * g18_correction; - - // Calculate travel in the depth axis of the helix and compute the time it - // should take to perform the move length is the total mm of travel of - // the helix (or just a planar arc) - float linear_travel = arc.target[arc.linear_axis] - position[arc.linear_axis]; - float planar_travel = angular_travel * arc.radius; - // hypot is insensitive to +/- signs - float length = hypotf(planar_travel, linear_travel); - - // trap zero length arcs that _compute_arc can throw - if (fp_ZERO(length)) return STAT_MINIMUM_LENGTH_MOVE; - - // get an estimate of execution time to inform segment calculation - float arc_time = _estimate_arc_time(length, linear_travel, planar_travel); - - // Find the minimum number of segments that meets these constraints... - float segments_for_chordal_accuracy = - length / sqrt(4 * CHORDAL_TOLERANCE * (2 * arc.radius - CHORDAL_TOLERANCE)); - float segments_for_minimum_distance = length / ARC_SEGMENT_LENGTH; - float segments_for_minimum_time = - arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC; - - float segments = floor(min3(segments_for_chordal_accuracy, - segments_for_minimum_distance, - segments_for_minimum_time)); - if (segments < 1) segments = 1; // at least 1 segment - - arc.segments = (uint32_t)segments; - arc.segment_theta = angular_travel / segments; - arc.segment_linear_travel = linear_travel / segments; - arc.center_0 = position[arc.plane_axis_0] - sin(arc.theta) * arc.radius; - arc.center_1 = position[arc.plane_axis_1] - cos(arc.theta) * arc.radius; - - // initialize the linear position - arc.position[arc.linear_axis] = position[arc.linear_axis]; - - return STAT_OK; -} - - -/*** Machine entry point for arc - * - * Generates an arc by queuing line segments to the move buffer. The arc is - * approximated by generating a large number of tiny, linear segments. - */ -stat_t mach_arc_feed(float values[], bool values_f[], // arc endpoints - float offsets[], bool offsets_f[], // arc offsets - float radius, bool radius_f, // radius - float P, bool P_f, // parameter - bool modal_g1_f, - motion_mode_t motion_mode) { // defined motion mode - - // Trap some precursor cases. Since motion mode (MODAL_GROUP_G1) persists - // from the previous move it's possible for non-modal commands such as F or P - // to arrive here when no motion has actually been specified. It's also - // possible to run an arc as simple as "I25" if CW or CCW motion mode was - // already set by a previous block. Here are 2 cases to handle if CW or CCW - // motion mode was set by a previous block: - // - // Case 1: F, P or other non modal is specified but no movement is specified - // (no offsets or radius). This is OK: return STAT_OK - // - // Case 2: Movement is specified w/o a new G2 or G3 word in the (new) block. - // This is OK: continue the move - // - if (!modal_g1_f && !offsets_f[0] && !offsets_f[1] && !offsets_f[2] && - !radius_f) return STAT_OK; - - // trap missing feed rate - if (fp_ZERO(mach_get_feed_rate()) || - (mach_is_inverse_time_mode() && !parser.gf.feed_rate)) - return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - - // radius must be positive and > minimum - if (radius_f && radius < MIN_ARC_RADIUS) - return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; - - // Set the arc plane for the current G17/G18/G19 setting and test arc - // specification Plane axis 0 and 1 are the arc plane, the linear axis is - // normal to the arc plane. - switch (mach_get_plane()) { - case PLANE_XY: // G17 - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Y; - arc.linear_axis = AXIS_Z; - break; - - case PLANE_XZ: // G18 - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_Y; - break; - - case PLANE_YZ: // G19 - arc.plane_axis_0 = AXIS_Y; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_X; - break; - } - - bool clockwise = motion_mode == MOTION_MODE_CW_ARC; - - // Test if endpoints are specified in the selected plane - bool full_circle = false; - if (!values_f[arc.plane_axis_0] && !values_f[arc.plane_axis_1]) { - if (radius_f) // in radius mode arcs missing both endpoints is an error - return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; - else full_circle = true; // in center format arc specifies full circle - } - - // Test radius arcs for radius tolerance - if (radius_f) { - arc.radius = TO_MM(radius); // set to internal format (mm) - if (fabs(arc.radius) < MIN_ARC_RADIUS) // radius value must be > minimum - return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; - - // Test that center format absolute distance mode arcs have both offsets - } else if (mach_get_arc_distance_mode() == ABSOLUTE_MODE && - !(offsets_f[arc.plane_axis_0] && offsets_f[arc.plane_axis_1])) - return STAT_ARC_OFFSETS_MISSING_FOR_PLANE; - - // Set arc rotations - float rotations = 0; - - if (P_f) { - // If P is present it must be a positive integer - if (P < 0 || 0 < floor(P) - P) return STAT_P_WORD_IS_NOT_A_POSITIVE_INTEGER; - - rotations = P; - - } else if (full_circle) rotations = 1; // default to 1 for full circles - - // Set model target - const float *position = mach_get_position(); - mach_calc_target(arc.target, values, values_f); - - // in radius mode it's an error for start == end - if (radius_f && fp_EQ(position[AXIS_X], arc.target[AXIS_X]) && - fp_EQ(position[AXIS_Y], arc.target[AXIS_Y]) && - fp_EQ(position[AXIS_Z], arc.target[AXIS_Z])) - return STAT_ARC_ENDPOINT_IS_STARTING_POINT; - - // now get down to the rest of the work setting up the arc for execution - mach_set_motion_mode(motion_mode); - mach_update_work_offsets(); // Update resolved offsets - arc.radius = TO_MM(radius); // set arc radius or zero - - float offset[3]; - for (int i = 0; i < 3; i++) offset[i] = TO_MM(offsets[i]); - - if (mach_get_arc_distance_mode() == ABSOLUTE_MODE) { - if (offsets_f[0]) offset[0] -= position[0]; - if (offsets_f[1]) offset[1] -= position[1]; - if (offsets_f[2]) offset[2] -= position[2]; - } - - // compute arc runtime values - RITORNO(_compute_arc - (radius_f, position, offset, rotations, clockwise, full_circle)); - - // Note, arc soft limits are not tested here - - arc.running = true; // Enable arc run in callback - mach_arc_callback(); // Queue initial arc moves - mach_set_position(arc.target); // update model position - - return STAT_OK; -} - - -/*** Generate an arc - * - * Called from the controller main loop. Each time it's called it queues - * as many arc segments (lines) as it can before it blocks, then returns. - */ -void mach_arc_callback() { - while (arc.running && mp_queue_get_room()) { - if (arc.segments == 1) { // Final segment - arc.position[arc.plane_axis_0] = arc.target[arc.plane_axis_0]; - arc.position[arc.plane_axis_1] = arc.target[arc.plane_axis_1]; - arc.position[arc.linear_axis] = arc.target[arc.linear_axis]; - - } else { - arc.theta += arc.segment_theta; - - arc.position[arc.plane_axis_0] = - arc.center_0 + sin(arc.theta) * arc.radius; - arc.position[arc.plane_axis_1] = - arc.center_1 + cos(arc.theta) * arc.radius; - arc.position[arc.linear_axis] += arc.segment_linear_travel; - } - - // run the line - mach_plan_line(arc.position); - - if (!--arc.segments) arc.running = false; - } -} - - -bool mach_arc_active() {return arc.running;} - - -/// Stop arc movement without maintaining position -/// OK to call if no arc is running -void mach_abort_arc() {arc.running = false;} diff --git a/src/plan/arc.h b/src/plan/arc.h deleted file mode 100644 index 943aa9e..0000000 --- a/src/plan/arc.h +++ /dev/null @@ -1,48 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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(); -bool mach_arc_active(); -void mach_abort_arc(); diff --git a/src/plan/buffer.c b/src/plan/buffer.c deleted file mode 100644 index 45146d7..0000000 --- a/src/plan/buffer.c +++ /dev/null @@ -1,156 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 { - uint8_t space; - mp_buffer_t *tail; - mp_buffer_t *head; - mp_buffer_t bf[PLANNER_BUFFER_POOL_SIZE]; -} buffer_pool_t; - - -buffer_pool_t mb; - - -/// Zeroes the contents of a buffer -static void _clear_buffer(mp_buffer_t *bf) { - mp_buffer_t *next = bf->next; // save pointers - mp_buffer_t *prev = bf->prev; - memset(bf, 0, sizeof(mp_buffer_t)); - bf->next = next; // restore pointers - bf->prev = prev; -} - - -static void _push() { - if (!mb.space) { - ALARM(STAT_INTERNAL_ERROR); - return; - } - - mb.tail = mb.tail->next; - mb.space--; -} - - -static void _pop() { - if (mb.space == PLANNER_BUFFER_POOL_SIZE) { - ALARM(STAT_INTERNAL_ERROR); - return; - } - - mb.head = mb.head->next; - mb.space++; -} - - -/// Initializes or resets buffers -void mp_queue_init() { - memset(&mb, 0, sizeof(mb)); // clear all values - - mb.tail = mb.head = &mb.bf[0]; // init head and tail - mb.space = PLANNER_BUFFER_POOL_SIZE; - - // Setup ring pointers - for (int i = 0; i < mb.space; i++) { - mb.bf[i].next = &mb.bf[i + 1]; - mb.bf[i].prev = &mb.bf[i - 1]; - } - - mb.bf[0].prev = &mb.bf[mb.space -1]; // Fix first->prev - mb.bf[mb.space - 1].next = &mb.bf[0]; // Fix last->next - - mp_state_idle(); -} - - -uint8_t mp_queue_get_room() { - return mb.space < PLANNER_BUFFER_HEADROOM ? - 0 : mb.space - PLANNER_BUFFER_HEADROOM; -} - - -uint8_t mp_queue_get_fill() { - return PLANNER_BUFFER_POOL_SIZE - mb.space; -} - - -bool mp_queue_is_empty() {return mb.tail == mb.head;} - - -/// Get pointer to next buffer, waiting until one is available. -mp_buffer_t *mp_queue_get_tail() { - while (!mb.space) continue; // Wait for a buffer - return mb.tail; -} - - -/*** Commit the next buffer to the queue. - * - * WARNING: The routine calling mp_queue_push() must not use the write - * buffer once it has been queued. Action may start on the buffer immediately, - * invalidating its contents - */ -void mp_queue_push(buffer_cb_t cb, uint32_t line) { - mp_state_running(); - - mb.tail->ts = rtc_get_time(); - mb.tail->cb = cb; - mb.tail->line = line; - mb.tail->state = BUFFER_NEW; - - _push(); -} - - -mp_buffer_t *mp_queue_get_head() { - return mp_queue_is_empty() ? 0 : mb.head; -} - - -/// Clear and release buffer to pool -void mp_queue_pop() { - _clear_buffer(mb.head); - _pop(); -} diff --git a/src/plan/buffer.h b/src/plan/buffer.h deleted file mode 100644 index 8554fdb..0000000 --- a/src/plan/buffer.h +++ /dev/null @@ -1,102 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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; - - -// Callbacks -struct mp_buffer_t; -typedef stat_t (*buffer_cb_t)(struct mp_buffer_t *bf); - - -typedef struct mp_buffer_t { // See Planning Velocity Notes - struct mp_buffer_t *prev; // pointer to previous buffer - struct mp_buffer_t *next; // pointer to next buffer - - uint32_t ts; // Time stamp - int32_t line; // gcode block line number - buffer_cb_t cb; // callback to buffer exec function - - buffer_state_t state; // buffer state - bool replannable; // true if move can be re-planned - bool hold; // hold at the start of this block - - float value; // used in dwell and other callbacks - - float target[AXES]; // XYZABC where the move should go - float unit[AXES]; // unit vector for axis scaling & planning - - float length; // total length of line or helix in mm - float head_length; - float body_length; - float tail_length; - - // See notes on these variables, in mp_aline() - float entry_velocity; // entry velocity requested for the move - float cruise_velocity; // cruise velocity requested & achieved - float exit_velocity; // exit velocity requested for the move - float braking_velocity; // current value for braking velocity - - float entry_vmax; // max junction velocity at entry of this move - float cruise_vmax; // max cruise velocity requested for move - float exit_vmax; // max exit velocity possible (redundant) - float delta_vmax; // max velocity difference for this move - - float jerk; // maximum linear jerk term for this move - float cbrt_jerk; // cube root of Jm (computed & cached) -} mp_buffer_t; - - -void mp_queue_init(); - -uint8_t mp_queue_get_room(); -uint8_t mp_queue_get_fill(); - -bool mp_queue_is_empty(); - -mp_buffer_t *mp_queue_get_tail(); -void mp_queue_push(buffer_cb_t func, uint32_t line); - -mp_buffer_t *mp_queue_get_head(); -void mp_queue_pop(); - -static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->prev;} -static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->next;} diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c deleted file mode 100644 index ee83d76..0000000 --- a/src/plan/calibrate.c +++ /dev/null @@ -1,175 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - - -#define CAL_VELOCITIES 256 -#define CAL_MIN_VELOCITY 1000 // mm/sec -#define CAL_TARGET_SG 100 -#define CAL_MAX_DELTA_SG 75 -#define CAL_WAIT_TIME 3 // ms - - -enum { - CAL_START, - CAL_ACCEL, - CAL_MEASURE, - CAL_DECEL, -}; - - -typedef struct { - bool stall_valid; - bool stalled; - bool reverse; - - uint32_t wait; - int state; - int motor; - int axis; - - float velocity; - uint16_t stallguard; -} calibrate_t; - -static calibrate_t cal = {0}; - - -static stat_t _exec_calibrate(mp_buffer_t *bf) { - const float time = MIN_SEGMENT_TIME; // In minutes - const float max_delta_v = CAL_ACCELERATION * time; - - do { - if (rtc_expired(cal.wait)) - switch (cal.state) { - case CAL_START: { - cal.axis = motor_get_axis(cal.motor); - cal.state = CAL_ACCEL; - cal.velocity = 0; - cal.stall_valid = false; - cal.stalled = false; - cal.reverse = false; - - //tmc2660_set_stallguard_threshold(cal.motor, 8); - cal.wait = rtc_get_time() + CAL_WAIT_TIME; - - break; - } - - case CAL_ACCEL: - if (CAL_MIN_VELOCITY < cal.velocity) cal.stall_valid = true; - - if (cal.velocity < CAL_MIN_VELOCITY || CAL_TARGET_SG < cal.stallguard) - cal.velocity += max_delta_v; - - if (cal.stalled) { - if (cal.reverse) { - int32_t steps = -motor_get_encoder(cal.motor); - float mm = (float)steps / motor_get_steps_per_unit(cal.motor); - STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm); - - //tmc2660_set_stallguard_threshold(cal.motor, 63); - - mp_set_cycle(CYCLE_MACHINING); // Default cycle - - return STAT_NOOP; // Done, no move queued - - } else { - motor_set_encoder(cal.motor, 0); - - cal.reverse = true; - cal.velocity = 0; - cal.stall_valid = false; - cal.stalled = false; - } - } - break; - } - } while (fp_ZERO(cal.velocity)); // Repeat if computed velocity was zero - - // Compute travel - float travel[AXES] = {0}; // In mm - travel[cal.axis] = time * cal.velocity * (cal.reverse ? -1 : 1); - - // Convert to steps - float steps[MOTORS] = {0}; - mp_kinematics(travel, steps); - - // Queue segment - st_prep_line(time, steps, 0); - - return STAT_EAGAIN; -} - - -bool calibrate_busy() {return mp_get_cycle() == CYCLE_CALIBRATING;} - - -void calibrate_set_stallguard(int motor, uint16_t sg) { - if (cal.motor != motor) return; - - if (cal.stall_valid) { - int16_t delta = sg - cal.stallguard; - if (!sg || CAL_MAX_DELTA_SG < abs(delta)) { - cal.stalled = true; - motor_end_move(cal.motor); - } - } - - cal.stallguard = sg; -} - - -uint8_t command_calibrate(int argc, char *argv[]) { - if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY) - return 0; - - // Start - memset(&cal, 0, sizeof(cal)); - mp_set_cycle(CYCLE_CALIBRATING); - cal.motor = 1; - - mp_queue_push_nonstop(_exec_calibrate, -1); - - return 0; -} diff --git a/src/plan/calibrate.h b/src/plan/calibrate.h deleted file mode 100644 index 4d97c60..0000000 --- a/src/plan/calibrate.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/plan/dwell.c b/src/plan/dwell.c deleted file mode 100644 index 07bba61..0000000 --- a/src/plan/dwell.c +++ /dev/null @@ -1,54 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/plan/dwell.h b/src/plan/dwell.h deleted file mode 100644 index 9d94555..0000000 --- a/src/plan/dwell.h +++ /dev/null @@ -1,32 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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" - -stat_t mp_dwell(float seconds, int32_t line); diff --git a/src/plan/exec.c b/src/plan/exec.c deleted file mode 100644 index 3ae0e03..0000000 --- a/src/plan/exec.c +++ /dev/null @@ -1,450 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "forward_dif.h" -#include "stepper.h" -#include "motor.h" -#include "rtc.h" -#include "usart.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; - - uint32_t segment_count; // count of running segments - float segment_velocity; // computed velocity for segment - float segment_time; // actual time increment per segment - forward_dif_t fdif; // forward difference levels - bool hold_planned; // true when a feedhold has been planned - move_section_t section; // current move section - bool section_new; // true if it's a new section - bool abort; -} mp_exec_t; - - -static mp_exec_t ex = {{0}}; - - -static stat_t _exec_aline_segment() { - float target[AXES]; - - // Set target position for the segment. If the segment ends on a section - // waypoint, synchronize to the head, body or tail end. Otherwise, if not at - // a section waypoint compute target from segment time and velocity. Don't - // do waypoint correction if you are going into a hold. - if (!--ex.segment_count && !ex.section_new && !ex.hold_planned) - copy_vector(target, ex.waypoint[ex.section]); - - else { - float segment_length = ex.segment_velocity * ex.segment_time; - - for (int axis = 0; axis < AXES; axis++) - target[axis] = - mp_runtime_get_axis_position(axis) + ex.unit[axis] * segment_length; - } - - mp_runtime_set_velocity(ex.segment_velocity); - RITORNO(mp_runtime_move_to_target(target, ex.segment_time)); - - // Return EAGAIN to continue or OK if this segment is done - return ex.segment_count ? STAT_EAGAIN : STAT_OK; -} - - -/// Common code for head and tail sections -static stat_t _exec_aline_section(float length, float vin, float vout) { - if (ex.section_new) { - if (fp_ZERO(length)) return STAT_NOOP; // end the section - - // len / avg. velocity - float move_time = 2 * length / (vin + vout); - float segments = ceil(move_time / NOM_SEGMENT_TIME); - ex.segment_time = move_time / segments; - ex.segment_count = round(segments); - - if (vin == vout) ex.segment_velocity = vin; - else ex.segment_velocity = - mp_init_forward_dif(ex.fdif, vin, vout, segments); - - if (ex.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position - - // First segment - if (_exec_aline_segment() == STAT_OK) { - ex.section_new = false; - return STAT_OK; - } - - ex.section_new = false; - - } else { - if (vin != vout) ex.segment_velocity += mp_next_forward_dif(ex.fdif); - - // Subsequent segments - if (_exec_aline_segment() == STAT_OK) return STAT_OK; - } - - return STAT_EAGAIN; -} - - -/// Callback for tail section -static stat_t _exec_aline_tail() { - ex.section = SECTION_TAIL; - return - _exec_aline_section(ex.tail_length, ex.cruise_velocity, ex.exit_velocity); -} - - -/// Callback for body section -static stat_t _exec_aline_body() { - ex.section = SECTION_BODY; - - stat_t status = - _exec_aline_section(ex.body_length, ex.cruise_velocity, ex.cruise_velocity); - - switch (status) { - case STAT_NOOP: return _exec_aline_tail(); - case STAT_OK: - ex.section = SECTION_TAIL; - ex.section_new = true; - - return STAT_EAGAIN; - - default: return status; - } -} - - -/// Callback for head section -static stat_t _exec_aline_head() { - ex.section = SECTION_HEAD; - stat_t status = - _exec_aline_section(ex.head_length, ex.entry_velocity, ex.cruise_velocity); - - switch (status) { - case STAT_NOOP: return _exec_aline_body(); - case STAT_OK: - ex.section = SECTION_BODY; - ex.section_new = true; - - return STAT_EAGAIN; - - default: return status; - } -} - - -static float _compute_next_segment_velocity() { - if (ex.section_new) { - if (ex.section == SECTION_HEAD) return mp_runtime_get_velocity(); - else return ex.cruise_velocity; - } - - return ex.segment_velocity + - (ex.section == SECTION_BODY ? 0 : mp_next_forward_dif(ex.fdif)); -} - - -/// Replan current move to execute hold -/// -/// Holds are initiated by the planner entering STATE_STOPPING. In which case -/// _plan_hold() is called to replan the current move towards zero. If it is -/// unable to plan to zero in the remaining length of the current move it will -/// decelerate as much as possible and then wait for the next move. Once it is -/// possible to plan to zero velocity in the current move the remaining length -/// is put into the run buffer, which is still allocated, and the run buffer -/// becomes the hold point. The hold is left by a start request in state.c. At -/// this point the remaining buffers, if any, are replanned from zero up to -/// speed. -static void _plan_hold() { - mp_buffer_t *bf = mp_queue_get_head(); // working buffer pointer - if (!bf) return; // Oops! nothing's running - - // Examine and process current buffer and compute length left for decel - float available_length = - axis_get_vector_length(ex.final_target, mp_runtime_get_position()); - // Compute next_segment velocity, velocity left to shed to brake to zero - float braking_velocity = _compute_next_segment_velocity(); - // Distance to brake to zero from braking_velocity, bf is OK to use here - float braking_length = mp_get_target_length(braking_velocity, 0, bf->jerk); - - // Hack to prevent Case 2 moves for perfect-fit decels. Happens when homing. - if (available_length < braking_length && fp_ZERO(bf->exit_velocity)) - braking_length = available_length; - - // Replan to decelerate - ex.section = SECTION_TAIL; - ex.section_new = true; - ex.cruise_velocity = braking_velocity; - ex.hold_planned = true; - - // Avoid creating segments before or after the hold which are too small. - if (fabs(available_length - braking_length) < HOLD_DECELERATION_TOLERANCE) { - // Case 0: deceleration fits almost exactly - ex.exit_velocity = 0; - ex.tail_length = available_length; - - } else if (braking_length <= available_length) { - // Case 1: deceleration fits entirely into the remaining length - // Setup tail to perform the deceleration - ex.exit_velocity = 0; - ex.tail_length = braking_length; - - // Re-use bf to run the remaining block length - bf->length = available_length - braking_length; - bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); - bf->entry_vmax = 0; - bf->state = BUFFER_RESTART; // Restart the buffer when done - - } else { - // Case 2: deceleration exceeds length remaining in buffer - // Replan to minimum (but non-zero) exit velocity - ex.tail_length = available_length; - ex.exit_velocity = - braking_velocity - mp_get_target_velocity(0, available_length, bf); - } -} - - -/// Initializes move runtime with a new planner buffer -static stat_t _exec_aline_init(mp_buffer_t *bf) { - // Remove zero length lines. Short lines have already been removed. - if (fp_ZERO(bf->length)) return STAT_NOOP; - - // Initialize move - copy_vector(ex.unit, bf->unit); - copy_vector(ex.final_target, bf->target); - - ex.head_length = bf->head_length; - ex.body_length = bf->body_length; - ex.tail_length = bf->tail_length; - ex.entry_velocity = bf->entry_velocity; - ex.cruise_velocity = bf->cruise_velocity; - ex.exit_velocity = bf->exit_velocity; - - ex.section = SECTION_HEAD; - ex.section_new = true; - ex.hold_planned = false; - - // Generate waypoints for position correction at section ends. This helps - // negate floating point errors in the forward differencing code. - for (int axis = 0; axis < AXES; axis++) { - float position = mp_runtime_get_axis_position(axis); - - ex.waypoint[SECTION_HEAD][axis] = position + ex.unit[axis] * ex.head_length; - ex.waypoint[SECTION_BODY][axis] = position + - ex.unit[axis] * (ex.head_length + ex.body_length); - ex.waypoint[SECTION_TAIL][axis] = ex.final_target[axis]; - } - - return STAT_OK; -} - - -void mp_exec_abort() {ex.abort = true;} - - -/// Aline execution routines -/// -/// Everything here fires from interrupts and must be interrupt safe -/// -/// Returns: -/// -/// STAT_OK move is done -/// STAT_EAGAIN move is not finished - has more segments to run -/// STAT_NOOP cause no stepper operation - do not load the move -/// STAT_xxxxx fatal error. Ends the move and frees the bf buffer -/// -/// This routine is called from the (LO) interrupt level. The interrupt -/// sequencing relies on the correct behavior of these routines. -/// Each call to _exec_aline() must execute and prep *one and only one* -/// segment. If the segment is not the last segment in the bf buffer the -/// _aline() returns STAT_EAGAIN. If it's the last segment it returns -/// STAT_OK. If it encounters a fatal error that would terminate the move it -/// returns a valid error code. -/// -/// Notes: -/// -/// [1] Returning STAT_OK ends the move and frees the bf buffer. -/// Returning STAT_OK at does NOT advance position meaning -/// any position error will be compensated by the next move. -/// -/// Operation: -/// -/// Aline generates jerk-controlled S-curves as per Ed Red's course notes: -/// -/// http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf -/// http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations -/// -/// A full trapezoid is divided into 5 periods. Periods 1 and 2 are the -/// first and second halves of the acceleration ramp (the concave and convex -/// parts of the S curve in the "head"). Periods 3 and 4 are the first -/// and second parts of the deceleration ramp (the tail). There is also -/// a period for the constant-velocity plateau of the trapezoid (the body). -/// There are many possible degenerate trapezoids where any of the 5 periods -/// may be zero length but note that either none or both of a ramping pair can -/// be zero. -/// -/// The equations that govern the acceleration and deceleration ramps are: -/// -/// Period 1 V = Vi + Jm * (T^2) / 2 -/// Period 2 V = Vh + As * T - Jm * (T^2) / 2 -/// Period 3 V = Vi - Jm * (T^2) / 2 -/// Period 4 V = Vh + As * T + Jm * (T^2) / 2 -/// -/// move_time is the actual time of the move, accel_time is the time value -/// needed to compute the velocity taking the initial velocity into account. -/// move_time does not need to. -stat_t mp_exec_aline(mp_buffer_t *bf) { - stat_t status = STAT_OK; - - if (ex.abort) { - ex.abort = false; - mp_runtime_set_velocity(0); // Assume a hard stop - return STAT_NOOP; - } - - // Start a new move - if (bf->state == BUFFER_INIT) { - bf->state = BUFFER_ACTIVE; - status = _exec_aline_init(bf); - if (status != STAT_OK) return status; - } - - // Plan holds - if (mp_get_state() == STATE_STOPPING && !ex.hold_planned) _plan_hold(); - - // Main segment dispatch - switch (ex.section) { - case SECTION_HEAD: status = _exec_aline_head(); break; - case SECTION_BODY: status = _exec_aline_body(); break; - case SECTION_TAIL: status = _exec_aline_tail(); break; - } - - // Set runtime velocity on exit - if (status != STAT_EAGAIN) mp_runtime_set_velocity(ex.exit_velocity); - - return status; -} - - -/// Dequeues buffers, initializes them, executes their callbacks and cleans up. -/// -/// This is the guts of the planner runtime execution. Because this routine is -/// run in an interrupt the state changes must be carefully ordered. -stat_t mp_exec_move() { - // Check if we can run a buffer - mp_buffer_t *bf = mp_queue_get_head(); - if (mp_get_state() == STATE_ESTOPPED || mp_get_state() == STATE_HOLDING || - !bf) { - mp_runtime_set_velocity(0); - mp_runtime_set_busy(false); - - return STAT_NOOP; // Nothing running - } - - // Process new buffers - if (bf->state == BUFFER_NEW) { - // On restart wait a bit to give planner queue a chance to fill - if (!mp_runtime_is_busy() && mp_queue_get_fill() < PLANNER_EXEC_MIN_FILL && - !rtc_expired(bf->ts + PLANNER_EXEC_DELAY)) return STAT_NOOP; - - // Take control of buffer - bf->state = BUFFER_INIT; - bf->replannable = false; - - // Update runtime - mp_runtime_set_line(bf->line); - } - - // Execute the buffer - stat_t status = bf->cb(bf); - - // Signal that we are busy only if a move was queued. This means that - // nonstop buffers, i.e. non-plan-to-zero commands, will not cause the - // runtime to enter the busy state. This causes mp_exec_move() to continue - // to wait above for the planner buffer to fill when a new stream starts - // with some nonstop buffers. If this weren't so, the code below - // which marks the next buffer not replannable would lock the first move - // buffer and cause it to be unnecessarily planned to zero. - if (status == STAT_EAGAIN || status == STAT_OK) mp_runtime_set_busy(true); - - // Process finished buffers - if (status != STAT_EAGAIN) { - // Signal that we've encountered a stopping point - if (fp_ZERO(mp_runtime_get_velocity()) && - (mp_get_state() == STATE_STOPPING || bf->hold)) mp_state_holding(); - - // Handle buffer restarts and deallocation - if (bf->state == BUFFER_RESTART) bf->state = BUFFER_NEW; - else { - // Solves a potential race condition where the current buffer ends but - // the new buffer has not started because the current one is still - // being run by the steppers. Planning can overwrite the new buffer. - // See notes above. - mp_buffer_next(bf)->replannable = false; - - mp_queue_pop(); // Release buffer - - // Enter READY state - if (mp_queue_is_empty()) mp_state_idle(); - } - } - - // Convert return status for stepper.c - switch (status) { - case STAT_NOOP: - // Tell caller to call again if there is more in the queue - return mp_queue_is_empty() ? STAT_NOOP : STAT_EAGAIN; - case STAT_EAGAIN: return STAT_OK; // A move was queued, call again later - default: return status; - } -} diff --git a/src/plan/exec.h b/src/plan/exec.h deleted file mode 100644 index 9020e96..0000000 --- a/src/plan/exec.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/plan/forward_dif.c b/src/plan/forward_dif.c deleted file mode 100644 index 955b15e..0000000 --- a/src/plan/forward_dif.c +++ /dev/null @@ -1,190 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "forward_dif.h" - -#include - - -/// Forward differencing math -/// -/// We are using a quintic (fifth-degree) Bezier polynomial for the velocity -/// curve. This gives us a "linear pop" velocity curve; with pop being the -/// sixth derivative of position: velocity - 1st, acceleration - 2nd, jerk - -/// 3rd, snap - 4th, crackle - 5th, pop - 6th -/// -/// The Bezier curve takes the form: -/// -/// V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + -/// P_4 * B_4(t) + P_5 * B_5(t) -/// -/// Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are -/// the control points, and B_0(t) through B_5(t) are the Bernstein -/// basis as follows: -/// -/// B_0(t) = (1 - t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1 -/// B_1(t) = 5(1 - t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t -/// B_2(t) = 10(1 - t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2 -/// B_3(t) = 10(1 - t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3 -/// B_4(t) = 5(1 - t) * t^4 = -5t^5 + 5t^4 -/// B_5(t) = t^5 = t^5 -/// -/// ^ ^ ^ ^ ^ ^ -/// A B C D E F -/// -/// We use forward-differencing to calculate each position through the curve. -/// This requires a formula of the form: -/// -/// V_f(t) = A * t^5 + B * t^4 + C * t^3 + D * t^2 + E * t + F -/// -/// Looking at the above B_0(t) through B_5(t) expanded forms, if we take the -/// coefficients of t^5 through t of the Bezier form of V(t), we can determine -/// that: -/// -/// A = -P_0 + 5 * P_1 - 10 * P_2 + 10 * P_3 - 5 * P_4 + P_5 -/// B = 5 * P_0 - 20 * P_1 + 30 * P_2 - 20 * P_3 + 5 * P_4 -/// C = -10 * P_0 + 30 * P_1 - 30 * P_2 + 10 * P_3 -/// D = 10 * P_0 - 20 * P_1 + 10 * P_2 -/// E = - 5 * P_0 + 5 * P_1 -/// F = P_0 -/// -/// Now, since we will (currently) *always* want the initial acceleration and -/// jerk values to be 0, We set P_i = P_0 = P_1 = P_2 (initial velocity), and -/// P_t = P_3 = P_4 = P_5 (target velocity), which, after simplification, -/// resolves to: -/// -/// A = - 6 * P_i + 6 * P_t -/// B = 15 * P_i - 15 * P_t -/// C = -10 * P_i + 10 * P_t -/// D = 0 -/// E = 0 -/// F = P_i -/// -/// Given an interval count of I to get from P_i to P_t, we get the parametric -/// "step" size of h = 1/I. We need to calculate the initial value of forward -/// differences (F_0 - F_5) such that the inital velocity V = P_i, then we -/// iterate over the following I times: -/// -/// V += F_5 -/// F_5 += F_4 -/// F_4 += F_3 -/// F_3 += F_2 -/// F_2 += F_1 -/// -/// See -/// http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417 -/// for an example of how to calculate F_0 - F_5 for a cubic bezier curve. Since -/// this is a quintic bezier curve, we need to extend the formulas somewhat. -/// I'll not go into the long-winded step-by-step here, but it gives the -/// resulting formulas: -/// -/// a = A, b = B, c = C, d = D, e = E, f = F -/// -/// F_5(t + h) - F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 + -/// (10ah^3 + 6bh^2 + 3ch)t^2 + (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 + -/// bh^4 + ch^3 + dh^2 + eh -/// -/// a = 5ah -/// b = 10ah^2 + 4bh -/// c = 10ah^3 + 6bh^2 + 3ch -/// d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh -/// -/// After substitution, simplification, and rearranging: -/// -/// F_4(t + h) - F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 + -/// (70ah^4 + 24bh^3 + 6ch^2)t + 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2 -/// -/// a = 20ah^2 -/// b = 60ah^3 + 12bh^2 -/// c = 70ah^4 + 24bh^3 + 6ch^2 -/// -/// After substitution, simplification, and rearranging: -/// -/// F_3(t + h) - F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 + -/// 36bh^4 + 6ch^3 -/// -/// You get the picture... -/// -/// F_2(t + h) - F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4 -/// F_1(t + h) - F_1(t) = 120ah^5 -/// -/// Normally, we could then assign t = 0, use the A-F values from above, and get -/// out initial F_* values. However, for the sake of "averaging" the velocity -/// of each segment, we actually want to have the initial V be be at t = h/2 and -/// iterate I-1 times. So, the resulting F_* values are (steps not shown): -/// -/// F_5 = 121Ah^5 / 16 + 5Bh^4 + 13Ch^3 / 4 + 2Dh^2 + Eh -/// F_4 = 165Ah^5 / 2 + 29Bh^4 + 9Ch^3 + 2Dh^2 -/// F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3 -/// F_2 = 300Ah^5 + 24Bh^4 -/// F_1 = 120Ah^5 -/// -/// Note that with our current control points, D and E are actually 0. -/// -/// This can be simplified even further by subsituting Ah, Bh & Ch back in and -/// reducing to: -/// -/// F_5 = (32.5 * s^2 - 75 * s + 45.375)(Vt - Vi) * h^5 -/// F_4 = (90.0 * s^2 - 435 * s + 495.0 )(Vt - Vi) * h^5 -/// F_3 = (60.0 * s^2 - 720 * s + 1530.0 )(Vt - Vi) * h^5 -/// F_2 = ( - 360 * s + 1800.0 )(Vt - Vi) * h^5 -/// F_1 = ( 720.0 )(Vt - Vi) * h^5 -/// -float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float s) { - const float h = 1 / s; - const float s2 = square(s); - const float Vdxh5 = (Vt - Vi) * h * h * h * h * h; - - fdif[4] = (32.5 * s2 - 75.0 * s + 45.375) * Vdxh5; - fdif[3] = (90.0 * s2 - 435.0 * s + 495.0 ) * Vdxh5; - fdif[2] = (60.0 * s2 - 720.0 * s + 1530.0 ) * Vdxh5; - fdif[1] = ( - 360.0 * s + 1800.0 ) * Vdxh5; - fdif[0] = ( 720.0 ) * Vdxh5; - - // Calculate the initial velocity by calculating: - // - // V(h / 2) = - // - // ( -6Vi + 6Vt) / (2^5 * s^8) + - // ( 15Vi - 15Vt) / (2^4 * s^8) + - // (-10Vi + 10Vt) / (2^3 * s^8) + Vi = - // - // (Vt - Vi) * 1/2 * h^8 + Vi - return (Vt - Vi) * 0.5 * square(square(square(h))) + Vi; -} - - -float mp_next_forward_dif(forward_dif_t fdif) { - float delta = fdif[4]; - - for (int i = 4; i; i--) - fdif[i] += fdif[i - 1]; - - return delta; -} diff --git a/src/plan/forward_dif.h b/src/plan/forward_dif.h deleted file mode 100644 index d1ceb75..0000000 --- a/src/plan/forward_dif.h +++ /dev/null @@ -1,34 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - - -typedef float forward_dif_t[5]; - -float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float s); -float mp_next_forward_dif(forward_dif_t fdif); diff --git a/src/plan/jog.c b/src/plan/jog.c deleted file mode 100644 index 18c5278..0000000 --- a/src/plan/jog.c +++ /dev/null @@ -1,186 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "forward_dif.h" -#include "runtime.h" -#include "machine.h" -#include "state.h" -#include "config.h" - -#include -#include -#include -#include -#include - - -typedef struct { - bool writing; - float Vi; - float Vt; - forward_dif_t fdifs[AXES]; - unsigned segments[AXES]; - - int sign[AXES]; - float velocity[AXES]; - float next_velocity[AXES]; - float target_velocity[AXES]; -} jog_runtime_t; - - -static jog_runtime_t jr; - - -static stat_t _exec_jog(mp_buffer_t *bf) { - // Load next velocity - bool changed = false; - bool done = true; - if (!jr.writing) - for (int axis = 0; axis < AXES; axis++) { - float Vn = jr.next_velocity[axis] * axis_get_velocity_max(axis); - float Vi = jr.velocity[axis]; - float Vt = jr.target_velocity[axis]; - - if (JOG_MIN_VELOCITY < fabs(Vn)) done = false; - - if (!fp_ZERO(Vi) && (Vn < 0) != (Vi < 0)) - Vn = 0; // Plan to zero on sign change - - if (fabs(Vn) < JOG_MIN_VELOCITY) Vn = 0; - - if (Vt != Vn) { - jr.target_velocity[axis] = Vn; - if (Vn) jr.sign[axis] = Vn < 0 ? -1 : 1; - changed = true; - } - } - - float velocity_sqr = 0; - - for (int axis = 0; axis < AXES; axis++) { - float Vi = fabs(jr.velocity[axis]); - float Vt = fabs(jr.target_velocity[axis]); - - if (changed) { - if (fp_EQ(Vi, Vt)) { - Vi = Vt; - jr.segments[axis] = 0; - - } else { - // Compute axis max jerk - float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER; - - // Compute length to velocity given max jerk - float length = mp_get_target_length(Vi, Vt, jerk * JOG_JERK_MULT); - - // Compute move time - float move_time = 2 * length / (Vi + Vt); - if (move_time < MIN_SEGMENT_TIME) { - Vi = Vt; - jr.segments[axis] = 0; - - } else { - jr.segments[axis] = ceil(move_time / NOM_SEGMENT_TIME); - - Vi = mp_init_forward_dif(jr.fdifs[axis], Vi, Vt, jr.segments[axis]); - jr.segments[axis]--; - } - } - - } else if (jr.segments[axis]) { - Vi += mp_next_forward_dif(jr.fdifs[axis]); - jr.segments[axis]--; - - } else Vi = Vt; - - if (JOG_MIN_VELOCITY < Vi || JOG_MIN_VELOCITY < Vt) done = false; - - velocity_sqr += square(Vi); - jr.velocity[axis] = Vi * jr.sign[axis]; - } - - // Check if we are done - if (done) { - // Update machine position - for (int axis = 0; axis < AXES; axis++) - mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); - - mp_set_cycle(CYCLE_MACHINING); // Default cycle - - return STAT_NOOP; // Done, no move executed - } - - // Compute target from velocity - float target[AXES]; - for (int axis = 0; axis < AXES; axis++) - target[axis] = mp_runtime_get_axis_position(axis) + - jr.velocity[axis] * NOM_SEGMENT_TIME; - - // Set velocity and target - mp_runtime_set_velocity(sqrt(velocity_sqr)); - stat_t status = mp_runtime_move_to_target(target, NOM_SEGMENT_TIME); - if (status != STAT_OK) return status; - - return STAT_EAGAIN; -} - - -bool mp_jog_busy() {return mp_get_cycle() == CYCLE_JOGGING;} - - -uint8_t command_jog(int argc, char *argv[]) { - if (!mp_jog_busy() && - (mp_get_state() != STATE_READY || mp_get_cycle() != CYCLE_MACHINING)) - return STAT_NOOP; - - float velocity[AXES]; - - for (int axis = 0; axis < AXES; axis++) - if (axis < argc - 1) velocity[axis] = atof(argv[axis + 1]); - else velocity[axis] = 0; - - // Reset - if (!mp_jog_busy()) memset(&jr, 0, sizeof(jr)); - - jr.writing = true; - for (int axis = 0; axis < AXES; axis++) - jr.next_velocity[axis] = velocity[axis]; - jr.writing = false; - - if (!mp_jog_busy()) { - mp_set_cycle(CYCLE_JOGGING); - mp_queue_push_nonstop(_exec_jog, -1); - } - - return STAT_OK; -} diff --git a/src/plan/jog.h b/src/plan/jog.h deleted file mode 100644 index 32554d8..0000000 --- a/src/plan/jog.h +++ /dev/null @@ -1,33 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - - -bool mp_jog_busy(); diff --git a/src/plan/line.c b/src/plan/line.c deleted file mode 100644 index 6e0c0e3..0000000 --- a/src/plan/line.c +++ /dev/null @@ -1,409 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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" - - -/* Sonny's algorithm - simple - * - * Computes the maximum allowable junction speed by finding the velocity that - * will yield the centripetal acceleration in the corner_acceleration value. The - * value of delta sets the effective radius of curvature. Here's Sonny's - * (Sungeun K. Jeon's) explanation of what's going on: - * - * "First let's assume that at a junction we only look a centripetal - * acceleration to simplify things. At a junction of two lines, let's place a - * circle such that both lines are tangent to the circle. The circular segment - * joining the lines represents the path for constant centripetal acceleration. - * This creates a deviation from the path (let's call this delta), - * which is the distance from the junction to the edge of the circular - * segment. Delta needs to be defined, so let's replace the term max_jerk (see - * note 1) with max_junction_deviation, or "delta". This indirectly sets the - * radius of the circle, and hence limits the velocity by the centripetal - * acceleration. Think of the this as widening the race track. If a race car is - * driving on a track only as wide as a car, it'll have to slow down a lot to - * turn corners. If we widen the track a bit, the car can start to use the - * track to go into the turn. The wider it is, the faster through the corner - * it can go. - * - * Note 1: "max_jerk" refers to the old grbl/marlin "max_jerk" approximation - * term, not the TinyG jerk terms. - * - * If you do the geometry in terms of the known variables, you get: - * - * sin(theta/2) = R / (R + delta) - * - * Re-arranging in terms of circle radius (R) - * - * R = delta * sin(theta/2) / (1 - sin(theta/2)) - * - * Theta is the angle between line segments given by: - * - * cos(theta) = dot(a, b) / (norm(a) * norm(b)) - * - * Most of these calculations are already done in the planner. - * To remove the acos() and sin() computations, use the trig half - * angle identity: - * - * sin(theta/2) = +/-sqrt((1 - cos(theta)) / 2) - * - * For our applications, this should always be positive. Now just plug the - * equations into the centripetal acceleration equation: - * - * v_c = sqrt(a_max * R) - * - * You'll see that there are only two sqrt computations and no sine/cosines. - * - * How to compute the radius using brute-force trig: - * - * float theta = acos(costheta); - * float radius = delta * sin(theta/2) / (1 - sin(theta/2)); - * - * This version extends Chamnit's algorithm by computing a value for delta that - * takes the contributions of the individual axes in the move into account. - * This allows the control radius to vary by axis. This is necessary to - * support axes that have different dynamics; such as a Z axis that doesn't - * move as fast as X and Y (such as a screw driven Z axis on machine with a belt - * driven XY - like a Shapeoko), or rotary axes ABC that have completely - * different dynamics than their linear counterparts. - * - * The function takes the absolute values of the sum of the unit vector - * components as a measure of contribution to the move, then scales the delta - * values from the non-zero axes into a composite delta to be used for the - * move. Shown for an XY vector: - * - * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i]) - * Usum Length of sums Ux + Uy - * d Delta of sums (Dx * Ux + DY * UY) / Usum - */ -static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { - float costheta = 0; - for (int axis = 0; axis < AXES; axis++) - costheta -= a_unit[axis] * b_unit[axis]; - - if (costheta < -0.99) return 10000000; // straight line cases - if (0.99 < costheta) return 0; // reversal cases - - // Fuse the junction deviations into a vector sum - float a_delta = 0; - float b_delta = 0; - - for (int axis = 0; axis < AXES; axis++) { - a_delta += square(a_unit[axis] * axis_get_junction_dev(axis)); - b_delta += square(b_unit[axis] * axis_get_junction_dev(axis)); - } - - if (!a_delta || !b_delta) return 0; // One or both unit vectors are null - - float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2; - float sintheta_over2 = sqrt((1 - costheta) / 2); - float radius = delta * sintheta_over2 / (1 - sintheta_over2); - float velocity = sqrt(radius * JUNCTION_ACCELERATION); - - return velocity; -} - - -/* Find the axis for which the jerk cannot be exceeded - the 'jerk_axis'. - * This is the axis for which the time to decelerate from the target velocity - * to zero would be the longest. - * - * We can determine the "longest" deceleration in terms of time or distance. - * - * The math for time-to-decelerate T from speed S to speed E with constant - * jerk J is: - * - * T = 2 * sqrt((S - E) / J) - * - * Since E is always zero in this calculation, we can simplify: - * - * T = 2 * sqrt(S / J) - * - * The math for distance-to-decelerate l from speed S to speed E with - * constant jerk J is: - * - * l = (S + E) * sqrt((S - E) / J) - * - * Since E is always zero in this calculation, we can simplify: - * - * l = S * sqrt(S / J) - * - * The final value we want to know is which one is *longest*, compared to the - * others, so we don't need the actual value. This means that the scale of - * the value doesn't matter, so for T we can remove the "2 *" and for L we can - * remove the "S *". This leaves both to be simply "sqrt(S / J)". Since we - * don't need the scale, it doesn't matter what speed we're coming from, so S - * can be replaced with 1. - * - * However, we *do* need to compensate for the fact that each axis contributes - * differently to the move, so we will scale each contribution C[n] by the - * proportion of the axis movement length D[n]. Using that, we construct the - * following, with these definitions: - * - * J[n] = max_jerk for axis n - * D[n] = distance traveled for this move for axis n - * C[n] = "axis contribution" of axis n - * - * For each axis n: - * - * C[n] = sqrt(1 / J[n]) * D[n] - * - * Keeping in mind that we only need a rank compared to the other axes, we can - * further optimize the calculations: - * - * Square the expression to remove the square root: - * - * C[n]^2 = 1 / J[n] * D[n]^2 - * - * We don't care that C is squared, so we'll use it that way. Also note that - * we already have 1 / J[n] calculated for each axis. - */ -int mp_find_jerk_axis(const float axis_square[]) { - float C; - float maxC = 0; - int jerk_axis = 0; - - for (int axis = 0; axis < AXES; axis++) - if (axis_square[axis]) { // Do not use fp_ZERO here - // Squaring axis_length ensures it's positive - C = axis_square[axis] * axis_get_recip_jerk(axis); - - if (maxC < C) { - maxC = C; - jerk_axis = axis; - } - } - - return jerk_axis; -} - - -/// Determine jerk value to use for the block. -static float _calc_jerk(const float axis_square[], const float unit[]) { - int axis = mp_find_jerk_axis(axis_square); - - // Finally, the selected jerk term needs to be scaled by the - // reciprocal of the absolute value of the axis's unit - // vector term. This way when the move is finally decomposed into - // its constituent axes for execution the jerk for that axis will be - // at it's maximum value. - return axis_get_jerk_max(axis) * JERK_MULTIPLIER / fabs(unit[axis]); -} - - -/// Compute cached jerk terms used by planning -static void _calc_and_cache_jerk_values(mp_buffer_t *bf) { - static float jerk = 0; - static float cbrt_jerk = 0; - - if (JERK_MATCH_PRECISION < fabs(bf->jerk - jerk)) { // Tolerance comparison - jerk = bf->jerk; - cbrt_jerk = cbrt(bf->jerk); - } - - bf->cbrt_jerk = cbrt_jerk; -} - - -static void _calc_max_velocities(mp_buffer_t *bf, float move_time, - bool exact_stop) { - float junction_velocity = - _get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit); - - bf->cruise_vmax = bf->length / move_time; // target velocity requested - bf->entry_vmax = min(bf->cruise_vmax, junction_velocity); - bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); - bf->exit_vmax = min(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax)); - bf->braking_velocity = bf->delta_vmax; - - // Zero out exact stop cases - if (exact_stop) bf->entry_vmax = bf->exit_vmax = 0; - else bf->replannable = true; -} - - -/* Compute optimal and minimum move times - * - * "Minimum time" is the fastest the move can be performed given the velocity - * constraints on each participating axis - regardless of the feed rate - * requested. The minimum time is the time limited by the rate-limiting - * axis. The minimum time is needed to compute the optimal time and is recorded - * for possible feed override computation. - * - * "Optimal time" is either the time resulting from the requested feed rate or - * the minimum time if the requested feed rate is not achievable. Optimal times - * for rapids are always the minimum time. - * - * The following times are compared and the longest is returned: - * - G93 inverse time (if G93 is active) - * - time for coordinated move at requested feed rate - * - time that the slowest axis would require for the move - * - * NIST RS274NGC_v3 Guidance - * - * The following is verbatim text from NIST RS274NGC_v3. As I interpret A for - * moves that combine both linear and rotational movement, the feed rate should - * apply to the XYZ movement, with the rotational axis (or axes) timed to start - * and end at the same time the linear move is performed. It is possible under - * this case for the rotational move to rate-limit the linear move. - * - * 2.1.2.5 Feed Rate - * - * The rate at which the controlled point or the axes move is nominally a steady - * rate which may be set by the user. In the Interpreter, the interpretation of - * the feed rate is as follows unless inverse time feed rate mode is being used - * in the RS274/NGC view (see Section 3.5.19). The machining functions view of - * feed rate, as described in Section 4.3.5.1, has conditions under which the - * set feed rate is applied differently, but none of these is used in the - * Interpreter. - * - * A. For motion involving one or more of the X, Y, and Z axes (with or without - * simultaneous rotational axis motion), the feed rate means length units - * per minute along the programmed XYZ path, as if the rotational axes were - * not moving. - * - * B. For motion of one rotational axis with X, Y, and Z axes not moving, the - * feed rate means degrees per minute rotation of the rotational axis. - * - * C. For motion of two or three rotational axes with X, Y, and Z axes not - * moving, the rate is applied as follows. Let dA, dB, and dC be the angles - * in degrees through which the A, B, and C axes, respectively, must move. - * Let D = sqrt(dA^2 + dB^2 + dC^2). Conceptually, D is a measure of total - * angular motion, using the usual Euclidean metric. Let T be the amount of - * time required to move through D degrees at the current feed rate in - * degrees per minute. The rotational axes should be moved in coordinated - * linear motion so that the elapsed time from the start to the end of the - * motion is T plus any time required for acceleration or deceleration. - */ -static float _calc_move_time(const float axis_length[], - const float axis_square[], bool rapid, - bool inverse_time, float feed_rate, - float feed_override) { - float max_time = 0; - - // Compute times for feed motion - if (!rapid) { - if (inverse_time) max_time = feed_rate; - else { - // Compute length of linear move in millimeters. Feed rate in mm/min. - max_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + - axis_square[AXIS_Z]) / feed_rate; - - // If no linear axes, compute length of multi-axis rotary move in degrees. - // Feed rate is provided as degrees/min - if (fp_ZERO(max_time)) - max_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + - axis_square[AXIS_C]) / feed_rate; - } - } - - // Apply feed override - max_time /= feed_override; - - // Compute time required for rate-limiting axis - for (int axis = 0; axis < AXES; axis++) { - float time = fabs(axis_length[axis]) / - (rapid ? axis_get_velocity_max(axis) : axis_get_feedrate_max(axis)); - - if (max_time < time) max_time = time; - } - - return max_time < MIN_SEGMENT_TIME ? MIN_SEGMENT_TIME : max_time; -} - - -/*** Plan line acceleration / deceleration - * - * This function uses constant jerk motion equations to plan acceleration and - * deceleration. Jerk is the rate of change of acceleration; it's the 1st - * derivative of acceleration, and the 3rd derivative of position. Jerk is a - * measure of impact to the machine. Controlling jerk smooths transitions - * between moves and allows for faster feeds while controlling machine - * oscillations and other undesirable side-effects. - * - * Notes: - * [1] All math is done in absolute coordinates using single precision floats. - * - * [2] Returning a status that is not STAT_OK means the endpoint is NOT - * advanced. So lines that are too short to move will accumulate and get - * executed once the accumulated error exceeds the minimums. - * - * @param reed_rate is in minutes when @param inverse_time is true. - * See mach_set_feed_rate() - */ -stat_t mp_aline(const float target[], bool rapid, bool inverse_time, - bool exact_stop, float feed_rate, float feed_override, - int32_t line) { - // Compute axis and move lengths - float axis_length[AXES]; - float axis_square[AXES]; - float length_square = 0; - - for (int axis = 0; axis < AXES; axis++) { - axis_length[axis] = target[axis] - mp_get_axis_position(axis); - axis_square[axis] = square(axis_length[axis]); - length_square += axis_square[axis]; - } - - float length = sqrt(length_square); - if (fp_ZERO(length)) return STAT_OK; - - // Get a buffer. Note, new buffers are initialized to zero. - mp_buffer_t *bf = mp_queue_get_tail(); // current move pointer - - // Set buffer values - bf->length = length; - copy_vector(bf->target, target); - - // Compute unit vector - for (int axis = 0; axis < AXES; axis++) - bf->unit[axis] = axis_length[axis] / length; - - // Compute and cache jerk values - bf->jerk = _calc_jerk(axis_square, bf->unit); - _calc_and_cache_jerk_values(bf); - - // Compute move time and velocities - float time = _calc_move_time(axis_length, axis_square, rapid, inverse_time, - feed_rate, feed_override); - _calc_max_velocities(bf, time, exact_stop); - - // Note, the following lines must remain in order. - bf->line = line; // Planner needs then when planning steps - mp_plan(bf); // Plan block list - mp_set_position(target); // Set planner position before committing buffer - mp_queue_push(mp_exec_aline, line); // After position update - - return STAT_OK; -} diff --git a/src/plan/line.h b/src/plan/line.h deleted file mode 100644 index 9b109e7..0000000 --- a/src/plan/line.h +++ /dev/null @@ -1,38 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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_aline(const float target[], bool rapid, bool inverse_time, - bool exact_stop, float feed_rate, float feed_override, - int32_t line); -int mp_find_jerk_axis(const float axis_square[]); diff --git a/src/plan/planner.c b/src/plan/planner.c deleted file mode 100644 index 56966a6..0000000 --- a/src/plan/planner.c +++ /dev/null @@ -1,752 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 \ - (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->entry_velocity)) -#define MIN_TAIL_LENGTH \ - (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->exit_velocity)) -#define MIN_BODY_LENGTH (MIN_SEGMENT_TIME_PLUS_MARGIN * bf->cruise_velocity) - - -/*** Calculate move acceleration / deceleration - * - * This rather brute-force and long-ish function sets section lengths and - * velocities based on the line length and velocities requested. It modifies - * the incoming bf buffer and returns accurate head, body and tail lengths, and - * accurate or reasonably approximate velocities. We care about accuracy on - * lengths, less so for velocity, as long as velocity errs on the side of too - * slow. - * - * Note: We need the velocities to be set even for zero-length sections (Note: - * sections, not moves) so we can compute entry and exits for adjacent sections. - * - * Inputs used are: - * - * bf->length - actual block length, length is never changed - * bf->entry_velocity - requested Ve, entry velocity is never changed - * bf->cruise_velocity - requested Vt, is often changed - * bf->exit_velocity - requested Vx, may change for degenerate cases - * bf->cruise_vmax - used in some comparisons - * bf->delta_vmax - used to degrade velocity of short blocks - * - * Variables that may be set/updated are: - * - * bf->entry_velocity - requested Ve - * bf->cruise_velocity - requested Vt - * bf->exit_velocity - requested Vx - * bf->head_length - bf->length allocated to head - * bf->body_length - bf->length allocated to body - * bf->tail_length - bf->length allocated to tail - * - * Note: The following conditions must be met on entry: - * - * bf->length must be non-zero (filter these out upstream) - * bf->entry_velocity <= bf->cruise_velocity >= bf->exit_velocity - * - * Classes of moves: - * - * Requested-Fit - The move has sufficient length to achieve the target - * velocity (cruise velocity). I.e it will accommodate the acceleration / - * deceleration profile in the given length. - * - * Rate-Limited-Fit - The move does not have sufficient length to achieve - * target velocity. In this case the cruise velocity will be set lower than - * the requested velocity (incoming bf->cruise_velocity). The entry and - * exit velocities are satisfied. - * - * Degraded-Fit - The move does not have sufficient length to transition from - * the entry velocity to the exit velocity in the available length. These - * velocities are not negotiable, so a degraded solution is found. - * - * In worst cases, the move cannot be executed as the required execution - * time is less than the minimum segment time. The first degradation is to - * reduce the move to a body-only segment with an average velocity. If that - * still doesn't fit then the move velocity is reduced so it fits into a - * minimum segment. This will reduce the velocities in that region of the - * planner buffer as the moves are replanned to that worst-case move. - * - * Various cases handled (H=head, B=body, T=tail) - * - * Requested-Fit cases: - * - * HBT 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) { - // RULE #1 of mp_calculate_trapezoid(): Don't change bf->length - - if (!bf->length) return; - - // F case: Block is too short - run time < minimum segment time - // Force block into a single segment body with limited velocities - // Accept the entry velocity, limit the cruise, and go for the best exit - // velocity you can get given the delta_vmax (maximum velocity slew). - - float naive_move_time = - 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average - - if (naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) { - bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN; - bf->exit_velocity = - max(0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax))); - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; - - // Violating jerk but since it's a single segment move we don't use it. - return; - } - - // B" case: Block is short, but fits into a single body segment - if (naive_move_time <= NOM_SEGMENT_TIME) { - bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity; - - if (fp_NOT_ZERO(bf->entry_velocity)) { - bf->cruise_velocity = bf->entry_velocity; - bf->exit_velocity = bf->entry_velocity; - - } else { - bf->cruise_velocity = bf->delta_vmax / 2; - bf->exit_velocity = bf->delta_vmax; - } - - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; - - // Violating jerk but since it's a single segment move we don't use it. - return; - } - - // B case: Velocities all match (or close enough) - // This occurs frequently in normal gcode files with lots of short lines. - // This case is not really necessary, but saves lots of processing time. - if (((bf->cruise_velocity - bf->entry_velocity) < - TRAPEZOID_VELOCITY_TOLERANCE) && - ((bf->cruise_velocity - bf->exit_velocity) < - TRAPEZOID_VELOCITY_TOLERANCE)) { - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; - - return; - } - - // Head-only and tail-only short-line cases - // H" and T" degraded-fit cases - // H' and T' requested-fit cases where the body residual is less than - // MIN_BODY_LENGTH - bf->body_length = 0; - float minimum_length = - mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf->jerk); - - // head-only & tail-only cases - if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) { - // tail-only cases (short decelerations) - if (bf->entry_velocity > bf->exit_velocity) { - // T" (degraded case) - if (bf->length < minimum_length) - bf->entry_velocity = - mp_get_target_velocity(bf->exit_velocity, bf->length, bf); - - bf->cruise_velocity = bf->entry_velocity; - bf->tail_length = bf->length; - bf->head_length = 0; - - return; - } - - // head-only cases (short accelerations) - if (bf->entry_velocity < bf->exit_velocity) { - // H" (degraded case) - if (bf->length < minimum_length) - bf->exit_velocity = - mp_get_target_velocity(bf->entry_velocity, bf->length, bf); - - bf->cruise_velocity = bf->exit_velocity; - bf->head_length = bf->length; - bf->tail_length = 0; - - return; - } - } - - // Set head and tail lengths for evaluating the next cases - bf->head_length = - mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk); - bf->tail_length = - mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk); - if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;} - if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;} - - // Rate-limited HT and HT' cases - if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited - - // Symmetric rate-limited case (HT) - if (fabs(bf->entry_velocity - bf->exit_velocity) < - TRAPEZOID_VELOCITY_TOLERANCE) { - bf->head_length = bf->length/2; - bf->tail_length = bf->head_length; - bf->cruise_velocity = - min(bf->cruise_vmax, - mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf)); - - if (bf->head_length < MIN_HEAD_LENGTH) { - // Convert this to a body-only move - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; - - // Average the entry speed and computed best cruise-speed - bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity)/2; - bf->entry_velocity = bf->cruise_velocity; - bf->exit_velocity = bf->cruise_velocity; - } - - return; - } - - // Asymmetric HT' rate-limited case. This is relatively expensive but it's - // not called very often - float computed_velocity = bf->cruise_vmax; - do { - // initialize from previous iteration - bf->cruise_velocity = computed_velocity; - bf->head_length = - mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk); - bf->tail_length = - mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk); - - if (bf->head_length > bf->tail_length) { - bf->head_length = - (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length; - computed_velocity = - mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf); - - } else { - bf->tail_length = - (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length; - computed_velocity = - mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf); - } - - } while ((fabs(bf->cruise_velocity - computed_velocity) / - computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT); - - // set velocity and clean up any parts that are too short - bf->cruise_velocity = computed_velocity; - bf->head_length = - mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk); - bf->tail_length = bf->length - bf->head_length; - - if (bf->head_length < MIN_HEAD_LENGTH) { - // adjust the move to be all tail... - bf->tail_length = bf->length; - bf->head_length = 0; - } - - if (bf->tail_length < MIN_TAIL_LENGTH) { - bf->head_length = bf->length; //...or all head - bf->tail_length = 0; - } - - return; - } - - // Requested-fit cases: remaining of: HBT, HB, BT, BT, H, T, B, cases - bf->body_length = bf->length - bf->head_length - bf->tail_length; - - // If a non-zero body is < minimum length distribute it to the head and/or - // tail. This will generate small (acceptable) velocity errors in runtime - // execution but preserve correct distance, which is more important. - if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) { - if (fp_NOT_ZERO(bf->head_length)) { - if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT - bf->head_length += bf->body_length / 2; - bf->tail_length += bf->body_length / 2; - - } else bf->head_length += bf->body_length; // HB reduces to H - } else bf->tail_length += bf->body_length; // BT reduces to T - - bf->body_length = 0; - - // If the body is a standalone make the cruise velocity match the entry - // velocity. This removes a potential velocity discontinuity at the expense - // of top speed - } else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length))) - bf->cruise_velocity = bf->entry_velocity; -} - - -#if 0 -/// Prints the entire planning queue as comma separated values embedded in -/// JSON ``msg`` entries. Used for debugging. -void mp_print_queue(mp_buffer_t *bf) { - printf_P(PSTR("{\"msg\":\"id,replannable,callback," - "length,head_length,body_length,tail_length," - "entry_velocity,cruise_velocity,exit_velocity,braking_velocity," - "entry_vmax,cruise_vmax,exit_vmax\"}\n")); - - int i = 0; - mp_buffer_t *bp = bf; - while (bp) { - printf_P(PSTR("{\"msg\":\"%d,%d,0x%04x," - "%0.2f,%0.2f,%0.2f,%0.2f," - "%0.2f,%0.2f,%0.2f,%0.2f," - "%0.2f,%0.2f,%0.2f\"}\n"), - i++, bp->replannable, bp->cb, - bp->length, bp->head_length, bp->body_length, bp->tail_length, - bp->entry_velocity, bp->cruise_velocity, bp->exit_velocity, - bp->braking_velocity, - bp->entry_vmax, bp->cruise_vmax, bp->exit_vmax); - - bp = mp_buffer_prev(bp); - if (bp == bf || bp->state == BUFFER_OFF) break; - } - - while (!usart_tx_empty()) continue; -} -#endif - - -/*** Plans the entire queue - * - * The block list is the circular buffer of planner buffers (bl's). The block - * currently being planned is the "bl" block. The "first block" is the next - * block to execute; queued immediately behind the currently executing block, - * aka the "running" block. In some cases, there is no first block because the - * list is empty or there is only one block and it is already running. - * - * If blocks following the first block are already optimally planned (non - * replannable) the first block that is not optimally planned becomes the - * effective first block. - * - * mp_plan() plans all blocks between and including the (effective) - * first block and the bl. It sets entry, exit and cruise v's from vmax's then - * calls trapezoid generation. - * - * Variables that must be provided in the mp_buffer_t that will be processed: - * - * bl (function arg) - end of block list (last block in time) - * bl->replannable - start of block list set by last FALSE value - * [Note 1] - * bl->move_type - typically MOVE_TYPE_ALINE. Other move_types should - * be set to length=0, entry_vmax=0 and exit_vmax=0 - * and are treated as a momentary stop (plan to zero - * and from zero). - * bl->length - provides block length - * bl->entry_vmax - used during forward planning to set entry velocity - * bl->cruise_vmax - used during forward planning to set cruise velocity - * bl->exit_vmax - used during forward planning to set exit velocity - * bl->delta_vmax - used during forward planning to set exit velocity - * bl->cbrt_jerk - used during trapezoid generation - * - * Variables that will be set during processing: - * - * bl->replannable - set if the block becomes optimally planned - * bl->braking_velocity - set during backward planning - * bl->entry_velocity - set during forward planning - * bl->cruise_velocity - set during forward planning - * bl->exit_velocity - set during forward planning - * bl->head_length - set during trapezoid generation - * bl->body_length - set during trapezoid generation - * bl->tail_length - set during trapezoid generation - * - * Variables that are ignored but here's what you would expect them to be: - * - * bl->state - BUFFER_NEW for all blocks but the earliest - * bl->target[] - block target position - * bl->unit[] - block unit vector - * bl->jerk - source of the other jerk variables. - * - * Notes: - * - * [1] Whether or not a block is planned is controlled by the bl->replannable - * setting. Replan flags are checked during the backwards pass. They prune - * the replan list to include only the latest blocks that require planning. - * - * In normal operation, the first block (currently running block) is not - * replanned, but may be for feedholds and feed overrides. In these cases, - * the prep routines modify the contents of the (ex) buffer and re-shuffle - * the block list, re-enlisting the current bl buffer with new parameters. - * These routines also set all blocks in the list to be replannable so the - * list can be recomputed regardless of exact stops and previous replanning - * optimizations. - */ -void mp_plan(mp_buffer_t *bl) { - mp_buffer_t *bp = bl; - - // Backward planning pass. Find first block and update braking velocities. - // By the end bp points to the buffer before the first block. - mp_buffer_t *next = bp; - while ((bp = mp_buffer_prev(bp)) != bl) { - if (!bp->replannable) break; - - bp->braking_velocity = - min(next->entry_vmax, next->braking_velocity) + bp->delta_vmax; - - next = bp; - } - - // Forward planning pass. Recompute trapezoids from the first block to bl. - mp_buffer_t *prev = bp; - while ((bp = mp_buffer_next(bp)) != bl) { - mp_buffer_t *next = mp_buffer_next(bp); - - bp->entry_velocity = prev == bl ? bp->entry_vmax : prev->exit_velocity; - bp->cruise_velocity = bp->cruise_vmax; - bp->exit_velocity = min4(bp->exit_vmax, next->entry_vmax, - next->braking_velocity, - bp->entry_velocity + bp->delta_vmax); - - if (mp.plan_steps && bp->line != next->line) { - bp->exit_velocity = 0; - bp->hold = true; - - } else bp->hold = false; - - mp_calculate_trapezoid(bp); - - // Test for optimally planned trapezoids by checking exit conditions - if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) || - fp_EQ(bp->exit_velocity, next->entry_vmax)) || - (!prev->replannable && - fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax)))) - bp->replannable = false; - - prev = bp; - } - - // Finish last block - bl->entry_velocity = prev->exit_velocity; - bl->cruise_velocity = bl->cruise_vmax; - bl->exit_velocity = 0; - - mp_calculate_trapezoid(bl); -} - - -void mp_replan_all() { - ASSERT(mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING); - - // Get next buffer - mp_buffer_t *bf = mp_queue_get_head(); - if (!bf) return; - - mp_buffer_t *bp = bf; - - // Mark all blocks replanable - while (true) { - bp->replannable = true; - mp_buffer_t *next = mp_buffer_next(bp); - if (next->state == BUFFER_OFF || next == bf) break; // Avoid wrap around - bp = next; - } - - // Plan blocks - mp_plan(bp); -} - - -/// Push a non-stop command to the queue. I.e. one that does not cause the -/// planner to plan to zero. -void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line) { - mp_buffer_t *bp = mp_queue_get_tail(); - - bp->entry_vmax = bp->cruise_vmax = bp->exit_vmax = INFINITY; - copy_vector(bp->unit, bp->prev->unit); - bp->replannable = true; - - mp_queue_push(cb, line); -} - - -/*** Derive accel/decel length from delta V and jerk - * - * A convenient function for determining the optimal_length (L) of a line - * given the initial velocity (Vi), final velocity (Vf) and maximum jerk (Jm). - * - * Definitions: - * - * Jm = the given maximum jerk - * T = time of the entire move - * Vi = initial velocity - * Vf = final velocity - * T = time - * As = accel at inflection point between convex & concave portions of S-curve - * Ar = ramp acceleration - * - * Formulas: - * - * T = 2 * sqrt((Vt - Vi) / Jm) - * As = (Jm * T) / 2 - * Ar = As / 2 = (Jm * T) / 4 - * - * Then the length (distance) equation is: - * - * a) L = (Vf - Vi) * T - (Ar * T^2) / 2 - * - * Substituting for Ar and T: - * - * b) L = (Vf - Vi) * 2 * sqrt((Vf - Vi) / Jm) - - * (2 * sqrt((Vf - Vi) / Jm) * (Vf - Vi)) / 2 - * - * Reducing b). See Wolfram Alpha: - * - * c) L = (Vf - Vi)^(3/2) / sqrt(Jm) - * - * Assuming Vf >= Vi [Note 2]: - * - * d) L = (Vf - Vi) * sqrt((Vf - Vi) / Jm) - * - * Notes: - * - * [1] Assuming Vi, Vf and L are positive or zero. - * [2] Cannot assume Vf >= Vi due to rounding errors and use of - * PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs() - */ -float mp_get_target_length(float Vi, float Vf, float jerk) { - return fp_EQ(Vi, Vf) ? 0 : fabs(Vi - Vf) * invsqrt(jerk / fabs(Vi - Vf)); -} - - -#define GET_VELOCITY_ITERATIONS 0 // must be zero or more - -/*** Derive velocity achievable from delta V and length - * - * A convenient function for determining Vf target velocity for a given - * initial velocity (Vi), length (L), and maximum jerk (Jm). Equation e) is - * b) solved for Vf. Equation f) is c) solved for Vf. Use f) (obviously) - * - * e) Vf = (sqrt(L) * (L / sqrt(1 / Jm))^(1/6) + - * (1 / Jm)^(1/4) * Vi) / (1 / Jm)^(1/4) - * - * f) Vf = L^(2/3) * Jm^(1/3) + Vi - * - * FYI: Here's an expression that returns the jerk for a given deltaV and L: - * - * cube(deltaV / (pow(L, 0.66666666))); - * - * We do some Newton-Raphson iterations to narrow it down. - * We need a formula that includes known variables except the one we want to - * find, and has a root [Z(x) = 0] at the value (x) we are looking for. - * - * Z(x) = zero at x - * - * We calculate the value from the knowns and the estimate (see below) and then - * subtract the known value to get zero (root) if x is the correct value. - * - * Vi = initial velocity (known) - * Vf = estimated final velocity - * J = jerk (known) - * L = length (know) - * - * There are (at least) two such functions we can use: - * - * L from J, Vi, and Vf: - * - * L = sqrt((Vf - Vi) / J) * (Vi + Vf) - * - * Replacing Vf with x, and subtracting the known L we get: - * - * 0 = sqrt((x - Vi) / J) * (Vi + x) - L - * Z(x) = sqrt((x - Vi) / J) * (Vi + x) - L - * - * Or J from L, Vi, and Vf: - * - * J = ((Vf - Vi) * (Vi + Vf)^2) / L^2 - * - * Replacing Vf with x, and subtracting the known J we get: - * - * 0 = ((x - Vi) * (Vi + x)^2) / L^2 - J - * Z(x) = ((x - Vi) * (Vi + x)^2) / L^2 - J - * - * L doesn't resolve to the value very quickly (its graph is nearly vertical). - * So, we'll use J, which resolves in < 10 iterations, often in only two or - * three with a good estimate. - * - * In order to do a Newton-Raphson iteration, we need the derivative. Here - * they are for both the (unused) L and the (used) J formulas above: - * - * J > 0, Vi > 0, Vf > 0 - * A = sqrt((x - Vi) * J) - * B = sqrt((x - Vi) / J) - * - * L'(x) = B + (Vi + x) / (2 * J) + (Vi + x) / (2 * A) - * - * J'(x) = (2 * Vi * x - Vi^2 + 3 * x^2) / L^2 - */ -float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf) { - // 0 iterations (a reasonable estimate) - float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate - -#if (GET_VELOCITY_ITERATIONS > 0) - const float L2 = L * L; - const float Vi2 = Vi * Vi; - - for (int i = 0; i < GET_VELOCITY_ITERATIONS; i++) - // x' = x - Z(x) / J'(x) - x = x - ((x - Vi) * square(Vi + x) / L2 - bf->jerk) / - ((2 * Vi * x - Vi2 + 3 * x * x) / L2); -#endif - - return x; -} diff --git a/src/plan/planner.h b/src/plan/planner.h deleted file mode 100644 index 04c5a6d..0000000 --- a/src/plan/planner.h +++ /dev/null @@ -1,89 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2013 - 2015 Alden S. Hart, Jr. - Copyright (c) 2013 - 2015 Robert Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - -#define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) -#define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) - -#define MIN_SEGMENT_TIME_PLUS_MARGIN \ - ((MIN_SEGMENT_USEC + 1) / MICROSECONDS_PER_MINUTE) - -/// Error percentage for iteration convergence. As percent - 0.01 = 1% -#define TRAPEZOID_ITERATION_ERROR_PERCENT 0.1 - -/// Adaptive velocity tolerance term -#define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100)) - -/*** If the absolute value of the remaining deceleration length would be less - * than this value then finish the deceleration in the current move. This is - * used to avoid creating segements before or after the hold which are too - * short to process correctly. - */ -#define HOLD_DECELERATION_TOLERANCE 1 // In mm - - -typedef enum { - SECTION_HEAD, // acceleration - SECTION_BODY, // cruise - SECTION_TAIL, // deceleration -} move_section_t; - - -void mp_init(); - -void mp_set_axis_position(int axis, float position); -float mp_get_axis_position(int axis); -void mp_set_position(const float p[]); -void mp_set_plan_steps(bool plan_steps); - -void mp_flush_planner(); -void mp_kinematics(const float travel[], float steps[]); - -void mp_plan(mp_buffer_t *bf); -void mp_replan_all(); - -void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line); - -float mp_get_target_length(float Vi, float Vf, float jerk); -float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf); diff --git a/src/plan/runtime.c b/src/plan/runtime.c deleted file mode 100644 index 01de326..0000000 --- a/src/plan/runtime.c +++ /dev/null @@ -1,213 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - - float feed; - feed_mode_t feed_mode; - float feed_override; - float spindle_override; - - plane_t plane; - units_t units; - coord_system_t coord_system; - bool absolute_mode; - path_mode_t path_mode; - distance_mode_t distance_mode; - distance_mode_t arc_distance_mode; - - float previous_error[MOTORS]; -} mp_runtime_t; - -static mp_runtime_t rt = {0}; - - -bool mp_runtime_is_busy() {return rt.busy;} -void mp_runtime_set_busy(bool busy) {rt.busy = busy;} -int32_t mp_runtime_get_line() {return rt.line;} -void mp_runtime_set_line(int32_t line) {rt.line = line; report_request();} -uint8_t mp_runtime_get_tool() {return rt.tool;} -void mp_runtime_set_tool(uint8_t tool) {rt.tool = tool; report_request();} - - -/// Returns current segment velocity -float mp_runtime_get_velocity() {return rt.velocity;} - - -void mp_runtime_set_velocity(float velocity) { - rt.velocity = velocity; - report_request(); -} - - -/// Set encoder counts to the runtime position -void mp_runtime_set_steps_from_position() { - // Convert lengths to steps in floating point - float steps[MOTORS]; - mp_kinematics(rt.position, steps); - - for (int motor = 0; motor < MOTORS; motor++) - // Write steps to encoder register - motor_set_encoder(motor, steps[motor]); -} - - -/* Since steps are in motor space you have to run the position vector - * through inverse kinematics to get the right numbers. This means - * that in a non-Cartesian robot changing any position can result in - * changes to multiple step values. So this operation is provided as a - * single function and always uses the new position vector as an - * input. - * - * Keeping track of position is complicated by the fact that moves - * exist in several reference frames. The scheme to keep this - * straight is: - * - * - mp_position - start and end position for planning - * - rt.position - current position of runtime segment - * - rt.steps.* - position in steps - * - * Note that position is set immediately when called and may not be - * an accurate representation of the tool position. The motors - * are still processing the action and the real tool position is - * still close to the starting point. - */ - - -/// Set runtime position for a single axis -void mp_runtime_set_axis_position(uint8_t axis, const float position) { - rt.position[axis] = position; - report_request(); -} - - -/// Returns current axis position in machine coordinates -float mp_runtime_get_axis_position(uint8_t axis) {return rt.position[axis];} -float *mp_runtime_get_position() {return rt.position;} - - -void mp_runtime_set_position(float position[]) { - copy_vector(rt.position, position); - report_request(); -} - - -/// Returns axis position in work coordinates that were in effect at plan time -float mp_runtime_get_work_position(uint8_t axis) { - return rt.position[axis] - rt.work_offset[axis]; -} - - -/// Set offsets -void mp_runtime_set_work_offsets(float offset[]) { - copy_vector(rt.work_offset, offset); -} - - -/// Segment runner -stat_t mp_runtime_move_to_target(float target[], float time) { - // Convert target position to steps. - float steps[MOTORS]; - mp_kinematics(target, steps); - -#ifdef STEP_CORRECTION - int32_t error[MOTORS]; - st_get_error(error); - - float travel[MOTORS]; - float new_length_sqr = 0; - float old_length_sqr = 0; - - for (int motor = 0; motor < MOTORS; motor++) { - travel[motor] = steps[motor] - motor_get_position(motor); - - if (fp_ZERO(travel[motor])) { - motor[travel] = 0; - motor[error] = 0; - } - - error[motor] = 0.5 * (error[motor] - rt.previous_error[motor]); - rt.previous_error[motor] = error[motor]; - - if (error[motor] < -MAX_STEP_CORRECTION) - error[motor] = -MAX_STEP_CORRECTION; - else if (MAX_STEP_CORRECTION < error[motor]) - error[motor] = MAX_STEP_CORRECTION; - - old_length_sqr += square(travel[motor]); - new_length_sqr += square(travel[motor] - error[motor]); - } - - bool use_error = false; - if (!fp_ZERO(new_length_sqr)) { - float new_time = time * invsqrt(old_length_sqr / new_length_sqr); - - if (!isnan(new_time) && !isinf(new_time) && - EPSILON <= new_time && new_time <= MAX_SEGMENT_TIME) { - time = new_time; - use_error = true; - } - } - - if (!use_error) memset(error, 0, sizeof(error)); - -#else - const int32_t error[MOTORS] = {0}; -#endif - - // Call the stepper prep function - RITORNO(st_prep_line(time, steps, error)); - - // Update positions - mp_runtime_set_position(target); - - return STAT_OK; -} diff --git a/src/plan/runtime.h b/src/plan/runtime.h deleted file mode 100644 index ba51b1e..0000000 --- a/src/plan/runtime.h +++ /dev/null @@ -1,58 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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, const float position); -float mp_runtime_get_axis_position(uint8_t axis); - -float *mp_runtime_get_position(); -void mp_runtime_set_position(float position[]); - -float mp_runtime_get_work_position(uint8_t axis); -void mp_runtime_set_work_offsets(float offset[]); - -stat_t mp_runtime_move_to_target(float target[], float time); diff --git a/src/plan/state.c b/src/plan/state.c deleted file mode 100644 index b53ef6f..0000000 --- a/src/plan/state.c +++ /dev/null @@ -1,262 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2013 - 2015 Alden S. Hart, Jr. - Copyright (c) 2013 - 2015 Robert Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "report.h" - -#include - - -typedef struct { - mp_state_t state; - mp_cycle_t cycle; - mp_hold_reason_t hold_reason; - - bool hold_requested; - bool flush_requested; - bool start_requested; - bool resume_requested; - bool optional_pause_requested; -} planner_state_t; - - -static planner_state_t ps = { - .flush_requested = true, // Start out flushing -}; - - -PGM_P mp_get_state_pgmstr(mp_state_t state) { - switch (state) { - case STATE_READY: return PSTR("READY"); - case STATE_ESTOPPED: return PSTR("ESTOPPED"); - case STATE_RUNNING: return PSTR("RUNNING"); - case STATE_STOPPING: return PSTR("STOPPING"); - case STATE_HOLDING: return PSTR("HOLDING"); - } - - return PSTR("INVALID"); -} - - -PGM_P mp_get_cycle_pgmstr(mp_cycle_t cycle) { - switch (cycle) { - case CYCLE_MACHINING: return PSTR("MACHINING"); - case CYCLE_HOMING: return PSTR("HOMING"); - case CYCLE_PROBING: return PSTR("PROBING"); - case CYCLE_CALIBRATING: return PSTR("CALIBRATING"); - case CYCLE_JOGGING: return PSTR("JOGGING"); - } - - return PSTR("INVALID"); -} - - -PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason) { - switch (reason) { - case HOLD_REASON_USER_PAUSE: return PSTR("USER"); - case HOLD_REASON_PROGRAM_PAUSE: return PSTR("PROGRAM"); - case HOLD_REASON_PROGRAM_END: return PSTR("END"); - case HOLD_REASON_PALLET_CHANGE: return PSTR("PALLET"); - case HOLD_REASON_TOOL_CHANGE: return PSTR("TOOL"); - } - - return PSTR("INVALID"); -} - - -mp_state_t mp_get_state() {return ps.state;} -mp_cycle_t mp_get_cycle() {return ps.cycle;} - - -static void _set_state(mp_state_t state) { - if (ps.state == state) return; // No change - if (ps.state == STATE_ESTOPPED) return; // Can't leave EStop state - if (state == STATE_READY) mp_runtime_set_line(0); - ps.state = state; - report_request(); -} - - -void mp_set_cycle(mp_cycle_t cycle) { - if (ps.cycle == cycle) return; // No change - - if (ps.state != STATE_READY && cycle != CYCLE_MACHINING) { - STATUS_ERROR(STAT_INTERNAL_ERROR, "Cannot transition to %S while %S", - mp_get_cycle_pgmstr(cycle), - mp_get_state_pgmstr(ps.state)); - return; - } - - if (ps.cycle != CYCLE_MACHINING && cycle != CYCLE_MACHINING) { - STATUS_ERROR(STAT_INTERNAL_ERROR, - "Cannot transition to cycle %S while in %S", - mp_get_cycle_pgmstr(cycle), - mp_get_cycle_pgmstr(ps.cycle)); - return; - } - - ps.cycle = cycle; - report_request(); -} - - -mp_hold_reason_t mp_get_hold_reason() {return ps.hold_reason;} - - -void mp_set_hold_reason(mp_hold_reason_t reason) { - if (ps.hold_reason == reason) return; // No change - ps.hold_reason = reason; - report_request(); -} - - -bool mp_is_flushing() {return ps.flush_requested && !ps.resume_requested;} -bool mp_is_resuming() {return ps.resume_requested;} - - -bool mp_is_quiescent() { - return (mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING) && - !st_is_busy() && !mp_runtime_is_busy(); -} - - -void mp_state_optional_pause() { - if (ps.optional_pause_requested) { - mp_set_hold_reason(HOLD_REASON_USER_PAUSE); - mp_state_holding(); - } -} - - -void mp_state_holding() { - _set_state(STATE_HOLDING); - mp_set_plan_steps(false); -} - - -void mp_state_running() { - if (mp_get_state() == STATE_READY) _set_state(STATE_RUNNING); -} - - -void mp_state_idle() { - if (mp_get_state() == STATE_RUNNING) _set_state(STATE_READY); -} - - -void mp_state_estop() {_set_state(STATE_ESTOPPED);} - - -void mp_request_hold() {ps.hold_requested = true;} -void mp_request_start() {ps.start_requested = true;} -void mp_request_flush() {ps.flush_requested = true;} -void mp_request_resume() {if (ps.flush_requested) ps.resume_requested = true;} -void mp_request_optional_pause() {ps.optional_pause_requested = true;} - - -void mp_request_step() { - mp_set_plan_steps(true); - ps.start_requested = true; -} - - -/*** Feedholds, queue flushes and starts are all related. Request functions - * set flags. The callback interprets the flags according to these rules: - * - * A hold request received: - * - during motion is honored - * - during a feedhold is ignored and reset - * - when already stopped is ignored and reset - * - * A flush request received: - * - during motion is ignored but not reset - * - during a feedhold is deferred until the feedhold enters HOLDING state. - * I.e. until deceleration is complete. - * - when stopped or holding and the planner is not busy, is honored - * - * A start request received: - * - during motion is ignored and reset - * - during a feedhold is deferred until the feedhold enters HOLDING state. - * I.e. until deceleration is complete. If a queue flush request is also - * present the queue flush is done first - * - when stopped is honored and starts to run anything in the planner queue - */ -void mp_state_callback() { - if (ps.hold_requested || ps.flush_requested) { - ps.hold_requested = false; - mp_set_hold_reason(HOLD_REASON_USER_PAUSE); - - if (mp_get_state() == STATE_RUNNING) _set_state(STATE_STOPPING); - } - - // Only flush queue when idle or holding. - if (ps.flush_requested && mp_is_quiescent()) { - mach_abort_arc(); - - if (!mp_queue_is_empty()) { - mp_flush_planner(); - - // NOTE The following uses low-level mp calls for absolute position. - // Reset to actual machine position. Otherwise machine is set to the - // position of the last queued move. - for (int axis = 0; axis < AXES; axis++) - mach_set_axis_position(axis, mp_runtime_get_axis_position(axis)); - } - - // Resume - if (ps.resume_requested) { - ps.flush_requested = ps.resume_requested = false; - _set_state(STATE_READY); - } - } - - // Don't start while flushing or stopping - if (ps.start_requested && !ps.flush_requested && - mp_get_state() != STATE_STOPPING) { - ps.start_requested = false; - ps.optional_pause_requested = false; - - if (mp_get_state() == STATE_HOLDING) { - // Check if any moves are buffered - if (!mp_queue_is_empty()) { - // Always replan when coming out of a hold - mp_replan_all(); - _set_state(STATE_RUNNING); - - } else _set_state(STATE_READY); - } - } -} diff --git a/src/plan/state.h b/src/plan/state.h deleted file mode 100644 index a6a62aa..0000000 --- a/src/plan/state.h +++ /dev/null @@ -1,93 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2013 - 2015 Alden S. Hart, Jr. - Copyright (c) 2013 - 2015 Robert Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 { - STATE_READY, - STATE_ESTOPPED, - STATE_RUNNING, - STATE_STOPPING, - STATE_HOLDING, -} mp_state_t; - - -typedef enum { - CYCLE_MACHINING, - CYCLE_HOMING, - CYCLE_PROBING, - CYCLE_CALIBRATING, - CYCLE_JOGGING, -} mp_cycle_t; - - -typedef enum { - HOLD_REASON_USER_PAUSE, - HOLD_REASON_PROGRAM_PAUSE, - HOLD_REASON_PROGRAM_END, - HOLD_REASON_PALLET_CHANGE, - HOLD_REASON_TOOL_CHANGE, -} mp_hold_reason_t; - - -PGM_P mp_get_state_pgmstr(mp_state_t state); -PGM_P mp_get_cycle_pgmstr(mp_cycle_t cycle); -PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason); - -mp_state_t mp_get_state(); - -mp_cycle_t mp_get_cycle(); -void mp_set_cycle(mp_cycle_t cycle); - -mp_hold_reason_t mp_get_hold_reason(); -void mp_set_hold_reason(mp_hold_reason_t reason); - -bool mp_is_flushing(); -bool mp_is_resuming(); -bool mp_is_quiescent(); - -void mp_state_optional_pause(); -void mp_state_holding(); -void mp_state_running(); -void mp_state_idle(); -void mp_state_estop(); - -void mp_request_hold(); -void mp_request_start(); -void mp_request_flush(); -void mp_request_resume(); -void mp_request_optional_pause(); -void mp_request_step(); - -void mp_state_callback(); diff --git a/src/probing.c b/src/probing.c deleted file mode 100644 index 5085847..0000000 --- a/src/probing.c +++ /dev/null @@ -1,216 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 "machine.h" -#include "axis.h" -#include "spindle.h" -#include "switch.h" -#include "util.h" - -#include "plan/planner.h" -#include "plan/runtime.h" -#include "plan/state.h" - -#include - -#include -#include -#include -#include - - -#define MINIMUM_PROBE_TRAVEL 0.254 - - -typedef enum { - PROBE_FAILED, // probe reached endpoint without triggering - PROBE_SUCCEEDED, // probe was triggered, pb.results has position - PROBE_WAITING, // probe is waiting to be started -} probe_state_t; - - -typedef struct { - probe_state_t state; - float results[AXES]; // probing results - - void (*func)(); // binding for callback function state machine - - // state saved from gcode model - uint8_t saved_distance_mode; // G90, G91 global setting - uint8_t saved_coord_system; // G54 - G59 setting - - // probe destination - float start_position[AXES]; - float target[AXES]; - bool flags[AXES]; -} probing_t; - - -static probing_t pb = {0}; - - -/* Note: When coding a cycle (like this one) you get to perform one - * queued move per entry into the continuation, then you must exit. - * - * Another Note: When coding a cycle (like this one) you must wait - * until the last move has actually been queued (or has finished) - * before declaring the cycle to be done. Otherwise there is a nasty - * race condition that will accept the next command before the position - * of the final move has been recorded in the Gcode model. - */ - - -static void _probe_restore_settings() { - // we should be stopped now, but in case of switch closure - mp_flush_planner(); - - // restore coordinate system and distance mode - mach_set_coord_system(pb.saved_coord_system); - mach_set_distance_mode(pb.saved_distance_mode); - - // update the model with actual position - mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); - - mp_set_cycle(CYCLE_MACHINING); // Default cycle -} - - -static void _probing_error_exit(stat_t status) { - _probe_restore_settings(); // clean up and exit - status_error(status); -} - - -static void _probing_finish() { - bool closed = switch_is_active(SW_PROBE); - pb.state = closed ? PROBE_SUCCEEDED : PROBE_FAILED; - - for (int axis = 0; axis < AXES; axis++) { - // if we got here because of a feed hold keep the model position correct - mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); - - // store the probe results - pb.results[axis] = mach_get_axis_position(axis); - } - - _probe_restore_settings(); -} - - -static void _probing_start() { - // initial probe state, don't probe if we're already contacted! - bool closed = switch_is_active(SW_PROBE); - - if (!closed) { - stat_t status = mach_feed(pb.target, pb.flags); - if (status) return _probing_error_exit(status); - } - - pb.func = _probing_finish; -} - - -/* G38.2 homing cycle using limit switches - * - * These initializations are required before starting the probing cycle. - * They must be done after the planner has exhasted all current CYCLE moves as - * they affect the runtime (specifically the switch modes). Side effects would - * include limit switches initiating probe actions instead of just killing - * movement - */ -static void _probing_init() { - // NOTE: it is *not* an error condition for the probe not to trigger. - // it is an error for the limit or homing switches to fire, or for some other - // configuration error. - pb.state = PROBE_FAILED; - mp_set_cycle(CYCLE_PROBING); - - // initialize the axes - for (int axis = 0; axis < AXES; axis++) - pb.start_position[axis] = mach_get_axis_position(axis); - - // error if the probe target is too close to the current position - if (axis_get_vector_length(pb.start_position, pb.target) < - MINIMUM_PROBE_TRAVEL) - _probing_error_exit(STAT_PROBE_INVALID_DEST); - - // error if the probe target requires a move along the A/B/C axes - for (int axis = 0; axis < AXES; axis++) - if (fp_NE(pb.start_position[axis], pb.target[axis])) - _probing_error_exit(STAT_MOVE_DURING_PROBE); - - // probe in absolute machine coords - pb.saved_coord_system = mach_get_coord_system(); - pb.saved_distance_mode = mach_get_distance_mode(); - mach_set_distance_mode(ABSOLUTE_MODE); - mach_set_coord_system(ABSOLUTE_COORDS); - - mach_set_spindle_mode(SPINDLE_OFF); - - // start the move - pb.func = _probing_start; -} - - -bool mach_is_probing() { - return mp_get_cycle() == CYCLE_PROBING || pb.state == PROBE_WAITING; -} - - -/// G38.2 homing cycle using limit switches -stat_t mach_probe(float target[], bool flags[]) { - // trap zero feed rate condition - if (mach_get_feed_mode() != INVERSE_TIME_MODE && - fp_ZERO(mach_get_feed_rate())) - return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - - // trap no axes specified - if (!flags[AXIS_X] && !flags[AXIS_Y] && !flags[AXIS_Z]) - return STAT_GCODE_AXIS_IS_MISSING; - - // set probe move endpoint - copy_vector(pb.target, target); // set probe move endpoint - copy_vector(pb.flags, flags); // set axes involved on the move - clear_vector(pb.results); // clear the old probe position. - // NOTE: relying on pb.results will not detect a probe to (0, 0, 0). - - // wait until planner queue empties before completing initialization - pb.state = PROBE_WAITING; - pb.func = _probing_init; // bind probing initialization function - - return STAT_OK; -} - - -/// main loop callback for running the homing cycle -void mach_probe_callback() { - // sync to planner move ends - if (!mach_is_probing() || mp_get_state() != STATE_READY) return; - - pb.func(); // execute the current homing move -} diff --git a/src/probing.h b/src/probing.h deleted file mode 100644 index 0e3fc01..0000000 --- a/src/probing.h +++ /dev/null @@ -1,37 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 mach_is_probing(); -stat_t mach_probe(float target[], bool flags[]); -void mach_probe_callback(); diff --git a/src/pwm_spindle.c b/src/pwm_spindle.c deleted file mode 100644 index 25d40eb..0000000 --- a/src/pwm_spindle.c +++ /dev/null @@ -1,152 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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" - - -typedef struct { - uint16_t freq; // base frequency for PWM driver, in Hz - float min_rpm; - float max_rpm; - float min_duty; - float max_duty; - bool reverse; - bool enable_invert; - bool estop; -} pwm_spindle_t; - - -static pwm_spindle_t spindle = { - .freq = SPINDLE_PWM_FREQUENCY, - .min_rpm = SPINDLE_MIN_RPM, - .max_rpm = SPINDLE_MAX_RPM, - .min_duty = SPINDLE_MIN_DUTY, - .max_duty = SPINDLE_MAX_DUTY, - .reverse = SPINDLE_REVERSE, - .enable_invert = false, - .estop = false, -}; - - -static void _spindle_set_pwm(spindle_mode_t mode, float speed) { - if (mode == SPINDLE_OFF || speed < spindle.min_rpm || spindle.estop) { - TIMER_PWM.CTRLA = 0; - return; - } - - // Clamp speed - if (spindle.max_rpm < speed) speed = spindle.max_rpm; - - // Set clock period and optimal prescaler value - float prescale = F_CPU / 65536.0 / spindle.freq; - if (prescale <= 1) { - TIMER_PWM.PER = F_CPU / spindle.freq; - TIMER_PWM.CTRLA = TC_CLKSEL_DIV1_gc; - - } else if (prescale <= 2) { - TIMER_PWM.PER = F_CPU / 2 / spindle.freq; - TIMER_PWM.CTRLA = TC_CLKSEL_DIV2_gc; - - } else if (prescale <= 4) { - TIMER_PWM.PER = F_CPU / 4 / spindle.freq; - TIMER_PWM.CTRLA = TC_CLKSEL_DIV4_gc; - - } else if (prescale <= 8) { - TIMER_PWM.PER = F_CPU / 8 / spindle.freq; - TIMER_PWM.CTRLA = TC_CLKSEL_DIV8_gc; - - } else if (prescale <= 64) { - TIMER_PWM.PER = F_CPU / 64 / spindle.freq; - TIMER_PWM.CTRLA = TC_CLKSEL_DIV64_gc; - - } else TIMER_PWM.CTRLA = 0; - - // Map RPM to duty cycle - float duty = (speed - spindle.min_rpm) / (spindle.max_rpm - spindle.min_rpm) * - (spindle.max_duty - spindle.min_duty) + spindle.min_duty; - - TIMER_PWM.CCB = TIMER_PWM.PER * duty; -} - - -static void _spindle_set_enable(bool enable) { - if (enable ^ spindle.enable_invert) - PORT(SPIN_ENABLE_PIN)->OUTSET = BM(SPIN_ENABLE_PIN); - else PORT(SPIN_ENABLE_PIN)->OUTCLR = BM(SPIN_ENABLE_PIN); -} - - -static void _spindle_set_dir(bool forward) { - if (forward ^ spindle.reverse) PORT(SPIN_DIR_PIN)->OUTCLR = BM(SPIN_DIR_PIN); - else PORT(SPIN_DIR_PIN)->OUTSET = BM(SPIN_DIR_PIN); -} - - -void pwm_spindle_init() { - // Configure IO - PORT(SPIN_PWM_PIN)->DIRSET = BM(SPIN_PWM_PIN); // PWM Output - _spindle_set_dir(true); - PORT(SPIN_DIR_PIN)->DIRSET = BM(SPIN_DIR_PIN); // Dir Output - _spindle_set_enable(false); - PORT(SPIN_ENABLE_PIN)->DIRSET = BM(SPIN_ENABLE_PIN); // Enable output - - // Configure clock - TIMER_PWM.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SINGLESLOPE_gc; -} - - -void pwm_spindle_set(spindle_mode_t mode, float speed) { - _spindle_set_dir(mode == SPINDLE_CW); - _spindle_set_pwm(mode, speed); - _spindle_set_enable(mode != SPINDLE_OFF && TIMER_PWM.CTRLA); -} - - -void pwm_spindle_estop() { - spindle.estop = true; - _spindle_set_pwm(SPINDLE_OFF, 0); -} - - -// TODO these need more effort and should work with the huanyang spindle too -float get_max_spin(int index) {return spindle.max_rpm;} -void set_max_spin(int axis, float value) {spindle.max_rpm = value;} -float get_min_spin(int index) {return spindle.min_rpm;} -void set_min_spin(int axis, float value) {spindle.min_rpm = value;} -float get_spin_min_pulse(int index) {return spindle.min_duty;} -void set_spin_min_pulse(int axis, float value) {spindle.min_duty = value;} -float get_spin_max_pulse(int index) {return spindle.max_duty;} -void set_spin_max_pulse(int axis, float value) {spindle.max_duty = value;} -uint8_t get_spin_reverse(int index) {return spindle.reverse;} -void set_spin_reverse(int axis, uint8_t value) {spindle.reverse = value;} -float get_spin_up(int index) {return 0;} -void set_spin_up(int axis, float value) {} -float get_spin_down(int index) {return 0;} -void set_spin_down(int axis, float value) {} diff --git a/src/pwm_spindle.h b/src/pwm_spindle.h deleted file mode 100644 index 2177876..0000000 --- a/src/pwm_spindle.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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_estop(); diff --git a/src/report.c b/src/report.c deleted file mode 100644 index b39bfd1..0000000 --- a/src/report.c +++ /dev/null @@ -1,66 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - -#include -#include - - -static bool report_requested = false; -static bool report_full = false; -static uint32_t last_report = 0; - - -void report_request() { - report_requested = true; -} - - -void report_request_full() { - report_requested = report_full = true; -} - - -void report_callback() { - if (usart_tx_full()) return; // Wait for buffer space - - if (report_requested && usart_tx_empty()) { - uint32_t now = rtc_get_time(); - if (now - last_report < 100) return; - last_report = now; - - vars_report(report_full); - report_requested = report_full = false; - } -} diff --git a/src/report.h b/src/report.h deleted file mode 100644 index c66f05a..0000000 --- a/src/report.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/ringbuf.def b/src/ringbuf.def deleted file mode 100644 index eff59c9..0000000 --- a/src/ringbuf.def +++ /dev/null @@ -1,158 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/rtc.c b/src/rtc.c deleted file mode 100644 index 73f5947..0000000 --- a/src/rtc.c +++ /dev/null @@ -1,77 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2013 Alden S. Hart Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/rtc.h b/src/rtc.h deleted file mode 100644 index 3d69237..0000000 --- a/src/rtc.h +++ /dev/null @@ -1,38 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2013 Alden S. Hart Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/spindle.c b/src/spindle.c deleted file mode 100644 index 7a57fb5..0000000 --- a/src/spindle.c +++ /dev/null @@ -1,113 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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_PWM, - SPINDLE_TYPE_HUANYANG, -} spindle_type_t; - - -typedef struct { - spindle_type_t type; - spindle_mode_t mode; - float speed; -} spindle_t; - - -static spindle_t spindle = {.type = SPINDLE_TYPE}; - - -void spindle_init() { - pwm_spindle_init(); - huanyang_init(); -} - - -void _spindle_set(spindle_mode_t mode, float speed) { - spindle.mode = mode; - spindle.speed = speed; - - switch (spindle.type) { - case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, speed); break; - case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, speed); break; - } -} - - -void spindle_set_mode(spindle_mode_t mode) { - spindle.mode = mode; - - switch (spindle.type) { - case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, spindle.speed); break; - case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, spindle.speed); break; - } -} - - -void spindle_set_speed(float speed) { - spindle.speed = speed; - - switch (spindle.type) { - case SPINDLE_TYPE_PWM: pwm_spindle_set(spindle.mode, speed); break; - case SPINDLE_TYPE_HUANYANG: huanyang_set(spindle.mode, speed); break; - } -} - - -spindle_mode_t spindle_get_mode() {return spindle.mode;} -float spindle_get_speed() {return spindle.speed;} - - -void spindle_estop() { - switch (spindle.type) { - case SPINDLE_TYPE_PWM: pwm_spindle_estop(); break; - case SPINDLE_TYPE_HUANYANG: huanyang_estop(); break; - } -} - - -uint8_t get_spindle_type() {return spindle.type;} - - -void set_spindle_type(uint8_t value) { - if (value != spindle.type) { - spindle_mode_t mode = spindle.mode; - float speed = spindle.speed; - - _spindle_set(SPINDLE_OFF, 0); - spindle.type = value; - _spindle_set(mode, speed); - } -} diff --git a/src/spindle.h b/src/spindle.h deleted file mode 100644 index b583d55..0000000 --- a/src/spindle.h +++ /dev/null @@ -1,39 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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_estop(); diff --git a/src/status.c b/src/status.c deleted file mode 100644 index ad06bd4..0000000 --- a/src/status.c +++ /dev/null @@ -1,120 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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\":\"%S\",\"msg\":\""), status_level_pgmstr(level)); - - // Message - if (msg) { - va_start(args, msg); - vfprintf_P(stdout, msg, args); - va_end(args); - - } else printf_P(status_to_pgmstr(code)); - - putchar('"'); - - // Code - if (code) printf_P(PSTR(", \"code\": %d"), code); - - // Location - if (location) printf_P(PSTR(", \"where\": \"%S\""), location); - - putchar('}'); - putchar('\n'); - - return code; -} - - -void status_help() { - putchar('{'); - - for (int i = 0; i < STAT_MAX; i++) { - if (i) putchar(','); - putchar('\n'); - printf_P(PSTR(" \"%d\": \"%S\""), i, status_to_pgmstr(i)); - } - - putchar('\n'); - putchar('}'); - putchar('\n'); -} - - -/// Alarm state; send an exception report and stop processing input -stat_t status_alarm(const char *location, stat_t code) { - status_message_P(location, STAT_LEVEL_ERROR, code, 0); - estop_trigger(code); - while (!usart_tx_empty()) continue; - return code; -} diff --git a/src/status.h b/src/status.h deleted file mode 100644 index 2615003..0000000 --- a/src/status.h +++ /dev/null @@ -1,87 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 - - -// RITORNO is a handy way to provide exception returns -// It returns only if an error occurred. (ritorno is Italian for return) -#define RITORNO(a) if ((status_code = a) != STAT_OK) {return status_code;} - -typedef enum { -#define STAT_MSG(NAME, TEXT) STAT_##NAME, -#include "messages.def" -#undef STAT_MSG - - STAT_DO_NOT_EXCEED = 255 // Do not exceed 255 -} stat_t; - - -typedef enum { - STAT_LEVEL_INFO, - STAT_LEVEL_DEBUG, - STAT_LEVEL_WARNING, - STAT_LEVEL_ERROR, -} status_level_t; - - -extern stat_t status_code; - -const char *status_to_pgmstr(stat_t code); -const char *status_level_pgmstr(status_level_t level); -stat_t status_error(stat_t code); -stat_t status_message_P(const char *location, status_level_t level, - stat_t code, const char *msg, ...); -void status_help(); - -/// Enter alarm state. returns same status code -stat_t status_alarm(const char *location, stat_t status); - -#define TO_STRING(x) _TO_STRING(x) -#define _TO_STRING(x) #x - -#define STATUS_LOCATION PSTR(__FILE__ ":" TO_STRING(__LINE__)) - -#define STATUS_MESSAGE(LEVEL, CODE, MSG, ...) \ - status_message_P(STATUS_LOCATION, LEVEL, CODE, PSTR(MSG), ##__VA_ARGS__) - -#define STATUS_INFO(MSG, ...) \ - STATUS_MESSAGE(STAT_LEVEL_INFO, STAT_OK, MSG, ##__VA_ARGS__) - -#define STATUS_DEBUG(MSG, ...) \ - STATUS_MESSAGE(STAT_LEVEL_WARNING, STAT_OK, MSG, ##__VA_ARGS__) - -#define STATUS_WARNING(MSG, ...) \ - STATUS_MESSAGE(STAT_LEVEL_WARNING, STAT_OK, MSG, ##__VA_ARGS__) - -#define STATUS_ERROR(CODE, MSG, ...) \ - STATUS_MESSAGE(STAT_LEVEL_ERROR, CODE, MSG, ##__VA_ARGS__) - -#define ALARM(CODE) status_alarm(STATUS_LOCATION, CODE) -#define ASSERT(COND) do {if (!(COND)) ALARM(STAT_INTERNAL_ERROR);} while (0) diff --git a/src/stepper.c b/src/stepper.c deleted file mode 100644 index c929d25..0000000 --- a/src/stepper.c +++ /dev/null @@ -1,233 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2013 - 2015 Robert Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 -#include - - -typedef enum { - MOVE_TYPE_NULL, // null move - does a no-op - MOVE_TYPE_ALINE, // acceleration planned line - MOVE_TYPE_DWELL, // delay with no movement -} move_type_t; - - -typedef struct { - // Runtime - bool busy; - bool requesting; - uint16_t dwell; - - // Move prep - bool move_ready; // prepped move ready for loader - bool move_queued; // prepped move queued - move_type_t move_type; - uint16_t seg_period; - uint32_t prep_dwell; -} stepper_t; - - -static stepper_t st = {0}; - - -void stepper_init() { - // Setup step timer - TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // waveform mode - TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // interrupt mode - TIMER_STEP.PER = STEP_TIMER_POLL; // timer idle rate - TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // start step timer -} - - -void st_shutdown() { - for (int motor = 0; motor < MOTORS; motor++) - motor_enable(motor, false); - - st.dwell = 0; - st.move_type = MOVE_TYPE_NULL; -} - - -/// Return true if motors or dwell are running -bool st_is_busy() {return st.busy;} - - -/// Interrupt handler for calling move exec function. -/// ADC channel 0 triggered by load ISR as a "software" interrupt. -ISR(ADCB_CH0_vect) { - while (true) { - stat_t status = mp_exec_move(); - - switch (status) { - case STAT_NOOP: st.busy = false; break; // No command executed - case STAT_EAGAIN: continue; // No command executed, try again - - case STAT_OK: // Move executed - if (!st.move_queued) ALARM(STAT_EXPECTED_MOVE); // No move was queued - st.move_queued = false; - st.move_ready = true; - break; - - default: ALARM(status); break; - } - - break; - } - - ADCB_CH0_INTCTRL = 0; - st.requesting = false; -} - - -static void _request_exec_move() { - if (st.requesting) return; - st.requesting = true; - - // Use ADC as a "software" interrupt to trigger next move exec - ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc; // LO level interrupt - ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm; -} - - -/// Step timer interrupt routine -/// Dequeue move and load into stepper struct -ISR(STEP_TIMER_ISR) { - // Dwell - if (st.dwell && --st.dwell) return; - - // End last move - TIMER_STEP.PER = STEP_TIMER_POLL; - - DMA.INTFLAGS = 0xff; // clear all interrups - for (int motor = 0; motor < MOTORS; motor++) - motor_end_move(motor); - - if (estop_triggered()) { - st.move_type = MOVE_TYPE_NULL; - return; - } - - // If the next move is not ready try to load it - if (!st.move_ready) { - _request_exec_move(); - return; - } - - // Wait until all motors have energized - if (motor_energizing()) return; - - // Start move - if (st.seg_period) { - for (int motor = 0; motor < MOTORS; motor++) - motor_load_move(motor); - - TIMER_STEP.PER = st.seg_period; - st.busy = true; - - // Start dwell - st.dwell = st.prep_dwell; - } - - // We are done with this move - st.move_type = MOVE_TYPE_NULL; - st.seg_period = 0; // clear timer - st.prep_dwell = 0; // clear dwell - st.move_ready = false; // flip the flag back - - // Request next move if not currently in a dwell. Requesting the next move - // may power up motors and the motors should not be powered up during a dwell. - if (!st.dwell) _request_exec_move(); -} - - -/* Prepare the next move - * - * This function precomputes the next pulse segment (move) so it can - * be executed quickly in the ISR. It works in steps, rather than - * length units. All args are provided as floats which converted here - * to integer values. - * - * Args: - * @param target signed position in steps for each motor. - * Steps are fractional. Their sign indicates direction. Motors not in the - * move have 0 steps. - * - * @param time is segment run time in minutes. If timing is not 100% - * accurate this will affect the move velocity but not travel distance. - */ -stat_t st_prep_line(float time, const float target[], const int32_t error[]) { - // Trap conditions that would prevent queueing the line - if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR); - if (isinf(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_INFINITE); - if (isnan(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_NAN); - if (time < EPSILON) return ALARM(STAT_MINIMUM_TIME_MOVE); - if (MAX_SEGMENT_TIME < time) return ALARM(STAT_MAXIMUM_TIME_MOVE); - - // Setup segment parameters - st.move_type = MOVE_TYPE_ALINE; - st.seg_period = round(time * 60 * STEP_TIMER_FREQ); // Must fit 16-bit - int32_t seg_clocks = (int32_t)st.seg_period * STEP_TIMER_DIV; - - // Prepare motor moves - for (int motor = 0; motor < MOTORS; motor++) - RITORNO - (motor_prep_move(motor, seg_clocks, target[motor], error[motor], time)); - - st.move_queued = true; // signal prep buffer ready (do this last) - - return STAT_OK; -} - - -/// Add a dwell to the move buffer -void st_prep_dwell(float seconds) { - if (st.move_ready) ALARM(STAT_INTERNAL_ERROR); - st.move_type = MOVE_TYPE_DWELL; - st.seg_period = STEP_TIMER_FREQ * 0.001; // 1 ms - st.prep_dwell = seconds * 1000; // convert to ms - st.move_queued = true; // signal prep buffer ready -} - - -void st_get_error(int32_t error[]) { - for (int motor = 0; motor < MOTORS; motor++) - error[motor] = motor_get_error(motor); -} diff --git a/src/stepper.h b/src/stepper.h deleted file mode 100644 index 3aa57b9..0000000 --- a/src/stepper.h +++ /dev/null @@ -1,43 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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(); -bool st_is_busy(); -stat_t st_prep_line(float time, const float target[], const int32_t error[]); -void st_prep_dwell(float seconds); -void st_get_error(int32_t error[]); diff --git a/src/switch.c b/src/switch.c deleted file mode 100644 index d97632a..0000000 --- a/src/switch.c +++ /dev/null @@ -1,204 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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" - -\******************************************************************************/ - -/* Normally open switches (NO) trigger an interrupt on the - * falling edge and lockout subsequent interrupts for the defined - * lockout period. This approach beats doing debouncing as an - * integration as switches fire immediately. - * - * Normally closed switches (NC) trigger an interrupt on the - * rising edge and lockout subsequent interrupts for the defined - * lockout period. - * - * These functions interact with each other to process switch closures - * and firing. Each switch has a counter which is initially set to - * negative SW_DEGLITCH_TICKS. When a switch closure is DETECTED the - * count increments for each RTC tick. When the count reaches zero - * the switch is tripped and action occurs. The counter continues to - * increment positive until the lockout is exceeded. - */ - -#include "switch.h" -#include "config.h" - -#include - -#include - - -typedef enum { - SW_IDLE, - SW_DEGLITCHING, - SW_LOCKOUT -} switch_debounce_t; - - -typedef struct { - uint8_t pin; - switch_type_t type; - - switch_callback_t cb; - bool state; - switch_debounce_t debounce; - int16_t count; -} switch_t; - - - -static switch_t switches[SWITCHES] = { - {.pin = MIN_X_PIN, .type = SW_NORMALLY_OPEN}, - {.pin = MAX_X_PIN, .type = SW_NORMALLY_OPEN}, - {.pin = MIN_Y_PIN, .type = SW_NORMALLY_OPEN}, - {.pin = MAX_X_PIN, .type = SW_NORMALLY_OPEN}, - {.pin = MIN_Z_PIN, .type = SW_NORMALLY_OPEN}, - {.pin = MAX_Z_PIN, .type = SW_NORMALLY_OPEN}, - {.pin = MIN_A_PIN, .type = SW_NORMALLY_OPEN}, - {.pin = MAX_A_PIN, .type = SW_NORMALLY_OPEN}, - {.pin = ESTOP_PIN, .type = SW_NORMALLY_OPEN}, - // {.pin = PROBE_PIN, .type = SW_NORMALLY_OPEN}, -}; - - -static bool _read_state(const switch_t *s) { - // A normally open switch drives the pin low when thrown - return (s->type == SW_NORMALLY_OPEN) ^ IN_PIN(s->pin); -} - - -static void _switch_isr() { - for (int i = 0; i < SWITCHES; i++) { - switch_t *s = &switches[i]; - bool state = _read_state(s); - - if (state == s->state || s->type == SW_DISABLED) continue; - - s->debounce = SW_DEGLITCHING; - s->count = -SW_DEGLITCH_TICKS; // reset deglitch count - s->state = state; - PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0 - } -} - - -// Switch interrupt handler vectors -ISR(PORTA_INT0_vect) {_switch_isr();} -ISR(PORTB_INT0_vect) {_switch_isr();} -ISR(PORTC_INT0_vect) {_switch_isr();} -ISR(PORTD_INT0_vect) {_switch_isr();} -ISR(PORTE_INT0_vect) {_switch_isr();} -ISR(PORTF_INT0_vect) {_switch_isr();} - - -void _switch_enable(switch_t *s, bool enable) { - if (enable) { - PORT(s->pin)->INT0MASK |= BM(s->pin); // Enable INT0 - s->state = _read_state(s); // Initialize state - s->debounce = SW_IDLE; - - } else PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0 -} - - -void switch_init() { - for (int i = 0; i < SWITCHES; i++) { - switch_t *s = &switches[i]; - - // Pull up and trigger on both edges - PINCTRL_PIN(s->pin) = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; - DIRCLR_PIN(s->pin); // Input - PORT(s->pin)->INTCTRL |= SWITCH_INTLVL; // Set interrupt level - - _switch_enable(s, s->type != SW_DISABLED); - } -} - - -/// Called from RTC on each tick -void switch_rtc_callback() { - for (int i = 0; i < SWITCHES; i++) { - switch_t *s = &switches[i]; - - if (s->type == SW_DISABLED || s->debounce == SW_IDLE) continue; - - // state is either lockout or deglitching - if (++s->count == SW_LOCKOUT_TICKS) { - PORT(s->pin)->INT0MASK |= BM(s->pin); // Renable INT0 - bool state = _read_state(s); - s->debounce = SW_IDLE; - - // check if the state has changed while we were in lockout - if (s->state != state) { - s->state = state; - s->debounce = SW_DEGLITCHING; - s->count = -SW_DEGLITCH_TICKS; - } - - continue; - } - - if (!s->count) { // switch triggered - s->debounce = SW_LOCKOUT; - if (s->cb) s->cb(i, s->state); - } - } -} - - -bool switch_is_active(int index) { - return switches[index].state; -} - - -bool switch_is_enabled(int index) { - return switch_get_type(index) != SW_DISABLED; -} - - -switch_type_t switch_get_type(int index) { - return switches[index].type; -} - - -void switch_set_type(int index, switch_type_t type) { - switch_t *s = &switches[index]; - - if (s->type != type) { - s->type = type; - _switch_enable(s, type != SW_DISABLED); - } -} - - -void switch_set_callback(int index, switch_callback_t cb) { - switches[index].cb = cb; -} - - -// Var callbacks -uint8_t get_switch_type(int index) {return switch_get_type(index);} -void set_switch_type(int index, uint8_t value) {switch_set_type(index, value);} diff --git a/src/switch.h b/src/switch.h deleted file mode 100644 index 3d0db61..0000000 --- a/src/switch.h +++ /dev/null @@ -1,69 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/usart.c b/src/usart.c deleted file mode 100644 index 7d66f63..0000000 --- a/src/usart.c +++ /dev/null @@ -1,282 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/usart.h b/src/usart.h deleted file mode 100644 index ccb3ac1..0000000 --- a/src/usart.h +++ /dev/null @@ -1,77 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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/util.c b/src/util.c deleted file mode 100644 index 74ea051..0000000 --- a/src/util.c +++ /dev/null @@ -1,49 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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" - - - - -/// Fast inverse square root originally from Quake III Arena code. Original -/// comments left intact. -/// See: https://en.wikipedia.org/wiki/Fast_inverse_square_root -float invsqrt(float number) { - const float threehalfs = 1.5F; - - float x2 = number * 0.5; - float y = number; - long i = *(long *)&y; // evil floating point bit level hacking - i = 0x5f3759df - (i >> 1); // what the fuck? - y = *(float *)&i; - y = y * (threehalfs - x2 * y * y); // 1st iteration - y = y * (threehalfs - x2 * y * y); // 2nd iteration, this can be removed - - return y; //isnan(y) ? 1.0 / sqrt(number) : y; -} diff --git a/src/util.h b/src/util.h deleted file mode 100644 index 581bbb3..0000000 --- a/src/util.h +++ /dev/null @@ -1,68 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2014 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 float min(float a, float b) {return a < b ? a : b;} -inline float max(float a, float b) {return a < b ? b : a;} -inline float min3(float a, float b, float c) {return min(min(a, b), c);} -inline float max3(float a, float b, float c) {return max(max(a, b), c);} -inline float min4(float a, float b, float c, float d) -{return min(min(a, b), min(c, d));} -inline float max4(float a, float b, float c, float d) -{return max(max(a, b), max(c, d));} - -float invsqrt(float number); - -// Floating-point utilities -#define EPSILON 0.00001 // allowable rounding error for floats -inline bool fp_EQ(float a, float b) {return fabs(a - b) < EPSILON;} -inline bool fp_NE(float a, float b) {return fabs(a - b) > EPSILON;} -inline bool fp_ZERO(float a) {return fabs(a) < EPSILON;} -inline bool fp_NOT_ZERO(float a) {return !fp_ZERO(a);} -inline bool fp_FALSE(float a) {return fp_ZERO(a);} -inline bool fp_TRUE(float a) {return !fp_ZERO(a);} - -// Constants -#define MM_PER_INCH 25.4 -#define INCHES_PER_MM (1 / 25.4) -#define MICROSECONDS_PER_MINUTE 60000000.0 diff --git a/src/varcb.c b/src/varcb.c deleted file mode 100644 index 8f425ad..0000000 --- a/src/varcb.c +++ /dev/null @@ -1,89 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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_position(int index) {return mp_runtime_get_axis_position(index);} - -// GCode -int32_t get_line() {return mp_runtime_get_line();} -PGM_P get_unit() {return gs_get_units_pgmstr(mach_get_units());} -float get_speed() {return spindle_get_speed();} -float get_feed() {return mach_get_feed_rate();} // TODO get runtime value -uint8_t get_tool() {return mp_runtime_get_tool();} - - -PGM_P get_feed_mode() { - return gs_get_feed_mode_pgmstr(mach_get_feed_mode()); -} - - -PGM_P get_plane() {return gs_get_plane_pgmstr(mach_get_plane());} - - -PGM_P get_coord_system() { - return gs_get_coord_system_pgmstr(mach_get_coord_system()); -} - - -bool get_abs_override() {return mach_get_absolute_mode();} -PGM_P get_path_mode() {return gs_get_path_mode_pgmstr(mach_get_path_mode());} - - -PGM_P get_distance_mode() { - return gs_get_distance_mode_pgmstr(mach_get_distance_mode()); -} - - -PGM_P get_arc_dist_mode() { - return gs_get_distance_mode_pgmstr(mach_get_arc_distance_mode()); -} - - -float get_feed_override() {return mach_get_feed_override();} -float get_speed_override() {return mach_get_spindle_override();} -bool get_mist_coolant() {return coolant_get_mist();} -bool get_flood_coolant() {return coolant_get_flood();} - -// System -float get_velocity() {return mp_runtime_get_velocity();} -bool get_echo() {return usart_is_set(USART_ECHO);} -void set_echo(bool value) {return usart_set(USART_ECHO, value);} -PGM_P get_state() {return mp_get_state_pgmstr(mp_get_state());} -PGM_P get_cycle() {return mp_get_cycle_pgmstr(mp_get_cycle());} - - -PGM_P get_hold_reason() { - return mp_get_hold_reason_pgmstr(mp_get_hold_reason()); -} diff --git a/src/vars.c b/src/vars.c deleted file mode 100644 index 6896ee6..0000000 --- a/src/vars.c +++ /dev/null @@ -1,513 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 -#include -#include -#include -#include -#include -#include - -#include -#include - - -typedef uint8_t flags_t; -typedef const char *string; -typedef PGM_P pstring; - - -// Format strings -static const char code_fmt[] PROGMEM = "\"%s\":"; -static const char indexed_code_fmt[] PROGMEM = "\"%c%s\":"; - - -// Type names -static const char bool_name [] PROGMEM = ""; -#define TYPE_NAME(TYPE) static const char TYPE##_name [] PROGMEM = "<" #TYPE ">" -MAP(TYPE_NAME, SEMI, flags_t, string, pstring, float, uint8_t, uint16_t, - int32_t, char); - - -// String -static void var_print_string(string s) { - printf_P(PSTR("\"%s\""), s); -} - -// Program string -static void var_print_pstring(pstring s) { - printf_P(PSTR("\"%S\""), s); -} - - -// Flags -extern void print_status_flags(uint8_t x); - -static void var_print_flags_t(uint8_t x) { - print_status_flags(x); -} - - -// Float -static void var_print_float(float x) { - char buf[20]; - - int len = sprintf_P(buf, PSTR("%.6f"), x); - - // Remove trailing zeros - for (int i = len; 0 < i; i--) { - if (buf[i - 1] == '.') buf[i - 1] = 0; - else if (buf[i - 1] == '0') { - buf[i - 1] = 0; - continue; - } - - break; - } - - printf(buf); -} - - -static float var_parse_float(const char *value) { - return strtod(value, 0); -} - - -// Bool -static void var_print_bool(bool x) { - printf_P(x ? PSTR("true") : PSTR("false")); -} - - -static bool var_parse_bool(const char *value) { - return !strcasecmp(value, "true") || var_parse_float(value); -} - - -static uint8_t eeprom_read_bool(bool *addr) { - return eeprom_read_byte((uint8_t *)addr); -} - - -static void eeprom_update_bool(bool *addr, bool value) { - eeprom_update_byte((uint8_t *)addr, value); -} - - -// Char -static void var_print_char(char x) {putchar('"'); putchar(x); putchar('"');} -static char var_parse_char(const char *value) {return value[0];} - - -static uint8_t eeprom_read_char(char *addr) { - return eeprom_read_byte((uint8_t *)addr); -} - - -static void eeprom_update_char(char *addr, char value) { - eeprom_update_byte((uint8_t *)addr, value); -} - - -// int8 -#if 0 -static void var_print_int8_t(int8_t x) { - printf_P(PSTR("%"PRIi8), x); -} - - -static int8_t var_parse_int8_t(const char *value) { - return strtol(value, 0, 0); -} - - -static int8_t eeprom_read_int8_t(int8_t *addr) { - return eeprom_read_byte((uint8_t *)addr); -} - - -static void eeprom_update_int8_t(int8_t *addr, int8_t value) { - eeprom_update_byte((uint8_t *)addr, value); -} -#endif - -// uint8 -static void var_print_uint8_t(uint8_t x) { - printf_P(PSTR("%"PRIu8), x); -} - - -static uint8_t var_parse_uint8_t(const char *value) { - return strtol(value, 0, 0); -} - - -static uint8_t eeprom_read_uint8_t(uint8_t *addr) { - return eeprom_read_byte(addr); -} - - -static void eeprom_update_uint8_t(uint8_t *addr, uint8_t value) { - eeprom_update_byte(addr, value); -} - - -// unit16 -static void var_print_uint16_t(uint16_t x) { - printf_P(PSTR("%"PRIu16), x); -} - - -static uint16_t var_parse_uint16_t(const char *value) { - return strtoul(value, 0, 0); -} - - -static uint16_t eeprom_read_uint16_t(uint16_t *addr) { - return eeprom_read_word(addr); -} - - -static void eeprom_update_uint16_t(uint16_t *addr, uint16_t value) { - eeprom_update_word(addr, value); -} - - -// int32 -static void var_print_int32_t(uint32_t x) { - printf_P(PSTR("%"PRIi32), x); -} - - -// Var forward declarations -#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ - TYPE get_##NAME(IF(INDEX)(int index)); \ - IF(SET) \ - (void set_##NAME(IF(INDEX)(int index,) TYPE value);) - -#include "vars.def" -#undef VAR - -// Var names, count & help -#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, HELP) \ - static const char NAME##_name[] PROGMEM = #NAME; \ - static const char NAME##_help[] PROGMEM = HELP; - -#include "vars.def" -#undef VAR - -// EEPROM storage -#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, HELP) \ - IF(SAVE) \ - (static TYPE NAME##_eeprom IF(INDEX)([INDEX]) EEMEM;) - -#include "vars.def" -#undef VAR - -static uint16_t eeprom_crc EEMEM; - -// Last value -#define VAR(NAME, CODE, TYPE, INDEX, ...) \ - static TYPE NAME##_state IF(INDEX)([INDEX]); - -#include "vars.def" -#undef VAR - - - -void vars_init() { - vars_restore(); - - // Initialize var state -#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ - IF(INDEX)(for (int i = 0; i < INDEX; i++)) \ - (NAME##_state)IF(INDEX)([i]) = get_##NAME(IF(INDEX)(i)); - -#include "vars.def" -#undef VAR -} - - -void vars_report(bool full) { - // Save and disable watchdog - uint8_t wd_state = hw_disable_watchdog(); - - bool reported = false; - -#define VAR(NAME, CODE, TYPE, INDEX, ...) \ - IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \ - TYPE value = get_##NAME(IF(INDEX)(i)); \ - TYPE last = (NAME##_state)IF(INDEX)([i]); \ - \ - if (value != last || full) { \ - (NAME##_state)IF(INDEX)([i]) = value; \ - \ - if (!reported) { \ - reported = true; \ - putchar('{'); \ - } else putchar(','); \ - \ - printf_P \ - (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt), \ - IF(INDEX)(INDEX##_LABEL[i],) #CODE); \ - \ - var_print_##TYPE(value); \ - } \ - } - -#include "vars.def" -#undef VAR - - if (reported) printf("}\n"); - - // Restore watchdog - hw_restore_watchdog(wd_state); -} - - -int vars_find(const char *name) { - uint8_t i = 0; - uint8_t n = 0; - unsigned len = strlen(name); - - if (!len) return -1; - -#define VAR(NAME, CODE, TYPE, INDEX, ...) \ - if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \ - IF(INDEX) \ - (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ - if (INDEX <= i) return -1); \ - return n; \ - } \ - n++; - -#include "vars.def" -#undef VAR - - return -1; -} - - -bool vars_print(const char *name) { - uint8_t i; - unsigned len = strlen(name); - - if (!len) return false; - -#define VAR(NAME, CODE, TYPE, INDEX, ...) \ - if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \ - IF(INDEX) \ - (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ - if (INDEX <= i) return false); \ - \ - putchar('{'); \ - printf_P \ - (IF_ELSE(INDEX)(indexed_code_fmt, code_fmt), \ - IF(INDEX)(INDEX##_LABEL[i],) #CODE); \ - var_print_##TYPE(get_##NAME(IF(INDEX)(i))); \ - putchar('}'); \ - \ - return true; \ - } - -#include "vars.def" -#undef VAR - - return false; -} - - -bool vars_set(const char *name, const char *value) { - uint8_t i; - unsigned len = strlen(name); - - if (!len) return false; - -#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ - IF(SET) \ - (if (!strcmp(IF_ELSE(INDEX)(name + 1, name), #CODE)) { \ - IF(INDEX) \ - (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ - if (INDEX <= i) return false); \ - \ - TYPE x = var_parse_##TYPE(value); \ - set_##NAME(IF(INDEX)(i,) x); \ - \ - return true; \ - }) \ - -#include "vars.def" -#undef VAR - - return false; -} - - -int vars_parser(char *vars) { - if (!*vars || !*vars++ == '{') return STAT_OK; - - while (*vars) { - while (isspace(*vars)) vars++; - if (*vars == '}') return STAT_OK; - if (*vars != '"') return STAT_COMMAND_NOT_ACCEPTED; - - // Parse name - vars++; // Skip " - const char *name = vars; - while (*vars && *vars != '"') vars++; - *vars++ = 0; - - while (isspace(*vars)) vars++; - if (*vars != ':') return STAT_COMMAND_NOT_ACCEPTED; - vars++; - while (isspace(*vars)) vars++; - - // Parse value - const char *value = vars; - while (*vars && *vars != ',' && *vars != '}') vars++; - if (*vars) { - char c = *vars; - *vars++ = 0; - vars_set(name, value); - if (c == '}') break; - } - } - - return STAT_OK; -} - - -void vars_print_help() { - static const char fmt[] PROGMEM = " $%-5s %-20S %-16S %S\n"; - - // Save and disable watchdog - uint8_t wd_state = hw_disable_watchdog(); - -#define VAR(NAME, CODE, TYPE, ...) \ - printf_P(fmt, #CODE, NAME##_name, TYPE##_name, NAME##_help); -#include "vars.def" -#undef VAR - - // Restore watchdog - hw_restore_watchdog(wd_state); -} - - -uint16_t vars_crc() { - // Save and disable watchdog - uint8_t wd_state = hw_disable_watchdog(); - - CRC.CTRL = CRC_RESET_RESET0_gc; - CRC.CTRL = CRC_SOURCE_IO_gc; // Must be after reset - -#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ - IF(SAVE) \ - ({ \ - for (int j = 0; ; j++) { \ - char c = pgm_read_byte(&NAME##_name[j]); \ - if (!c) break; \ - CRC.DATAIN = c; \ - } \ - \ - CRC.DATAIN = INDEX; \ - }) - -#include "vars.def" -#undef VAR - - // Restore watchdog - hw_restore_watchdog(wd_state); - - CRC.STATUS = CRC_BUSY_bm; // Done - return CRC.CHECKSUM1 << 8 | CRC.CHECKSUM0; -} - - -void vars_save() { - // Save and disable watchdog - uint8_t wd_state = hw_disable_watchdog(); - -#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ - IF(SAVE) \ - (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \ - TYPE value = get_##NAME(IF(INDEX)(i)); \ - eeprom_update_##TYPE(&NAME##_eeprom IF(INDEX)([i]), value); \ - }) \ - -#include "vars.def" -#undef VAR - - // Restore watchdog - hw_restore_watchdog(wd_state); - - // Save CRC - eeprom_update_word(&eeprom_crc, vars_crc()); -} - - - -bool vars_valid() { - return eeprom_read_word(&eeprom_crc) == vars_crc(); -} - - -stat_t vars_restore() { - if (!vars_valid()) return STAT_EEPROM_DATA_INVALID; - - // Save and disable watchdog - uint8_t wd_state = hw_disable_watchdog(); - -#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ - IF(SAVE) \ - (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \ - TYPE value = eeprom_read_##TYPE(&NAME##_eeprom IF(INDEX)([i])); \ - set_##NAME(IF(INDEX)(i,) value); \ - NAME##_state IF(INDEX)([i]) = value; \ - }) \ - -#include "vars.def" -#undef VAR - - // Restore watchdog - hw_restore_watchdog(wd_state); - - return STAT_OK; -} - - -void vars_clear() { - eeprom_update_word(&eeprom_crc, -1); -} diff --git a/src/vars.def b/src/vars.def deleted file mode 100644 index c7f9dee..0000000 --- a/src/vars.def +++ /dev/null @@ -1,122 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 SWITCHES_LABEL "0123456789" - -// VAR(name, code, type, index, settable, save, help) - -// Motor -VAR(axis_name, an, char, MOTORS, 1, 1, "Maps motor to axis name") -VAR(step_angle, sa, float, MOTORS, 1, 1, "In degrees per full step") -VAR(travel, tr, float, MOTORS, 1, 1, "Travel in mm per revolution") -VAR(microstep, mi, uint16_t, MOTORS, 1, 1, "Microsteps per full step") -VAR(reverse, rv, uint8_t, MOTORS, 1, 1, "Reverse motor polarity") - -VAR(power_mode, pm, uint8_t, MOTORS, 1, 1, "Motor power mode") -VAR(drive_power, dp, float, MOTORS, 1, 1, "Motor drive power") -VAR(idle_power, ip, float, MOTORS, 1, 1, "Motor idle power") -VAR(current_power, cp, float, MOTORS, 0, 0, "Motor power now") -VAR(status_flags, mf, uint8_t, MOTORS, 0, 0, "Motor status flags") -VAR(status_strings, ms, flags_t, MOTORS, 0, 0, "Motor status strings") - -VAR(motor_fault, mf, bool, 0, 0, 0, "Motor fault status") - -VAR(velocity_max, vm, float, MOTORS, 1, 1, "Maxium velocity in mm/min") -VAR(feedrate_max, fr, float, MOTORS, 1, 1, "Maxium feedrate in mm/min") -VAR(jerk_max, jm, float, MOTORS, 1, 1, "Maxium jerk in mm/min^3") -VAR(junction_dev, jd, float, MOTORS, 1, 1, "Junction deviation") -VAR(radius, ra, float, MOTORS, 1, 1, "Axis radius or zero") - -// Homing -VAR(homing_mode, hm, uint8_t, MOTORS, 1, 1, "Homing type") -VAR(travel_min, tn, float, MOTORS, 1, 1, "Minimum soft limit") -VAR(travel_max, tm, float, MOTORS, 1, 1, "Maximum soft limit") -VAR(search_velocity,sv, float, MOTORS, 1, 1, "Homing search velocity") -VAR(latch_velocity, lv, float, MOTORS, 1, 1, "Homing latch velocity") -VAR(latch_backoff, lb, float, MOTORS, 1, 1, "Homing latch backof") -VAR(zero_backoff, zb, float, MOTORS, 1, 1, "Homing zero backof") - -// Axis -VAR(position, p, float, AXES, 0, 0, "Current axis position") - -// Spindle -VAR(spindle_type, st, uint8_t, 0, 1, 1, "PWM=0 or HUANYANG=1") -VAR(spin_reverse, sr, bool, 0, 1, 1, "Reverse spin") -VAR(max_spin, sx, float, 0, 1, 1, "Maximum spindle speed") -VAR(min_spin, sm, float, 0, 1, 1, "Minimum spindle speed") -VAR(spin_min_pulse, np, float, 0, 1, 1, "Minimum pulse width") -VAR(spin_max_pulse, mp, float, 0, 1, 1, "Maximum pulse width") -VAR(spin_up, su, float, 0, 1, 1, "Spin up velocity") -VAR(spin_down, sd, float, 0, 1, 1, "Spin down velocity") - -// Huanyang spindle -VAR(huanyang_id, hi, uint8_t, 0, 1, 1, "Huanyang ID") -VAR(huanyang_freq, hz, float, 0, 0, 0, "Huanyang actual freq") -VAR(huanyang_current, hc, float, 0, 0, 0, "Huanyang actual current") -VAR(huanyang_rpm, hr, uint16_t, 0, 0, 0, "Huanyang actual RPM") -//VAR(huanyang_dcv, hd, uint16_t, 0, 0, 0, "Huanyang DC voltage") -//VAR(huanyang_acv, ha, uint16_t, 0, 0, 0, "Huanyang AC voltage") -VAR(huanyang_temp, ht, uint16_t, 0, 0, 0, "Huanyang temperature") -VAR(huanyang_max_freq, hx, float, 0, 0, 0, "Huanyang max freq") -VAR(huanyang_min_freq, hm, float, 0, 0, 0, "Huanyang min freq") -VAR(huanyang_rated_rpm, hq, uint16_t, 0, 0, 0, "Huanyang rated RPM") -VAR(huanyang_status, hs, uint8_t, 0, 0, 0, "Huanyang status flags") -VAR(huanyang_debug, hb, bool, 0, 1, 0, "Huanyang debugging") -VAR(huanyang_connected, he, bool, 0, 0, 0, "Huanyang connected") - -// Switches -VAR(switch_type, sw, uint8_t, SWITCHES, 1, 1, "Normally open or closed") - -// GCode -VAR(line, ln, int32_t, 0, 0, 0, "Last GCode line executed") -VAR(unit, u, pstring, 0, 0, 0, "Current unit of measure") -VAR(speed, s, float, 0, 0, 0, "Current spindle speed") -VAR(feed, f, float, 0, 0, 0, "Current feed rate") -VAR(tool, t, uint8_t, 0, 0, 0, "Current tool") -VAR(feed_mode, fm, pstring, 0, 0, 0, "Current feed rate mode") -VAR(plane, pa, pstring, 0, 0, 0, "Current plane") -VAR(coord_system, cs, pstring, 0, 0, 0, "Current coordinate system") -VAR(abs_override, ao, bool, 0, 0, 0, "Absolute override enabled") -VAR(path_mode, pc, pstring, 0, 0, 0, "Current path control mode") -VAR(distance_mode, dm, pstring, 0, 0, 0, "Current distance mode") -VAR(arc_dist_mode, ad, pstring, 0, 0, 0, "Current arc distance mode") -VAR(mist_coolant, mc, bool, 0, 0, 0, "Mist coolant enabled") -VAR(flood_coolant, fc, bool, 0, 0, 0, "Flood coolant enabled") -VAR(feed_override, fo, float, 0, 0, 0, "Feed rate override") -VAR(speed_override, so, float, 0, 0, 0, "Spindle speed override") - -// System -VAR(velocity, v, float, 0, 0, 0, "Current velocity") -VAR(hw_id, id, string, 0, 0, 0, "Hardware ID") -VAR(echo, ec, bool, 0, 1, 0, "Enable or disable echo") -VAR(estop, es, bool, 0, 1, 0, "Emergency stop") -VAR(estop_reason, er, pstring, 0, 0, 0, "Emergency stop reason") -VAR(state, x, pstring, 0, 0, 0, "Machine state") -VAR(cycle, c, pstring, 0, 0, 0, "Machine cycle") -VAR(hold_reason, pr, pstring, 0, 0, 0, "Machine pause reason") diff --git a/src/vars.h b/src/vars.h deleted file mode 100644 index e918e91..0000000 --- a/src/vars.h +++ /dev/null @@ -1,47 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2016 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public 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 vars_init(); - -void vars_report(bool full); -int vars_find(const char *name); -bool vars_print(const char *name); -bool vars_set(const char *name, const char *value); -int vars_parser(char *vars); -void vars_print_help(); - -void vars_save(); -bool vars_valid(); -stat_t vars_restore(); -void vars_clear(); diff --git a/src/xboot/eeprom_driver.c b/src/xboot/eeprom_driver.c deleted file mode 100644 index 38b6eea..0000000 --- a/src/xboot/eeprom_driver.c +++ /dev/null @@ -1,58 +0,0 @@ -/************************************************************************/ -/* XMEGA EEPROM Driver */ -/* */ -/* eeprom.c */ -/* */ -/* Alex Forencich */ -/* */ -/* Copyright (c) 2011 Alex Forencich */ -/* */ -/* Permission is hereby granted, free of charge, to any person */ -/* obtaining a copy of this software and associated documentation */ -/* files(the "Software"), to deal in the Software without restriction, */ -/* including without limitation the rights to use, copy, modify, merge, */ -/* publish, distribute, sublicense, and/or sell copies of the Software, */ -/* and to permit persons to whom the Software is furnished to do so, */ -/* subject to the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ -/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ -/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ -/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ -/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ -/* SOFTWARE. */ -/* */ -/************************************************************************/ - -#include "eeprom_driver.h" - - -static inline void NVM_EXEC(void) { - void *z = (void *)&NVM_CTRLA; - - __asm__ volatile("out %[ccp], %[ioreg]" "\n\t" - "st z, %[cmdex]" - : - : [ccp] "I" (_SFR_IO_ADDR(CCP)), - [ioreg] "d" (CCP_IOREG_gc), - [cmdex] "r" (NVM_CMDEX_bm), - [z] "z" (z) - ); -} - - -void wait_for_nvm() { - while (NVM.STATUS & NVM_NVMBUSY_bm) continue; -} - - -void EEPROM_erase_all() { - wait_for_nvm(); - NVM.CMD = NVM_CMD_ERASE_EEPROM_gc; - NVM_EXEC(); -} diff --git a/src/xboot/eeprom_driver.h b/src/xboot/eeprom_driver.h deleted file mode 100644 index c139a43..0000000 --- a/src/xboot/eeprom_driver.h +++ /dev/null @@ -1,56 +0,0 @@ -/************************************************************************/ -/* XMEGA EEPROM Driver */ -/* */ -/* eeprom.h */ -/* */ -/* Alex Forencich */ -/* */ -/* Copyright (c) 2011 Alex Forencich */ -/* */ -/* Permission is hereby granted, free of charge, to any person */ -/* obtaining a copy of this software and associated documentation */ -/* files(the "Software"), to deal in the Software without restriction, */ -/* including without limitation the rights to use, copy, modify, merge, */ -/* publish, distribute, sublicense, and/or sell copies of the Software, */ -/* and to permit persons to whom the Software is furnished to do so, */ -/* subject to the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ -/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ -/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ -/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ -/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ -/* SOFTWARE. */ -/* */ -/************************************************************************/ - -#pragma once - -#include -#include -#include - -#include "xboot.h" - -#ifndef EEPROM_PAGE_SIZE -#define EEPROM_PAGE_SIZE E2PAGESIZE -#endif - -#define EEPROM_read_byte(addr) \ - eeprom_read_byte((const uint8_t *)((uint16_t)(addr))) - -#define EEPROM_write_byte(addr, value) \ - eeprom_write_byte((uint8_t *)((uint16_t)(addr)), (value)) - -#define EEPROM_read_block(addr, dest, len) \ - eeprom_read_block((dest), (void *)((uint16_t)(addr)), (len)) - -#define EEPROM_write_block(addr, src, len) \ - eeprom_write_block((src), (void *)((uint16_t)(addr)), (len)) - -void EEPROM_erase_all(); diff --git a/src/xboot/protocol.h b/src/xboot/protocol.h deleted file mode 100644 index 4336094..0000000 --- a/src/xboot/protocol.h +++ /dev/null @@ -1,102 +0,0 @@ -/************************************************************************/ -/* XBoot Extensible AVR Bootloader */ -/* */ -/* XBoot Protocol Definition */ -/* */ -/* protocol.h */ -/* */ -/* Alex Forencich */ -/* */ -/* Copyright (c) 2010 Alex Forencich */ -/* */ -/* Permission is hereby granted, free of charge, to any person */ -/* obtaining a copy of this software and associated documentation */ -/* files(the "Software"), to deal in the Software without restriction, */ -/* including without limitation the rights to use, copy, modify, merge, */ -/* publish, distribute, sublicense, and/or sell copies of the Software, */ -/* and to permit persons to whom the Software is furnished to do so, */ -/* subject to the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ -/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ -/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ -/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ -/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ -/* SOFTWARE. */ -/* */ -/************************************************************************/ - -#pragma once - -#include "xboot.h" - -// General Commands -#define CMD_SYNC '\x1b' - -// Informational Commands -#define CMD_CHECK_AUTOINCREMENT 'a' -#define CMD_CHECK_BLOCK_SUPPORT 'b' -#define CMD_PROGRAMMER_TYPE 'p' -#define CMD_DEVICE_CODE 't' -#define CMD_PROGRAM_ID 'S' -#define CMD_VERSION 'V' -#define CMD_READ_SIGNATURE 's' - -// Addressing -#define CMD_SET_ADDRESS 'A' -#define CMD_SET_EXT_ADDRESS 'H' - -// Erase -#define CMD_CHIP_ERASE 'e' - -// Block Access -#define CMD_BLOCK_LOAD 'B' -#define CMD_BLOCK_READ 'g' - -// Byte Access -#define CMD_READ_BYTE 'R' -#define CMD_WRITE_LOW_BYTE 'c' -#define CMD_WRITE_HIGH_BYTE 'C' -#define CMD_WRITE_PAGE 'm' -#define CMD_WRITE_EEPROM_BYTE 'D' -#define CMD_READ_EEPROM_BYTE 'd' - -// Lock and Fuse Bits -#define CMD_WRITE_LOCK_BITS 'l' -#define CMD_READ_LOCK_BITS 'r' -#define CMD_READ_LOW_FUSE_BITS 'F' -#define CMD_READ_HIGH_FUSE_BITS 'N' -#define CMD_READ_EXT_FUSE_BITS 'Q' - -// Bootloader Commands -#define CMD_ENTER_PROG_MODE 'P' -#define CMD_LEAVE_PROG_MODE 'L' -#define CMD_EXIT_BOOTLOADER 'E' -#define CMD_SET_LED 'x' -#define CMD_CLEAR_LED 'y' -#define CMD_SET_TYPE 'T' - -#define CMD_CRC 'h' - -// Memory types for block access -#define MEM_EEPROM 'E' -#define MEM_FLASH 'F' -#define MEM_USERSIG 'U' -#define MEM_PRODSIG 'P' - -// Sections for CRC checks -#define SECTION_FLASH 'F' -#define SECTION_APPLICATION 'A' -#define SECTION_BOOT 'B' -#define SECTION_APP 'a' -#define SECTION_APP_TEMP 't' - -// Command Responses -#define REPLY_ACK '\r' -#define REPLY_YES 'Y' -#define REPLY_ERROR '?' diff --git a/src/xboot/sp_driver.S b/src/xboot/sp_driver.S deleted file mode 100644 index e6fff99..0000000 --- a/src/xboot/sp_driver.S +++ /dev/null @@ -1,794 +0,0 @@ -;****************************************************************************** -;* -;* XMEGA Self-programming driver assembly source file. -;* -;* This file contains the low-level implementations for the -;* XMEGA Self-programming driver. It is written for the GCC Assembler. -;* -;* If any SPM instructions are used, the linker file must define -;* a segment named bootloader which must be located in the device Boot section. -;* This can be done by passing "-Wl,--section-start=.BOOT=0x020000" to the -;* linker with the correct address for the boot section. -;* -;* None of these routines clean up the NVM Command Register after use. -;* It is therefore important to write NVM_CMD_NO_OPERATION_gc (0x00) to this -;* register when you are finished using any of the functions in this driver. -;* -;* For all routines, it is important that any interrupt handlers do not -;* perform any NVM operations. The user must implement a scheme for mutually -;* exclusive access to the NVM. However, the 4-cycle timeout will work fine, -;* since writing to the Configuration Change Protection register (CCP) -;* automatically disables interrupts for 4 instruction cycles. -;* -;* Note on IAR calling convention: -;* Scratch registers: R18-R27, R30-R31 -;* Preserved registers: R2-R17, R28-R29 -;* Parameter registers: R8-R25 (2-,4-, or 8- byte alignment) -;* Return registers: R18-R25 (up to 64-bit) -;* -;* Application note: -;* AVR1316: XMEGA Self-programming -;* -;* Documentation -;* For comprehensive code documentation, supported compilers, compiler -;* settings and supported devices see readme.html -;* -;* Atmel Corporation: http:;www.atmel.com \n -;* Support email: avr@atmel.com -;* -;* $Revision: 1153 $ -;* $Date: 2007-12-18 09:48:23 +0100 (ti, 18 des 2007) $ -;* -;* Copyright (c) 2007, Atmel Corporation All rights reserved. -;* -;* Redistribution and use in source and binary forms, with or without -;* modification, are permitted provided that the following conditions are met: -;* -;* 1. Redistributions of source code must retain the above copyright notice, -;* this list of conditions and the following disclaimer. -;* -;* 2. Redistributions in binary form must reproduce the above copyright notice, -;* this list of conditions and the following disclaimer in the documentation -;* and/or other materials provided with the distribution. -;* -;* 3. The name of ATMEL may not be used to endorse or promote products derived -;* from this software without specific prior written permission. -;* -;* THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED -;* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -;* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND -;* SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, -;* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -;* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -;* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -;* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -;* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF -;* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -;****************************************************************************** - -#include -;#include "Flash_Defines.h" - -/* Define the size of the flash page if not defined in the header files. */ -#ifndef APP_SECTION_PAGE_SIZE - #error APP_SECTION_PAGE_SIZE must be defined if not defined in header files. - //#define APP_SECTION_PAGE_SIZE 512 -#endif /*APP_SECTION_PAGE_SIZE*/ - -/* Defines not yet included in header file. */ -#define NVM_CMD_NO_OPERATION_gc (0x00<<0) // Noop/Ordinary LPM -#define NVM_CMD_READ_USER_SIG_ROW_gc (0x01<<0) // Read user signature row -#define NVM_CMD_READ_CALIB_ROW_gc (0x02<<0) // Read calibration row -#define NVM_CMD_READ_EEPROM_gc (0x06<<0) // Read EEPROM -#define NVM_CMD_READ_FUSES_gc (0x07<<0) // Read fuse byte -#define NVM_CMD_WRITE_LOCK_BITS_gc (0x08<<0) // Write lock bits -#define NVM_CMD_ERASE_USER_SIG_ROW_gc (0x18<<0) // Erase user signature row -#define NVM_CMD_WRITE_USER_SIG_ROW_gc (0x1A<<0) // Write user signature row -#define NVM_CMD_ERASE_APP_gc (0x20<<0) // Erase Application Section -#define NVM_CMD_ERASE_APP_PAGE_gc (0x22<<0) // Erase Application Section page -#define NVM_CMD_LOAD_FLASH_BUFFER_gc (0x23<<0) // Load Flash page buffer -#define NVM_CMD_WRITE_APP_PAGE_gc (0x24<<0) // Write Application Section page -#define NVM_CMD_ERASE_WRITE_APP_PAGE_gc (0x25<<0) // Erase-and-write Application Section page -#define NVM_CMD_ERASE_FLASH_BUFFER_gc (0x26<<0) // Erase/flush Flash page buffer -#define NVM_CMD_ERASE_BOOT_PAGE_gc (0x2A<<0) // Erase Boot Section page -#define NVM_CMD_WRITE_BOOT_PAGE_gc (0x2C<<0) // Write Boot Section page -#define NVM_CMD_ERASE_WRITE_BOOT_PAGE_gc (0x2D<<0) // Erase-and-write Boot Section page -#define NVM_CMD_ERASE_EEPROM_gc (0x30<<0) // Erase EEPROM -#define NVM_CMD_ERASE_EEPROM_PAGE_gc (0x32<<0) // Erase EEPROM page -#define NVM_CMD_LOAD_EEPROM_BUFFER_gc (0x33<<0) // Load EEPROM page buffer -#define NVM_CMD_WRITE_EEPROM_PAGE_gc (0x34<<0) // Write EEPROM page -#define NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc (0x35<<0) // Erase-and-write EEPROM page -#define NVM_CMD_ERASE_EEPROM_BUFFER_gc (0x36<<0) // Erase/flush EEPROM page buffer -#define NVM_CMD_APP_CRC_gc (0x38<<0) // Generate Application section CRC -#define NVM_CMD_BOOT_CRC_gc (0x39<<0) // Generate Boot Section CRC -#define NVM_CMD_FLASH_RANGE_CRC_gc (0x3A<<0) // Generate Flash Range CRC -#define CCP_SPM_gc (0x9D<<0) // SPM Instruction Protection -#define CCP_IOREG_gc (0xD8<<0) // IO Register Protection - - - -; --- -; This routine reads a byte from flash given by the address in -; R25:R24:R23:R22. -; -; Input: -; R25:R24:R23:R22. -; -; Returns: -; R24 - Read byte. -; --- - -.section .text -.global SP_ReadByte - -SP_ReadByte: - in r19, RAMPZ ; Save RAMPZ. - out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. - movw ZL, r22 ; Move the low bytes to the Z pointer - elpm r24, Z ; Extended load byte from address pointed to by Z. - out RAMPZ, r19 ; Restore RAMPZ register. - ret - - - -; --- -; This routine reads a word from flash given by the address in -; R25:R24:R23:R22. -; -; Input: -; R25:R24:R23:R22. -; -; Returns: -; R25:R24 - Read word. -; --- - -.section .text -.global SP_ReadWord - -SP_ReadWord: - in r19, RAMPZ ; Save RAMPZ. - out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. - movw ZL, r22 ; Move the low bytes to the Z pointer - elpm r24, Z+ ; Extended load byte from address pointed to by Z. - elpm r25, Z ; Extended load byte from address pointed to by Z. - out RAMPZ, r19 ; Restore RAMPZ register. - ret - - - -; --- -; This routine reads the calibration byte given by the index in R24. -; -; Input: -; R24 - Byte index. -; -; Returns: -; R24 - Calibration byte. -; --- - -.section .text -.global SP_ReadCalibrationByte - -SP_ReadCalibrationByte: - ldi r20, NVM_CMD_READ_CALIB_ROW_gc ; Prepare NVM command in R20. - rjmp SP_CommonLPM ; Jump to common LPM code. - - - -; --- -; This routine reads the user signature byte given by the index in R25:R24. -; -; Input: -; R25:R24 - Byte index. -; -; Returns: -; R24 - Signature byte. -; --- - -.section .text -.global SP_ReadUserSignatureByte - -SP_ReadUserSignatureByte: - ldi r20, NVM_CMD_READ_USER_SIG_ROW_gc ; Prepare NVM command in R20. - rjmp SP_CommonLPM ; Jump to common LPM code. - - - -; --- -; This routine reads the fuse byte given by the index in R24. -; -; Input: -; R24 - Byte index. -; -; Returns: -; R24 - Fuse byte. -; --- - -.section .text -.global SP_ReadFuseByte - -SP_ReadFuseByte: - sts NVM_ADDR0, r24 ; Load fuse byte index into NVM Address Register 0. - clr r24 ; Prepare a zero. - sts NVM_ADDR1, r24 ; Load zero into NVM Address Register 1. - sts NVM_ADDR2, r24 ; Load zero into NVM Address Register 2. - ldi r20, NVM_CMD_READ_FUSES_gc ; Prepare NVM command in R20. - rcall SP_CommonCMD ; Jump to common NVM Action code. - movw r24, r22 ; Move low byte to 1 byte return address. - ret - - - -; --- -; This routine sets the lock bits from R24. Note that unlocking is only -; possible by doing a full chip erase, not available from software. -; -; Input: -; R24 - Lock bits. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_WriteLockBits - -SP_WriteLockBits: - sts NVM_DATA0, r24 ; Load lock bits into NVM Data Register 0. - ldi r20, NVM_CMD_WRITE_LOCK_BITS_gc ; Prepare NVM command in R20. - rjmp SP_CommonCMD ; Jump to common NVM Action code. - - - -; --- -; This routine reads the lock bits. -; -; Input: -; Nothing. -; -; Returns: -; R24 - Lock bits. -; --- - -.section .text -.global SP_ReadLockBits - -SP_ReadLockBits: - lds r24, NVM_LOCKBITS ; Read IO-mapped lock bits. - ret - - - -; --- -; This routine erases the user signature row. -; -; Input: -; Nothing. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_EraseUserSignatureRow - -SP_EraseUserSignatureRow: - in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. - ldi r20, NVM_CMD_ERASE_USER_SIG_ROW_gc ; Prepare NVM command in R20. - jmp SP_CommonSPM ; Jump to common SPM code. - - - -; --- -; This routine writes the flash buffer to the user signature row. -; -; Input: -; Nothing. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_WriteUserSignatureRow - -SP_WriteUserSignatureRow: - in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. - ldi r20, NVM_CMD_WRITE_USER_SIG_ROW_gc ; Prepare NVM command in R20. - jmp SP_CommonSPM ; Jump to common SPM code. - - - -; --- -; This routine erases the entire application section. -; -; Input: -; Nothing. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_EraseApplicationSection - -SP_EraseApplicationSection: - in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. - clr r24 ; Prepare a zero. - clr r25 - out RAMPZ, r24 ; Point into Application area. - ldi r20, NVM_CMD_ERASE_APP_gc ; Prepare NVM command in R20. - jmp SP_CommonSPM ; Jump to common SPM code. - - - -; --- -; This routine erases the page at address R25:R24:R23:R22 in the application -; section. The address can point anywhere inside the page. -; -; Input: -; R25:R24:R23:R22 - Byte address into Flash page. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_EraseApplicationPage - -SP_EraseApplicationPage: - in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. - out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. - movw r24, r22 ; Move low bytes for ZH:ZL to R25:R24 - ldi r20, NVM_CMD_ERASE_APP_PAGE_gc ; Prepare NVM command in R20. - jmp SP_CommonSPM ; Jump to common SPM code. - - - -; --- -; This routine writes the word from R23:R22 into the Flash page buffer at -; address R25:R24. -; -; Input: -; R25:R24 - Byte address into Flash page. -; R23:R22 - Word to write. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_LoadFlashWord - -SP_LoadFlashWord: - in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. - movw r0, r22 ; Prepare flash word in R1:R0. - ldi r20, NVM_CMD_LOAD_FLASH_BUFFER_gc ; Prepare NVM command in R20. - jmp SP_CommonSPM ; Jump to common SPM code. - - - -; --- -; This routine writes an entire page from the SRAM buffer at -; address R25:R24 into the Flash page buffer. -; -; Note that you must define "-Wl,--section-start=.BOOT=0x020000" for the -; linker to place this function in the boot section with the correct address. -; -; Input: -; R25:R24 - 16-bit pointer to SRAM buffer. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_LoadFlashPage - -SP_LoadFlashPage: - clr ZL ; Clear low byte of Z, to indicate start of page. - clr ZH ; Clear high byte of Z, to indicate start of page. - - out RAMPX, r1 ; Clear RAMPX pointer. - movw XL, r24 ; Load X with data buffer address. - - ldi r20, NVM_CMD_LOAD_FLASH_BUFFER_gc ; Prepare NVM command code in R20. - sts NVM_CMD, r20 ; Load it into NVM command register. - -#if APP_SECTION_PAGE_SIZE > 512 - ldi r22, ((APP_SECTION_PAGE_SIZE/2) >> 8) -#endif - - ldi r21, ((APP_SECTION_PAGE_SIZE/2)&0xFF) ; Load R21 with page word count. - ldi r18, CCP_SPM_gc ; Prepare Protect SPM signature in R16. - -SP_LoadFlashPage_1: - ld r0, X+ ; Load low byte from buffer into R0. - ld r1, X+ ; Load high byte from buffer into R1. - sts CCP, r18 ; Enable SPM operation (this disables interrupts for 4 cycles). - spm ; Self-program. - adiw ZL, 2 ; Move Z to next Flash word. - -#if APP_SECTION_PAGE_SIZE > 512 - subi r21, 1 ; Decrement word count. - sbci r22, 0 -#else - dec r21 ; Decrement word count. -#endif - - brne SP_LoadFlashPage_1 ; Repeat until word cont is zero. - - clr r1 ; Clear R1 for GCC _zero_reg_ to function properly. - ret - - - -; --- -; This routine reads an entire Flash page from address R23:R22:R21:R20 into the -; SRAM buffer at address R25:R24. -; -; -; Input: -; R23:R22:R21:R20 - Flash byte address. -; R25:R24 - 16-bit pointer to SRAM buffer. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_ReadFlashPage - -SP_ReadFlashPage: - - in r19, RAMPZ ; Save RAMPZ during assembly. - out RAMPZ, r22 ; Load RAMPZ with MSB of address - movw ZL, r20 ; Load Z with Flash address. - - out RAMPX, r1 ; Load RAMPX with data pointer - movw XL, r24 ; Load X with data buffer address. - - ldi r20, NVM_CMD_NO_OPERATION_gc ; Prepare NVM command code in R20. - sts NVM_CMD, r20 ; Set NVM command to No Operation so that LPM reads Flash. - -#if APP_SECTION_PAGE_SIZE > 512 - ldi r22, ((APP_SECTION_PAGE_SIZE/2) >> 8) ; Load R22 with byte cont high if flash page is large. -#endif - - ldi r21, ((APP_SECTION_PAGE_SIZE)&0xFF) ; Load R21 with byte count. - -SP_ReadFlashPage_1: - elpm r24, Z+ ; Load Flash bytes into R18:r19 - elpm r25, Z+ - st X+, r24 ; Write bytes to buffer. - st X+, r25 - -#if APP_SECTION_PAGE_SIZE > 512 - subi r21, 1 ; Decrement word count. - sbci r22, 0 -#else - dec r21 ; Decrement word count. -#endif - - brne SP_ReadFlashPage_1 ; Repeat until byte count is zero. - - out RAMPZ, r19 - ret - - - -; --- -; This routine writes the page buffer to the Flash page at address R25:R24:R23:R22 -; in the application section. The address can point anywhere inside the page. -; -; Input: -; R25:R24:R23:R22 - Byte address into Flash page. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_WriteApplicationPage - -SP_WriteApplicationPage: - in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. - out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. - movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22 - ldi r20, NVM_CMD_WRITE_APP_PAGE_gc ; Prepare NVM command in R20. - jmp SP_CommonSPM ; Jump to common SPM code. - - - -; --- -; This routine erases first and then writes the page buffer to the -; Flash page at address R25:R24:R23:R22 in the application section. The address -; can point anywhere inside the page. -; -; Input: -; R25:R24:R23:R22 - Byte address into Flash page. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_EraseWriteApplicationPage - -SP_EraseWriteApplicationPage: - in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. - out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. - movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22 - ldi r20, NVM_CMD_ERASE_WRITE_APP_PAGE_gc ; Prepare NVM command in R20. - jmp SP_CommonSPM ; Jump to common SPM code. - - - -; --- -; This routine flushes the Flash page buffer. -; -; Input: -; Nothing. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_EraseFlashBuffer - -SP_EraseFlashBuffer: - in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. - ldi r20, NVM_CMD_ERASE_FLASH_BUFFER_gc ; Prepare NVM command in R20. - jmp SP_CommonSPM ; Jump to common SPM code. - - - -; --- -; This routine erases the page at address R25:R24:R23:R22 in the Boot section. The -; address can point anywhere inside the page. -; -; Input: -; R25:R24:R23:R22 - Byte address into Flash page. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_EraseBootPage - -SP_EraseBootPage: - in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. - out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. - movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22 - ldi r20, NVM_CMD_ERASE_BOOT_PAGE_gc ; Prepare NVM command in R20. - jmp SP_CommonSPM ; Jump to common SPM code. - - - -; --- -; This routine writes the page buffer to the Flash page at address R25:R24:R23:R22 -; in the BOOT section. The address can point anywhere inside the page. -; -; Input: -; R25:R24:R23:R22 - Byte address into Flash page. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_WriteBootPage - -SP_WriteBootPage: - in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. - out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. - movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22 - ldi r20, NVM_CMD_WRITE_BOOT_PAGE_gc ; Prepare NVM command in R20. - jmp SP_CommonSPM ; Jump to common SPM code. - - - -; --- -; This routine erases first and then writes the page buffer to the -; Flash page at address R25:R24:R23:R22 in the Boot section. The address -; can point anywhere inside the page. -; -; Input: -; R25:R24:R23:R22 - Byte address into Flash page. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_EraseWriteBootPage - -SP_EraseWriteBootPage: - in r19, RAMPZ ; Save RAMPZ, which is restored in SP_CommonSPM. - out RAMPZ, r24 ; Load RAMPZ with the MSB of the address. - movw r24, r22 ; Move low bytes of address to ZH:ZL from R23:R22 - ldi r20, NVM_CMD_ERASE_WRITE_BOOT_PAGE_gc ; Prepare NVM command in R20. - jmp SP_CommonSPM ; Jump to common SPM code. - - - -; --- -; This routine calculates a CRC for the application section. -; -; Input: -; Nothing. -; -; Returns: -; R25:R24:R23:R22 - 32-bit CRC result (actually only 24-bit used). -; --- - -.section .text -.global SP_ApplicationCRC - -SP_ApplicationCRC: - ldi r20, NVM_CMD_APP_CRC_gc ; Prepare NVM command in R20. - rjmp SP_CommonCMD ; Jump to common NVM Action code. - - - -; --- -; This routine calculates a CRC for the Boot section. -; -; Input: -; Nothing. -; -; Returns: -; R25:R24:R23:R22 - 32-bit CRC result (actually only 24-bit used). -; --- - -.section .text -.global SP_BootCRC - -SP_BootCRC: - ldi r20, NVM_CMD_BOOT_CRC_gc ; Prepare NVM command in R20. - rjmp SP_CommonCMD ; Jump to common NVM Action code. - - - -; --- -; This routine locks all further access to SPM operations until next reset. -; -; Input: -; Nothing. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_LockSPM - -SP_LockSPM: - ldi r18, CCP_IOREG_gc ; Prepare Protect IO-register signature in R18. - sts CCP, r18 ; Enable IO-register operation (this disables interrupts for 4 cycles). - ldi r18, NVM_SPMLOCK_bm ; Prepare bitmask for locking SPM into R18. - sts NVM_CTRLB, r18 ; Load bitmask into NVM Control Register B, which locks SPM. - ret - - - -; --- -; This routine wait for the SPM to finish and clears the command register. -; -; Note that this routine is blocking, and will halt any execution until the SPM -; is finished. -; -; Input: -; Nothing. -; -; Returns: -; Nothing. -; --- - -.section .text -.global SP_WaitForSPM - -SP_WaitForSPM: - lds r18, NVM_STATUS ; Load the NVM Status register. - sbrc r18, NVM_NVMBUSY_bp ; Check if bit is cleared. - rjmp SP_WaitForSPM ; Repeat check if bit is not cleared. - clr r18 - sts NVM_CMD, r18 ; Clear up command register to NO_OPERATION. - ret - - - -; --- -; This routine is called by several other routines, and contains common code -; for executing an NVM command, including the return statement itself. -; -; If the operation (NVM command) requires the NVM Address registers to be -; prepared, this must be done before jumping to this routine. -; -; Note that R25:R24:R23:R22 is used for returning results, even if the -; C-domain calling function only expects a single byte or even void. -; -; Input: -; R20 - NVM Command code. -; -; Returns: -; R25:R24:R23:R22 - 32-bit result from NVM operation. -; --- - -.section .text - -SP_CommonCMD: - sts NVM_CMD, r20 ; Load command into NVM Command register. - ldi r18, CCP_IOREG_gc ; Prepare Protect IO-register signature in R18. - ldi r19, NVM_CMDEX_bm ; Prepare bitmask for setting NVM Command Execute bit into R19. - sts CCP, r18 ; Enable IO-register operation (this disables interrupts for 4 cycles). - sts NVM_CTRLA, r19 ; Load bitmask into NVM Control Register A, which executes the command. - lds r22, NVM_DATA0 ; Load NVM Data Register 0 into R22. - lds r23, NVM_DATA1 ; Load NVM Data Register 1 into R23. - lds r24, NVM_DATA2 ; Load NVM Data Register 2 into R24. - clr r25 ; Clear R25 in order to return a clean 32-bit value. - ret - - - -; --- -; This routine is called by several other routines, and contains common code -; for executing an LPM command, including the return statement itself. -; -; Note that R24 is used for returning results, even if the -; C-domain calling function expects a void. -; -; Input: -; R25:R24 - Low bytes of Z pointer. -; R20 - NVM Command code. -; -; Returns: -; R24 - Result from LPM operation. -; --- - -.section .text - -SP_CommonLPM: - movw ZL, r24 ; Load index into Z. - sts NVM_CMD, r20 ; Load prepared command into NVM Command register. - lpm r24,Z - ret - - - -; --- -; This routine is called by several other routines, and contains common code -; for executing an SPM command, including the return statement itself. -; -; If the operation (SPM command) requires the R1:R0 registers to be -; prepared, this must be done before jumping to this routine. -; -; Note that you must define "-Wl,--section-start=.BOOT=0x020000" for the -; linker to place this function in the boot section with the correct address. -; -; Input: -; R1:R0 - Optional input to SPM command. -; R25:R24 - Low bytes of Z pointer. -; R20 - NVM Command code. -; -; Returns: -; Nothing. -; --- - -.section .text - -SP_CommonSPM: - movw ZL, r24 ; Load R25:R24 into Z. - sts NVM_CMD, r20 ; Load prepared command into NVM Command register. - ldi r18, CCP_SPM_gc ; Prepare Protect SPM signature in R18 - sts CCP, r18 ; Enable SPM operation (this disables interrupts for 4 cycles). - spm ; Self-program. - clr r1 ; Clear R1 for GCC _zero_reg_ to function properly. - out RAMPZ, r19 ; Restore RAMPZ register. - ret - - -; END OF FILE - diff --git a/src/xboot/sp_driver.h b/src/xboot/sp_driver.h deleted file mode 100644 index 0bcbce9..0000000 --- a/src/xboot/sp_driver.h +++ /dev/null @@ -1,298 +0,0 @@ -/* This file has been prepared for Doxygen automatic documentation generation.*/ -/*! \file ********************************************************************* - * - * \brief XMEGA Self-programming driver header file. - * - * This file contains the function prototypes for the - * XMEGA Self-programming driver. - * If any SPM instructions are used, the linker file must define - * a segment named BOOT which must be located in the device boot section. - * - * - * None of these functions clean up the NVM Command Register after use. - * It is therefore important to write NVMCMD_NO_OPERATION (0x00) to this - * register when you are finished using any of the functions in this - * driver. - * - * For all functions, it is important that no interrupt handlers perform - * any NVM operations. The user must implement a scheme for mutually - * exclusive access to the NVM. However, the 4-cycle timeout will work - * fine, since writing to the Configuration Change Protection register - * (CCP) automatically disables interrupts for 4 instruction cycles. - * - * \par Application note: - * AVR1316: XMEGA Self-programming - * - * \par Documentation - * For comprehensive code documentation, supported compilers, compiler - * settings and supported devices see readme.html - * - * \author - * Atmel Corporation: http://www.atmel.com \n - * Support email: avr@atmel.com - * - * $Revision: 1691 $ - * $Date: 2008-07-29 13:25:40 +0200 (ti, 29 jul 2008) $ \n - * - * Copyright (c) 2008, Atmel Corporation All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. The name of ATMEL may not be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED - * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE EXPRESSLY AND - * SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, - * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF - * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *****************************************************************************/ -#ifndef SP_DRIVER_H -#define SP_DRIVER_H - -//#include "avr_compiler.h" -//#include "Flash_Defines.h" - - - -/* Define the size of the flash page if not defined in the header files. */ -#ifndef APP_SECTION_PAGE_SIZE -#error APP_SECTION_PAGE_SIZE must be defined if not defined in header files. -//#define APP_SECTION_PAGE_SIZE 512 -#endif /*FLASH_PAGE_SIZE*/ - -// Define the Start of the application table if not defined in the header files. -#ifndef APPTABLE_SECTION_START -#error APPTABLE_SECTION_START must be defined if not defined in header files. -//#define APPTABLE_SECTION_START 0x01E000 //APPTABLE address for ATxmega128A1 -#endif /*APPTABLE_SECTION_START*/ - -/*! \brief Read a byte from flash. - * - * This function reads one byte from the flash. - * - * \note Both IAR and GCC have functions to do this, but - * we include the fucntions for easier use. - * - * \param address Address to the location of the byte to read. - * - * \retval Byte read from flash. - */ -uint8_t SP_ReadByte(uint32_t address); - -/*! \brief Read a word from flash. - * - * This function reads one word from the flash. - * - * \note Both IAR and GCC have functions to do this automatically, but - * we include the fucntions for easier use. - * - * \param address Address to the location of the word to read. - * - * \retval word read from flash. - */ -uint16_t SP_ReadWord(uint32_t address); - -/*! \brief Read calibration byte at given index. - * - * This function reads one calibration byte from the Calibration signature row. - * - * \param index Index of the byte in the calibration signature row. - * - * \retval Calibration byte - */ -uint8_t SP_ReadCalibrationByte(uint8_t index); - -/*! \brief Read fuse byte from given index. - * - * This function reads the fuse byte at the given index. - * - * \param index Index of the fuse byte. - * - * \retval Fuse byte - */ -uint8_t SP_ReadFuseByte(uint8_t index); - -/*! \brief Write lock bits. - * - * This function changes the lock bits. - * - * \note It is only possible to change the lock bits to a higher security l - * evel. - * - * \param data The new value of the lock bits. - */ -void SP_WriteLockBits(uint8_t data); - -/*! \brief Read lock bits. - * - * This function reads the lock bits. - * - * \retval Lock bits - */ -uint8_t SP_ReadLockBits(void); - -/*! \brief Read user signature at given index. - * - * This function reads one byte from the user signature row. - * - * \param index Index of the byte in the user signature row. - * - * \retval User signature byte - */ -uint8_t SP_ReadUserSignatureByte(uint16_t index); - -/*! \brief Erase user signature row. - * - * This function erase the entire user signature row. - */ -void SP_EraseUserSignatureRow(void); - -/*! \brief Write user signature row. - * - * This function write the flash buffer in the user signature row. - */ -void SP_WriteUserSignatureRow(void); - -/*! \brief Erase entire application section. - * - * This function erases the entire application and application table section - * - * \note If the lock bits is set to not allow spm in the application or - * application table section the erase is not done. - */ -void SP_EraseApplicationSection(void); - -/*! \brief Erase page at byte address in application or application table - * section. - * - * This function erase one page given by the byte address. - * - * \param address Byte address for flash page. - */ -void SP_EraseApplicationPage(uint32_t address); - -/*! \brief Erase and write page buffer to application or application table - * section at byte address. - * - * This function does a combined erase and write to a flash page in the - * application or application table section. - * - * \param address Byte address for flash page. - */ -void SP_EraseWriteApplicationPage(uint32_t address); - -/*! \brief Write page buffer to application or application table section at - * byte address. - * - * This function writes the Flash page buffer to a page in the application or - * application table section given by the byte address. - * - * \note The page that is written to must be erased before it is written to. - * - * \param address Byte address for flash page. - */ -void SP_WriteApplicationPage(uint32_t address); - -/*! \brief Load one word into Flash page buffer. - * - * This function Loads one word into the Flash page buffer. - * - * \param address Position in inside the flash page buffer. - * \param data Value to be put into the buffer. - */ -void SP_LoadFlashWord(uint16_t address, uint16_t data); - -/*! \brief Load entire page from SRAM buffer into Flash page buffer. - * - * This function load an entire page from SRAM. - * - * \param data Pointer to the data to put in buffer. - * - * \note The __near keyword limits the pointer to two bytes which means that - * only data up to 64K (internal SRAM) can be used. - */ -void SP_LoadFlashPage(const uint8_t * data); - -/*! \brief Read entire Flash page into SRAM buffer. - * - * This function reads an entire flash page and puts it to SRAM. - * - * \param data Pointer to where to store the data. - * \param address Address to page to read from flash. - */ -void SP_ReadFlashPage(const uint8_t * data, uint32_t address); - -/*! \brief Flush Flash page buffer. - * - * This function flush the Flash page buffer. - */ -void SP_EraseFlashBuffer(void); - -/*! \brief Erase page at byte address in boot section. - * - * This function erase one page given by the byte address. - * - * \param address Byte address for flash page. - */ -void SP_EraseBootPage(uint32_t address); - -/*! \brief Erase and write page buffer to boot section at byte address. - * - * This function does a combined erase and write to a flash page in the boot - * section. - * - * \param address Byte address for flash page. - */ -void SP_EraseWriteBootPage(uint32_t address); - -/*! \brief Write page buffer to boot section at byte address. - * - * This function writes the Flash page buffer to a page in the boot section - * given by the byte address. - * - * \note The page that is written to must be erased before it is written to. - * - * \param address Byte address for flash page. - */ -void SP_WriteBootPage(uint32_t address); - -/*! \brief Generate CRC from application section. - * - * \retval 24-bit CRC value - */ -uint32_t SP_ApplicationCRC(void); - -/*! \brief Generate CRC from boot section. - * - * \retval 24-bit CRC value - */ -uint32_t SP_BootCRC(void); - -/*! \brief Lock SPM instruction. - * - * This function locks the SPM instruction, and will disable the use of - * SPM until the next reset occurs. - */ -void SP_LockSPM(void); - -/*! \brief Wait for SPM to finish. - * - * This routine waits for the SPM to finish and clears the command register. - */ -void SP_WaitForSPM(void); - -#endif /* SP_DRIVER_H */ diff --git a/src/xboot/uart.c b/src/xboot/uart.c deleted file mode 100644 index c4cf091..0000000 --- a/src/xboot/uart.c +++ /dev/null @@ -1,72 +0,0 @@ -/************************************************************************/ -/* XBoot Extensible AVR Bootloader */ -/* */ -/* UART Module */ -/* */ -/* uart.c */ -/* */ -/* Alex Forencich */ -/* */ -/* Copyright (c) 2010 Alex Forencich */ -/* */ -/* Permission is hereby granted, free of charge, to any person */ -/* obtaining a copy of this software and associated documentation */ -/* files(the "Software"), to deal in the Software without restriction, */ -/* including without limitation the rights to use, copy, modify, merge, */ -/* publish, distribute, sublicense, and/or sell copies of the Software, */ -/* and to permit persons to whom the Software is furnished to do so, */ -/* subject to the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ -/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ -/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ -/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ -/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ -/* SOFTWARE. */ -/* */ -/************************************************************************/ - -#include "uart.h" -#include "xboot.h" - - -bool uart_has_char() {return UART_DEVICE.STATUS & USART_RXCIF_bm;} -uint8_t uart_recv_char() {return UART_DEVICE.DATA;} - - -void uart_send_char_blocking(uint8_t c) { - UART_DEVICE.DATA = c; - while (!(UART_DEVICE.STATUS & USART_TXCIF_bm)) continue; - UART_DEVICE.STATUS |= USART_TXCIF_bm; -} - - -void uart_init() { - UART_PORT.DIRSET = 1 << UART_TX_PIN; - UART_DEVICE.BAUDCTRLA = UART_BSEL_VALUE & USART_BSEL_gm; - UART_DEVICE.BAUDCTRLB = - ((UART_BSCALE_VALUE << USART_BSCALE_gp) & USART_BSCALE_gm) | - ((UART_BSEL_VALUE >> 8) & ~USART_BSCALE_gm); - -#if UART_CLK2X - UART_DEVICE.CTRLB = USART_RXEN_bm | USART_CLK2X_bm | USART_TXEN_bm; -#else - UART_DEVICE.CTRLB = USART_RXEN_bm | USART_TXEN_bm; -#endif // UART_CLK2X - - PORTC.OUTCLR = 1 << 4; // CTS Lo (enable) - PORTC.DIRSET = 1 << 4; // CTS Output -} - - -void uart_deinit() { - UART_DEVICE.CTRLB = 0; - UART_DEVICE.BAUDCTRLA = 0; - UART_DEVICE.BAUDCTRLB = 0; - UART_PORT.DIRCLR = 1 << UART_TX_PIN; -} diff --git a/src/xboot/uart.h b/src/xboot/uart.h deleted file mode 100644 index 0392f7e..0000000 --- a/src/xboot/uart.h +++ /dev/null @@ -1,43 +0,0 @@ -/************************************************************************/ -/* XBoot Extensible AVR Bootloader */ -/* */ -/* UART Module */ -/* */ -/* uart.h */ -/* */ -/* Alex Forencich */ -/* */ -/* Copyright (c) 2010 Alex Forencich */ -/* */ -/* Permission is hereby granted, free of charge, to any person */ -/* obtaining a copy of this software and associated documentation */ -/* files(the "Software"), to deal in the Software without restriction, */ -/* including without limitation the rights to use, copy, modify, merge, */ -/* publish, distribute, sublicense, and/or sell copies of the Software, */ -/* and to permit persons to whom the Software is furnished to do so, */ -/* subject to the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ -/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ -/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ -/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ -/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ -/* SOFTWARE. */ -/* */ -/************************************************************************/ - -#pragma once - -#include -#include - -bool uart_has_char(); -uint8_t uart_recv_char(); -void uart_send_char_blocking(uint8_t c); -void uart_init(void); -void uart_deinit(void); diff --git a/src/xboot/watchdog.c b/src/xboot/watchdog.c deleted file mode 100644 index a7e1064..0000000 --- a/src/xboot/watchdog.c +++ /dev/null @@ -1,51 +0,0 @@ -/************************************************************************/ -/* XBoot Extensible AVR Bootloader */ -/* */ -/* Watchdog Module */ -/* */ -/* watchdog.c */ -/* */ -/* Alex Forencich */ -/* */ -/* Copyright (c) 2010 Alex Forencich */ -/* */ -/* Permission is hereby granted, free of charge, to any person */ -/* obtaining a copy of this software and associated documentation */ -/* files(the "Software"), to deal in the Software without restriction, */ -/* including without limitation the rights to use, copy, modify, merge, */ -/* publish, distribute, sublicense, and/or sell copies of the Software, */ -/* and to permit persons to whom the Software is furnished to do so, */ -/* subject to the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ -/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ -/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ -/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ -/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ -/* SOFTWARE. */ -/* */ -/************************************************************************/ - -#include "watchdog.h" - - -void WDT_EnableAndSetTimeout() { - uint8_t temp = WDT_ENABLE_bm | WDT_CEN_bm | WATCHDOG_TIMEOUT; - CCP = CCP_IOREG_gc; - WDT.CTRL = temp; - - // Wait for WD to synchronize with new settings. - while (WDT_IsSyncBusy()) continue; -} - - -void WDT_Disable() { - uint8_t temp = (WDT.CTRL & ~WDT_ENABLE_bm) | WDT_CEN_bm; - CCP = CCP_IOREG_gc; - WDT.CTRL = temp; -} diff --git a/src/xboot/watchdog.h b/src/xboot/watchdog.h deleted file mode 100644 index f9fcc74..0000000 --- a/src/xboot/watchdog.h +++ /dev/null @@ -1,42 +0,0 @@ -/************************************************************************/ -/* XBoot Extensible AVR Bootloader */ -/* */ -/* Watchdog Module */ -/* */ -/* watchdog.c */ -/* */ -/* Alex Forencich */ -/* */ -/* Copyright (c) 2010 Alex Forencich */ -/* */ -/* Permission is hereby granted, free of charge, to any person */ -/* obtaining a copy of this software and associated documentation */ -/* files(the "Software"), to deal in the Software without restriction, */ -/* including without limitation the rights to use, copy, modify, merge, */ -/* publish, distribute, sublicense, and/or sell copies of the Software, */ -/* and to permit persons to whom the Software is furnished to do so, */ -/* subject to the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ -/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ -/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ -/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ -/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ -/* SOFTWARE. */ -/* */ -/************************************************************************/ - -#pragma once - -#include "xboot.h" - -#define WDT_IsSyncBusy() (WDT.STATUS & WDT_SYNCBUSY_bm) -#define WDT_Reset() asm("wdr") - -void WDT_EnableAndSetTimeout(); -void WDT_Disable(); diff --git a/src/xboot/xboot.c b/src/xboot/xboot.c deleted file mode 100644 index 7fd5ac0..0000000 --- a/src/xboot/xboot.c +++ /dev/null @@ -1,401 +0,0 @@ -/************************************************************************/ -/* XBoot Extensible AVR Bootloader */ -/* */ -/* xboot.c */ -/* */ -/* Alex Forencich */ -/* */ -/* Copyright (c) 2010 Alex Forencich */ -/* */ -/* Permission is hereby granted, free of charge, to any person */ -/* obtaining a copy of this software and associated documentation */ -/* files(the "Software"), to deal in the Software without restriction, */ -/* including without limitation the rights to use, copy, modify, merge, */ -/* publish, distribute, sublicense, and/or sell copies of the Software, */ -/* and to permit persons to whom the Software is furnished to do so, */ -/* subject to the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ -/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ -/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ -/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ -/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ -/* SOFTWARE. */ -/* */ -/************************************************************************/ - -#include "xboot.h" - -#include - -#include - - -uint8_t buffer[SPM_PAGESIZE]; - - -void NVM_Wait() { - while (NVM_STATUS & NVM_NVMBUSY_bp) - // reset watchdog while waiting for erase completion - WDT_Reset(); -} - - -int main() { - // Initialization section - // Entry point and communication methods are initialized here - // -------------------------------------------------- -#ifdef USE_32MHZ_RC -#if (F_CPU != 32000000L) -#error F_CPU must match oscillator setting! -#endif // F_CPU - OSC.CTRL |= OSC_RC32MEN_bm; // turn on 32 MHz oscillator - while (!(OSC.STATUS & OSC_RC32MRDY_bm)) continue; // wait for it to start - CCP = CCP_IOREG_gc; - CLK.CTRL = CLK_SCLKSEL_RC32M_gc; -#ifdef USE_DFLL - OSC.CTRL |= OSC_RC32KEN_bm; - while(!(OSC.STATUS & OSC_RC32KRDY_bm)); - DFLLRC32M.CTRL = DFLL_ENABLE_bm; -#endif // USE_DFLL -#else // USE_32MHZ_RC -#if (F_CPU != 2000000L) -#error F_CPU must match oscillator setting! -#endif // F_CPU -#ifdef USE_DFLL - OSC.CTRL |= OSC_RC32KEN_bm; - while(!(OSC.STATUS & OSC_RC32KRDY_bm)); - DFLLRC2M.CTRL = DFLL_ENABLE_bm; -#endif // USE_DFLL -#endif // USE_32MHZ_RC - - // Initialize UART - uart_init(); - - // End initialization section - - uint32_t address = 0; - uint16_t i = 0; - - bool in_bootloader = false; - uint16_t j = ENTER_WAIT; - while (!in_bootloader && 0 < j--) { - // Check for trigger - if (uart_has_char()) in_bootloader = uart_recv_char() == CMD_SYNC; - - _delay_ms(1); - } - - WDT_EnableAndSetTimeout(); - - // Main bootloader - while (in_bootloader) { - uint8_t val = get_char(); - WDT_Reset(); - - // Main bootloader parser - switch (val) { - case CMD_CHECK_AUTOINCREMENT: send_char(REPLY_YES); break; - - case CMD_SET_ADDRESS: - address = get_word(); - send_char(REPLY_ACK); - break; - - case CMD_SET_EXT_ADDRESS: - address = ((uint32_t)get_char() << 16) | get_word(); - send_char(REPLY_ACK); - break; - - case CMD_CHIP_ERASE: - // Erase the application section - SP_EraseApplicationSection(); - - // Wait for completion - NVM_Wait(); - - // Erase EEPROM - EEPROM_erase_all(); - - send_char(REPLY_ACK); - break; - - case CMD_CHECK_BLOCK_SUPPORT: - send_char(REPLY_YES); - // Send block size (page size) - send_char((SPM_PAGESIZE >> 8) & 0xFF); - send_char(SPM_PAGESIZE & 0xFF); - break; - - case CMD_BLOCK_LOAD: - i = get_word(); // Block size - val = get_char(); // Memory type - send_char(BlockLoad(i, val, &address)); // Load it - break; - - case CMD_BLOCK_READ: - i = get_word(); // Block size - val = get_char(); // Memory type - BlockRead(i, val, &address); // Read it - break; - - case CMD_READ_BYTE: { - unsigned w = SP_ReadWord((address << 1)); - - send_char(w >> 8); - send_char(w); - - address++; - break; - } - - case CMD_WRITE_LOW_BYTE: - i = get_char(); // get low byte - send_char(REPLY_ACK); - break; - - case CMD_WRITE_HIGH_BYTE: - i |= (get_char() << 8); // get high byte; combine - SP_LoadFlashWord((address << 1), i); - address++; - send_char(REPLY_ACK); - break; - - case CMD_WRITE_PAGE: - if (address >= (APP_SECTION_SIZE >> 1)) - send_char(REPLY_ERROR); // don't allow bootloader overwrite - - else { - SP_WriteApplicationPage(address << 1); - send_char(REPLY_ACK); - } - break; - - case CMD_WRITE_EEPROM_BYTE: - EEPROM_write_byte(address, get_char()); - address++; - send_char(REPLY_ACK); - break; - - case CMD_READ_EEPROM_BYTE: - send_char(EEPROM_read_byte(address)); - address++; - break; - - case CMD_READ_LOW_FUSE_BITS: send_char(SP_ReadFuseByte(0)); break; - case CMD_READ_HIGH_FUSE_BITS: send_char(SP_ReadFuseByte(1)); break; - case CMD_READ_EXT_FUSE_BITS: send_char(SP_ReadFuseByte(2)); break; - - case CMD_ENTER_PROG_MODE: case CMD_LEAVE_PROG_MODE: - send_char(REPLY_ACK); - break; - - case CMD_EXIT_BOOTLOADER: - in_bootloader = false; - send_char(REPLY_ACK); - break; - - case CMD_PROGRAMMER_TYPE: send_char('S'); break; // serial - - case CMD_DEVICE_CODE: - send_char(123); // send only this device - send_char(0); // terminator - break; - - case CMD_SET_LED: case CMD_CLEAR_LED: case CMD_SET_TYPE: - get_char(); // discard parameter - send_char(REPLY_ACK); - break; - - case CMD_PROGRAM_ID: - send_char('X'); - send_char('B'); - send_char('o'); - send_char('o'); - send_char('t'); - send_char('+'); - send_char('+'); - break; - - case CMD_VERSION: - send_char('0' + XBOOT_VERSION_MAJOR); - send_char('0' + XBOOT_VERSION_MINOR); - break; - - case CMD_READ_SIGNATURE: - send_char(SIGNATURE_2); - send_char(SIGNATURE_1); - send_char(SIGNATURE_0); - break; - - case CMD_CRC: { - uint32_t start = 0; - uint32_t length = 0; - uint16_t crc; - - val = get_char(); - - switch (val) { - case SECTION_FLASH: length = PROGMEM_SIZE; break; - case SECTION_APPLICATION: length = APP_SECTION_SIZE; break; - case SECTION_BOOT: - start = BOOT_SECTION_START; - length = BOOT_SECTION_SIZE; - break; - - default: - send_char(REPLY_ERROR); - continue; - } - - crc = crc16_block(start, length); - - send_char((crc >> 8) & 0xff); - send_char(crc & 0xff); - break; - } - - case CMD_SYNC: break; // ESC (0x1b) to sync - - default: // otherwise, error - send_char(REPLY_ERROR); - break; - } - - // Wait for any lingering SPM instructions to finish - NVM_Wait(); - } - - // Bootloader exit - // Code here runs after the bootloader has exited, - // but before the application code has started - - // Shut down UART - uart_deinit(); - - WDT_Disable(); - - // Jump into main code - asm("jmp 0"); -} - - -uint8_t get_char() { - while (!uart_has_char()) continue; - return uart_recv_char(); -} - - -void send_char(uint8_t c) {uart_send_char_blocking(c);} -uint16_t get_word() {return (get_char() << 8) | get_char();} - - -uint8_t BlockLoad(unsigned size, uint8_t mem, uint32_t *address) { - WDT_Reset(); - - // fill up buffer - for (int i = 0; i < SPM_PAGESIZE; i++) { - char c = 0xff; - if (i < size) c = get_char(); - buffer[i] = c; - } - - switch (mem) { - case MEM_EEPROM: - EEPROM_write_block(*address, buffer, size); - *address += size; - break; - - case MEM_FLASH: - // NOTE: Flash programming, address is given in words. - SP_LoadFlashPage(buffer); - SP_EraseWriteApplicationPage(*address << 1); - *address += size >> 1; - NVM_Wait(); - break; - - case MEM_USERSIG: - SP_LoadFlashPage(buffer); - SP_EraseUserSignatureRow(); - NVM_Wait(); - SP_WriteUserSignatureRow(); - NVM_Wait(); - break; - - default: return REPLY_ERROR; - } - - return REPLY_ACK; -} - - - -void BlockRead(unsigned size, uint8_t mem, uint32_t *address) { - int offset = 0; - int size2 = size; - - switch (mem) { - case MEM_EEPROM: - EEPROM_read_block(*address, buffer, size); - (*address) += size; - break; - - case MEM_FLASH: case MEM_USERSIG: case MEM_PRODSIG: { - *address <<= 1; // Convert address to bytes temporarily - - do { - switch (mem) { - case MEM_FLASH: buffer[offset++] = SP_ReadByte(*address); break; - - case MEM_USERSIG: - buffer[offset++] = SP_ReadUserSignatureByte(*address); - break; - - case MEM_PRODSIG: - buffer[offset++] = SP_ReadCalibrationByte(*address); - break; - } - - NVM_Wait(); - - (*address)++; // Select next word in memory. - size--; // Subtract two bytes from number of bytes to read - } while (size); // Repeat until all block has been read - - *address >>= 1; // Convert address back to Flash words again. - break; - } - - default: send_char(REPLY_ERROR); break; - } - - // send bytes - for (int i = 0; i < size2; i++) - send_char(buffer[i]); -} - - -uint16_t crc16_block(uint32_t start, uint32_t length) { - uint16_t crc = 0; - - int bc = SPM_PAGESIZE; - - for (; length > 0; length--) { - if (bc == SPM_PAGESIZE) { - SP_ReadFlashPage(buffer, start); - start += SPM_PAGESIZE; - bc = 0; - } - - crc = _crc16_update(crc, buffer[bc]); - - bc++; - } - - return crc; -} diff --git a/src/xboot/xboot.h b/src/xboot/xboot.h deleted file mode 100644 index 7b327d7..0000000 --- a/src/xboot/xboot.h +++ /dev/null @@ -1,168 +0,0 @@ -/************************************************************************/ -/* XBoot Extensible AVR Bootloader */ -/* */ -/* xboot.h */ -/* */ -/* Alex Forencich */ -/* */ -/* Copyright (c) 2010 Alex Forencich */ -/* */ -/* Permission is hereby granted, free of charge, to any person */ -/* obtaining a copy of this software and associated documentation */ -/* files(the "Software"), to deal in the Software without restriction, */ -/* including without limitation the rights to use, copy, modify, merge, */ -/* publish, distribute, sublicense, and/or sell copies of the Software, */ -/* and to permit persons to whom the Software is furnished to do so, */ -/* subject to the following conditions: */ -/* */ -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ -/* */ -/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ -/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ -/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS */ -/* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN */ -/* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN */ -/* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE */ -/* SOFTWARE. */ -/* */ -/************************************************************************/ - -#pragma once - -#include -#include -#include - -// token pasting -#define CAT2_int(x, y) x ## y -#define CAT2(x, y) CAT2_int(x, y) -#define CAT3_int(x, y, z) x ## y ## z -#define CAT3(x, y, z) CAT3_int(x, y, z) - -// Version -#define XBOOT_VERSION_MAJOR 1 -#define XBOOT_VERSION_MINOR 7 - -// Configuration -#define ENTER_WAIT 1000 // In ms -#define UART_BAUD_RATE 115200 -#define UART_NUMBER 0 -#define UART_PORT_NAME C -#define USE_AVR1008_EEPROM -#define USE_DFLL -#define WATCHDOG_TIMEOUT WDT_PER_1KCLK_gc - -// clock config -// use 32MHz osc if makefile calls for it -#if (F_CPU == 32000000L) -// defaults to 2MHz RC oscillator -// define USE_32MHZ_RC to override -#define USE_32MHZ_RC -#endif // F_CPU - -// UART RS485 Enable Output -#define UART_EN_PORT CAT2(PORT, UART_EN_PORT_NAME) - -#if (UART_NUMBER == 0) -#define UART_RX_PIN 2 -#define UART_TX_PIN 3 -#else -#define UART_RX_PIN 6 -#define UART_TX_PIN 7 -#endif -#define UART_PORT CAT2(PORT, UART_PORT_NAME) -#define UART_DEVICE_PORT CAT2(UART_PORT_NAME, UART_NUMBER) -#define UART_DEVICE CAT2(USART, UART_DEVICE_PORT) -#define UART_DEVICE_RXC_ISR CAT3(USART, UART_DEVICE_PORT, _RXC_vect) -#define UART_DEVICE_DRE_ISR CAT3(USART, UART_DEVICE_PORT, _DRE_vect) -#define UART_DEVICE_TXC_ISR CAT3(USART, UART_DEVICE_PORT, _TXC_vect) -#define UART_RX_PIN_CTRL CAT3(UART_PORT.PIN, UART_RX_PIN, CTRL) -#define UART_TX_PIN_CTRL CAT3(UART_PORT.PIN, UART_TX_PIN, CTRL) - -// BAUD Rate Values -// Known good at 2MHz -#if (F_CPU == 2000000L) && (UART_BAUD_RATE == 19200) -#define UART_BSEL_VALUE 12 -#define UART_BSCALE_VALUE 0 -#define UART_CLK2X 1 -#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 38400) -#define UART_BSEL_VALUE 22 -#define UART_BSCALE_VALUE -2 -#define UART_CLK2X 1 -#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 57600) -#define UART_BSEL_VALUE 26 -#define UART_BSCALE_VALUE -3 -#define UART_CLK2X 1 -#elif (F_CPU == 2000000L) && (UART_BAUD_RATE == 115200) -#define UART_BSEL_VALUE 19 -#define UART_BSCALE_VALUE -4 -#define UART_CLK2X 1 - -// Known good at 32MHz -#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 19200) -#define UART_BSEL_VALUE 103 -#define UART_BSCALE_VALUE 0 -#define UART_CLK2X 0 -#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 38400) -#define UART_BSEL_VALUE 51 -#define UART_BSCALE_VALUE 0 -#define UART_CLK2X 0 -#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 57600) -#define UART_BSEL_VALUE 34 -#define UART_BSCALE_VALUE 0 -#define UART_CLK2X 0 -#elif (F_CPU == 32000000L) && (UART_BAUD_RATE == 115200) -#define UART_BSEL_VALUE 1047 -#define UART_BSCALE_VALUE -6 -#define UART_CLK2X 0 - -#else // None of the above, so calculate something -#warning Not using predefined BAUD rate, possible BAUD rate error! -#if (F_CPU == 2000000L) -#define UART_BSEL_VALUE ((F_CPU) / ((uint32_t)UART_BAUD_RATE * 8) - 1) -#define UART_BSCALE_VALUE 0 -#define UART_CLK2X 1 - -#else -#define UART_BSEL_VALUE ((F_CPU) / ((uint32_t)UART_BAUD_RATE * 16) - 1) -#define UART_BSCALE_VALUE 0 -#define UART_CLK2X 0 -#endif -#endif - -#ifndef EEPROM_PAGE_SIZE -#define EEPROM_PAGE_SIZE E2PAGESIZE -#endif - -#ifndef EEPROM_BYTE_ADDRESS_MASK -#if EEPROM_PAGE_SIZE == 32 -#define EEPROM_BYTE_ADDRESS_MASK 0x1f -#elif EEPROM_PAGE_SIZE == 16 -#define EEPROM_BYTE_ADDRESS_MASK = 0x0f -#elif EEPROM_PAGE_SIZE == 8 -#define EEPROM_BYTE_ADDRESS_MASK = 0x07 -#elif EEPROM_PAGE_SIZE == 4 -#define EEPROM_BYTE_ADDRESS_MASK = 0x03 -#else -#error Unknown EEPROM page size! Please add new byte address value! -#endif -#endif - -// Includes -#include "protocol.h" -#include "sp_driver.h" -#include "eeprom_driver.h" -#include "uart.h" -#include "watchdog.h" - -// Functions -uint8_t get_char(void); -void send_char(uint8_t c); -uint16_t get_word(void); - -uint8_t BlockLoad(unsigned size, uint8_t mem, uint32_t *address); -void BlockRead(unsigned size, uint8_t mem, uint32_t *address); - -uint16_t crc16_block(uint32_t start, uint32_t length); diff --git a/tmc2660_decode.py b/tmc2660_decode.py deleted file mode 100755 index 1b5adb3..0000000 --- a/tmc2660_decode.py +++ /dev/null @@ -1,131 +0,0 @@ -#!/usr/bin/env python3 - -import sys -import csv -import json - - -rdsel = 1 - - -def twos_comp(val, bits): - if val & (1 << (bits - 1)): return val - (1 << bits) - return val - - -def tmc2660_decode_response(x, rdsel = 1): - x >>= 4 # Shift right 4 bits - d = {'_hex': '0x%05x' % x} - - if rdsel == 0: - d['mstep'] = (x >> 10) & 0x3ff - - elif rdsel == 1: - d['sg'] = (x >> 10) & 0x3ff - - elif rdsel == 2: - d['sg'] = x >> 15 & 0x1f - d['se'] = (x >> 10) & 0x1f - - flags = [] - if x & (1 << 7): flags += ['stand'] - if x & (1 << 6): flags += ['open B'] - if x & (1 << 5): flags += ['open A'] - if x & (1 << 4): flags += ['short B'] - if x & (1 << 3): flags += ['short A'] - if x & (1 << 2): flags += ['temp warn'] - if x & (1 << 1): flags += ['overtemp'] - if x & (1 << 0): flags += ['stall'] - - d['flags'] = flags - - return d - - -def tmc2660_decode_cmd(x, r): - global rdsel - - addr = x >> 17 - - d = {'_hex': '0x%05x' % x} - - if addr == 0 or addr == 1: - cmd = 'DRVCTRL' - d['mstep'] = (256, 128, 64, 32, 16, 8, 4, 2, 1)[x & 0xf] - d['dedge'] = bool(x & (1 << 8)) - d['intpol'] = bool(x & (1 << 9)) - - elif addr == 4: - chm = bool(x & (1 << 14)) - - cmd = 'CHOPCONF' - d['blank'] = (16, 24, 36, 54)[(x >> 15) & 3] - d['chm'] = 'standard' if chm else 'constant' - d['random toff'] = bool(x & (1 << 13)) - - if chm: d['fast decay mode'] = ('current', 'timer')[x >> 12] - else: d['hdec'] = (16, 32, 48, 64)[(x >> 11) & 3] - - if chm: d['SWO'] = ((x >> 7) & 0xf) - 3 - else: d['HEND'] = ((x >> 7) & 0xf) - 3 - - if chm: d['fast decay'] = (((x >> 4) & 7) + ((x >> 7) & 8)) * 32 - else: d['HSTART'] = ((x >> 4) & 7) + 1 - - # TODO this isn't quite right - toff = x & 0xf - if toff == 0: d['TOFF'] = 'disabled' - else: d['TOFF'] = 12 + (32 * toff) - - elif addr == 5: - cmd = 'SMARTEN' - d['SEIMIN'] = '1/4 CS' if x & (1 << 15) else '1/2 CS' - d['SEDN'] = (32, 8, 2, 1)[(x >> 13) & 3] - d['SEMAX'] = (x >> 8) & 0xf - d['SEUP'] = (1, 2, 4, 8)[(x >> 5) & 3] - semin = x & 0xf - d['SEMIN'] = semin if semin else 'disabled' - - elif addr == 6: - cmd = 'SGCSCONF' - d['SFILT'] = bool(x & (1 << 16)) - d['SGT'] = twos_comp((x >> 8) & 0x7f, 7) - d['CS'] = ((x & 0x1f) + 1) / 32.0 - - elif addr == 7: - cmd = 'DRVCONF' - d['TST'] = bool(x & (1 << 16)) - d['SLPH'] = ('min', 'min temp', 'med temp', 'max')[(x >> 14) & 3] - d['SLPL'] = ('min', 'min', 'med', 'max')[(x >> 12) & 3] - d['DISS2G'] = bool(x & (1 << 10)) - d['TS2G'] = (3.2, 1.6, 1.2, 0.8)[(x >> 8) & 3] - d['SDOFF'] = 'SPI' if x & (1 << 7) else 'step' - d['VSENSE'] = '165mV' if x & (1 << 6) else '305mV' - rdsel = (x >> 4) & 3 - d['RDSEL'] = ('mstep', 'SG', 'SG & CS', 'Invalid')[rdsel] - - else: cmd = 'INVALID' - - return {cmd: d, 'response': tmc2660_decode_response(r, rdsel)} - - -def tmc2660_decode(path): - with open(path, newline = '') as f: - reader = csv.reader(f) - - first = True - for time, packet, mosi, miso in reader: - if first: - first = False - continue - - cmd = tmc2660_decode_cmd(int(mosi, 0), int(miso, 0)) - cmd['id'] = int(packet) - cmd['ts'] = float(time) - - print(json.dumps(cmd, sort_keys = True)) - - -if __name__ == "__main__": - for path in sys.argv[1:]: - tmc2660_decode(path)