From 29beb0a5a0c219d7568b3b1be8ae677675e9de9d Mon Sep 17 00:00:00 2001 From: timkrins Date: Tue, 17 May 2011 21:31:40 +1000 Subject: [PATCH] Using GRBL instead now. Not yet working. --- .gitignore | 19 +- COPYING | 165 ++++++++++++++ LICENSE | 340 ----------------------------- Makefile | 246 ++++++++------------- README | 212 ------------------ arduino.h | 55 ----- arduino_644.h | 363 ------------------------------- clock.c | 39 ---- clock.h | 7 - config.h | 282 ++++-------------------- config.h.dist | 255 ---------------------- dda.c | 531 ---------------------------------------------- dda.h | 125 ----------- dda_queue.c | 130 ------------ dda_queue.h | 42 ---- debug.c | 3 - debug.h | 21 -- delay.c | 50 ----- delay.h | 15 -- eeprom.c | 151 +++++++++++++ eeprom.h | 9 + fuses.h | 11 - gcode.c | 467 ++++++++++++++++++++++++++++++++++++++++ gcode.h | 38 ++++ gcode_parse.c | 361 ------------------------------- gcode_parse.h | 82 ------- gcode_process.c | 293 ------------------------- gcode_process.h | 21 -- main.c | 55 +++++ mendel.c | 157 -------------- motion_control.c | 86 ++++++++ motion_control.h | 49 +++++ nuts_bolts.h | 35 +++ pinio.c | 20 -- pinio.h | 138 ------------ planner.c | 421 ++++++++++++++++++++++++++++++++++++ planner.h | 76 +++++++ readme.textile | 15 ++ serial.c | 219 ------------------- serial.h | 30 --- serial_protocol.c | 77 +++++++ serial_protocol.h | 30 +++ sermsg.c | 53 ----- sermsg.h | 21 -- sersendf.c | 133 ------------ sersendf.h | 9 - settings.c | 130 ++++++++++++ settings.h | 70 ++++++ spindle_control.c | 45 ++++ spindle_control.h | 30 +++ stepper.c | 297 ++++++++++++++++++++++++++ stepper.h | 40 ++++ timer.c | 120 ----------- timer.h | 30 --- watchdog.c | 46 ---- watchdog.h | 22 -- wiring_serial.c | 211 ++++++++++++++++++ wiring_serial.h | 45 ++++ 58 files changed, 2677 insertions(+), 4366 deletions(-) create mode 100644 COPYING delete mode 100644 LICENSE delete mode 100644 README delete mode 100644 arduino.h delete mode 100644 arduino_644.h delete mode 100644 clock.c delete mode 100644 clock.h delete mode 100644 config.h.dist delete mode 100644 dda.c delete mode 100644 dda.h delete mode 100644 dda_queue.c delete mode 100644 dda_queue.h delete mode 100644 debug.c delete mode 100644 debug.h delete mode 100644 delay.c delete mode 100644 delay.h create mode 100644 eeprom.c create mode 100644 eeprom.h delete mode 100644 fuses.h create mode 100644 gcode.c create mode 100644 gcode.h delete mode 100644 gcode_parse.c delete mode 100644 gcode_parse.h delete mode 100644 gcode_process.c delete mode 100644 gcode_process.h create mode 100644 main.c delete mode 100644 mendel.c create mode 100644 motion_control.c create mode 100644 motion_control.h create mode 100644 nuts_bolts.h delete mode 100644 pinio.c delete mode 100644 pinio.h create mode 100644 planner.c create mode 100644 planner.h create mode 100644 readme.textile delete mode 100644 serial.c delete mode 100644 serial.h create mode 100644 serial_protocol.c create mode 100644 serial_protocol.h delete mode 100644 sermsg.c delete mode 100644 sermsg.h delete mode 100644 sersendf.c delete mode 100644 sersendf.h create mode 100644 settings.c create mode 100644 settings.h create mode 100644 spindle_control.c create mode 100644 spindle_control.h create mode 100644 stepper.c create mode 100644 stepper.h delete mode 100644 timer.c delete mode 100644 timer.h delete mode 100644 watchdog.c delete mode 100644 watchdog.h create mode 100644 wiring_serial.c create mode 100644 wiring_serial.h diff --git a/.gitignore b/.gitignore index 22c8ce7..7ad1a0e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,19 +1,4 @@ +*.hex *.o *.elf -*.lst -*.map -*.sym -*.lss -*.eep -*.srec -*.bin -*.hex -*.al -*.i -*.s -*~ - -temporal.png -temporal_data -config.h -sim +*.DS_Store diff --git a/COPYING b/COPYING new file mode 100644 index 0000000..3f9959f --- /dev/null +++ b/COPYING @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser 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 +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. \ No newline at end of file diff --git a/LICENSE b/LICENSE deleted file mode 100644 index 3912109..0000000 --- a/LICENSE +++ /dev/null @@ -1,340 +0,0 @@ - GNU GENERAL PUBLIC LICENSE - Version 2, June 1991 - - Copyright (C) 1989, 1991 Free Software Foundation, Inc. - 51 Franklin St, 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 Library 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 St, 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 Library General -Public License instead of this License. diff --git a/Makefile b/Makefile index 50565c2..9627b73 100644 --- a/Makefile +++ b/Makefile @@ -1,161 +1,89 @@ -############################################################################## -# # -# FiveD on Arduino - alternative firmware for repraps # -# # -# by Triffid Hunter, Traumflug, jakepoz # -# # -# # -# This firmware is Copyright (C) 2009-2010 Michael Moon aka Triffid_Hunter # -# # -# 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 St, Fifth Floor, Boston, MA 02110-1301 USA # -# # -############################################################################## - -MCU_TARGET = atmega644p -F_CPU = 20000000L - -############################################################################## -# # -# Available Defines: # -# # -# DEBUG # -# enables tons of debugging output. may cause host-side talkers to choke # -# XONXOFF # -# enables XON/XOFF flow control for stupid or crude talkers # -# ACCELERATION_REPRAP # -# enables reprap-style acceleration # -# ACCELERATION_RAMPING # -# enables start/stop ramping # -# ACCELERATION_TEMPORAL # -# enables experimental temporal step algorithm - not technically a type of # -# acceleration, but since it controls step timing it seems appropriate # -# GEN3 # -# build for standard reprap electronics instead of your custom rig # -# HOST # -# this is the motherboard for GEN3- don't touch! Extruder has its own # -# Makefile. # -# # -############################################################################## - -DEFS = -DF_CPU=$(F_CPU) -#-DHOST -DGEN3 -# DEFS += "-DDEBUG=1" - -############################################################################## -# # -# Programmer settings for "make program" # -# # -############################################################################## - -AVRDUDE = avrdude -AVRDUDECONF = /etc/avrdude.conf - -############################################################################## -# # -# udev rule for /dev/arduino (insert into /etc/udev/rules.d/99-local.rules) # -# SUBSYSTEMS=="usb", ATTRS{idProduct}=="6001", ATTRS{idVendor}=="0403", # -# NAME="%k", SYMLINK+="arduino", SYMLINK+="arduino_$attr{serial}", # -# MODE="0660" # -# # -############################################################################## - -PROGPORT = /dev/arduino - -# atmega168 -#PROGBAUD = 19200 -# atmega328p, 644p, 1280 -PROGBAUD = 57600 - - -############################################################################## -# # -# These defaults should be ok, change if you need to # -# # -############################################################################## - -PROGRAM = mendel - -SOURCES = $(PROGRAM).c serial.c dda.c gcode_parse.c gcode_process.c timer.c sermsg.c dda_queue.c debug.c sersendf.c delay.c pinio.c clock.c watchdog.c - -ARCH = avr- -CC = $(ARCH)gcc -OBJDUMP = $(ARCH)objdump -OBJCOPY = $(ARCH)objcopy - -OPTIMIZE = -Os -ffunction-sections -finline-functions-called-once -mcall-prologues -# OPTIMIZE = -O0 -CFLAGS = -g -Wall -Wstrict-prototypes $(OPTIMIZE) -mmcu=$(MCU_TARGET) $(DEFS) -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -save-temps -LDFLAGS = -Wl,--as-needed -Wl,--gc-sections - -OBJ = $(patsubst %.c,%.o,${SOURCES}) - -.PHONY: all program clean size -.PRECIOUS: %.o %.elf - -all: config.h $(PROGRAM).hex $(PROGRAM).lst - -program: $(PROGRAM).hex config.h - stty $(PROGBAUD) raw ignbrk hup < $(PROGPORT) - @sleep 0.1 - @stty $(PROGBAUD) raw ignbrk hup < $(PROGPORT) - $(AVRDUDE) -cstk500v1 -b$(PROGBAUD) -p$(MCU_TARGET) -P$(PROGPORT) -C$(AVRDUDECONF) -U flash:w:$^ - stty 115200 raw ignbrk -hup -echo ixoff < $(PROGPORT) - -program-fuses: - avr-objdump -s -j .fuse mendel.o | perl -ne '/\s0000\s([0-9a-f]{2})/ && print "$$1\n"' > lfuse - avr-objdump -s -j .fuse mendel.o | perl -ne '/\s0000\s..([0-9a-f]{2})/ && print "$$1\n"' > hfuse - avr-objdump -s -j .fuse mendel.o | perl -ne '/\s0000\s....([0-9a-f]{2})/ && print "$$1\n"' > efuse - $(AVRDUDE) -cstk500v1 -b$(PROGBAUD) -p$(MCU_TARGET) -P$(PROGPORT) -C$(AVRDUDECONF) -U lfuse:w:lfuse - $(AVRDUDE) -cstk500v1 -b$(PROGBAUD) -p$(MCU_TARGET) -P$(PROGPORT) -C$(AVRDUDECONF) -U hfuse:w:hfuse - $(AVRDUDE) -cstk500v1 -b$(PROGBAUD) -p$(MCU_TARGET) -P$(PROGPORT) -C$(AVRDUDECONF) -U efuse:w:efuse +# Part of Grbl +# +# Copyright (c) 2009-2011 Simen Svale Skogsrud +# +# Grbl 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 3 of the License, or +# (at your option) any later version. +# +# Grbl is distributed in the hope that 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 Grbl. If not, see . + + +# This is a prototype Makefile. Modify it according to your needs. +# You should at least check the settings for +# DEVICE ....... The AVR device you compile for +# CLOCK ........ Target AVR clock rate in Hertz +# OBJECTS ...... The object files created from your source files. This list is +# usually the same as the list of source files with suffix ".o". +# PROGRAMMER ... Options to avrdude which define the hardware you use for +# uploading to the AVR and the interface where this hardware +# is connected. +# FUSES ........ Parameters for avrdude to flash the fuses appropriately. + +DEVICE = atmega644p +CLOCK = 20000000L +PROGRAMMER = -cusbtiny -P usb +OBJECTS = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o \ + eeprom.o settings.o planner.o + + +# Tune the lines below only if you know what you are doing: + +AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE) -B 10 -F +COMPILE = avr-gcc -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE) -I. -ffunction-sections + +# symbolic targets: +all: grbl.hex + +.c.o: + $(COMPILE) -c $< -o $@ + +.S.o: + $(COMPILE) -x assembler-with-cpp -c $< -o $@ +# "-x assembler-with-cpp" should not be necessary since this is the default +# file type for the .S (with capital S) extension. However, upper case +# characters are not always preserved on Windows. To ensure WinAVR +# compatibility define the file type manually. + +.c.s: + $(COMPILE) -S $< -o $@ + +flash: grbl.hex + $(AVRDUDE) -cusbtiny -p$(DEVICE) -V -U flash:w:$^ + +fuse: + $(AVRDUDE) $(FUSES) + +# Xcode uses the Makefile targets "", "clean" and "install" +install: flash fuse + +# if you use a bootloader, change the command below appropriately: +load: all + bootloadHID grbl.hex clean: - rm -rf *.o *.elf *.lst *.map *.sym *.lss *.eep *.srec *.bin *.hex *.al *.i *.s *~ *fuse - -size: $(PROGRAM).elf - @echo " SIZE Atmega168 Atmega328p Atmega644" - @$(OBJDUMP) -h $^ | perl -MPOSIX -ne '/.(text)\s+([0-9a-f]+)/ && do { $$a += eval "0x$$2" }; END { printf " FLASH : %5d bytes (%2d%% of %2dkb) (%2d%% of %2dkb) (%2d%% of %2dkb)\n", $$a, ceil($$a * 100 / (15 * 1024)), 15, ceil($$a * 100 / (31 * 1024)), 31, ceil($$a * 100 / (63 * 1024)), 63 }' - @$(OBJDUMP) -h $^ | perl -MPOSIX -ne '/.(data|bss)\s+([0-9a-f]+)/ && do { $$a += eval "0x$$2" }; END { printf " RAM : %5d bytes (%2d%% of %2dkb) (%2d%% of %2dkb) (%2d%% of %2dkb)\n", $$a, ceil($$a * 100 / (1 * 1024)), 1, ceil($$a * 100 / (2 * 1024)), 2, ceil($$a * 100 / (4 * 1024)), 4 }' - @$(OBJDUMP) -h $^ | perl -MPOSIX -ne '/.(eeprom)\s+([0-9a-f]+)/ && do { $$a += eval "0x$$2" }; END { printf " EEPROM: %5d bytes (%2d%% of %2dkb) (%2d%% of %2dkb) (%2d%% of %2dkb)\n", $$a, ceil($$a * 100 / (1 * 1024)), 1, ceil($$a * 100 / (2 * 1024)), 2, ceil($$a * 100 / (2 * 1024)), 2 }' - -config.h: config.h.dist - @echo "Please review config.h, as config.h.dist is more recent." - @echo - @diff -bBEuF '^. [[:digit:]]. [[:upper:]]' config.h config.h.dist - @false - -%.o: %.c config.h Makefile - @echo " CC $@" - @$(CC) -c $(CFLAGS) -Wa,-adhlns=$(<:.c=.al) -o $@ $(subst .o,.c,$@) - -%.elf: $(OBJ) - @echo " LINK $@" - @$(CC) $(CFLAGS) $(LDFLAGS) -o $@ $^ $(LIBS) - -%.lst: %.elf - @echo " OBJDUMP $@" - @$(OBJDUMP) -h -S $< > $@ - -%.hex: %.elf - @echo " OBJCOPY $@" - @$(OBJCOPY) -j .text -j .data -O ihex $< $@ - -%.bin: %.elf - @echo " OBJCOPY $@" - @$(OBJCOPY) -j .text -j .data -O binary $< $@ - -%.sym: %.elf - @echo " SYM $@" - @$(OBJDUMP) -t $< | perl -ne 'BEGIN { printf " ADDR NAME SIZE\n"; } /([0-9a-f]+)\s+(\w+)\s+O\s+\.(bss|data)\s+([0-9a-f]+)\s+(\w+)/ && printf "0x%04x %-20s +%d\n", eval("0x$$1") & 0xFFFF, $$5, eval("0x$$4")' | sort -k1 > $@ + rm -f grbl.hex main.elf $(OBJECTS) + +# file targets: +main.elf: $(OBJECTS) + $(COMPILE) -o main.elf $(OBJECTS) -lm -Wl,--gc-sections + +grbl.hex: main.elf + rm -f grbl.hex + avr-objcopy -j .text -j .data -O ihex main.elf grbl.hex +# If you have an EEPROM section, you must also create a hex file for the +# EEPROM and add it to the "flash" target. + +# Targets for code debugging and analysis: +disasm: main.elf + avr-objdump -d main.elf + +cpp: + $(COMPILE) -E main.c diff --git a/README b/README deleted file mode 100644 index d27534a..0000000 --- a/README +++ /dev/null @@ -1,212 +0,0 @@ -Rewrite of Reprap Mendel firmware: - -* 100% integer computations -* serial transmit buffer -* can fit onto atmega168 depending on selected options -* works on atmega328p -* works on atmega644p -* porting to atmega1280 in progress -* will work on larger atmegas with minor porting - -############################################################################## -# # -# How to use # -# # -############################################################################## - -1) COPY config.h.dist to config.h and edit to suit your electronics -2) check programming settings in Makefile -3) make -4) make program -4a) if programming blank chip, make program-fuses -5) ./sender.sh -6) have a play, go to 1) if not right -7) try printing something! - -############################################################################## -# # -# Requirements # -# # -############################################################################## - -Compile: - gnu make - binutils, gcc, etc built for avr target (avr-gcc, avr-as, etc) - avr-libc -Program: - avrdude - something that avrdude supports: bootloader, separate programmer, whatever - -############################################################################## -# # -# License # -# # -############################################################################## - -This firmware is Copyright (C) 2009-2010 Michael Moon aka Triffid_Hunter - -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 St, Fifth Floor, Boston, MA 02110-1301 USA - -############################################################################## -# # -# Rationale and History # -# # -############################################################################## - -I started building my electronics with only a regular arduino to test with. -This was perfectly sufficient for playing with the pololu stepper controllers and the max6675 I bought after reading about all the issues with thermistors that people were having. After a while I decided to check out the official firmware but it required an atmega644. I wondered why. -So, I decided to skim through the code to see what took up so much space. From what I could see, it was written by someone who was familiar with programming desktop systems and larger embedded devices, but didn't have much experience with small devices such as the atmega168 and atmega644. -This showed in the use of C++ which served only to make the code harder to read, and the prolific use of floating-point math, with some appearing even in interrupt context! -I came to the conclusion that there was no reason that the main body of code couldn't fit onto an atmega168 except for the burdensome and unnecessary overheads from object-oriented code and floating point math. A quick count assured me that the atmega168 had enough pins, but only barely, and I started reading the official firmware properly, with an eye to rewriting as much as possible in a fashion suitable for small microcontrollers. - -Starting with an arduino skeleton library I had assembled over time, some of my test code and the official firmware, I hacked up a passable integer-only, straight C implementation of the dda, and wrote my own gcode parser from scratch which processed each character as it arrived (with some buffering of course) instead of waiting for a whole line and then trying to process it all at once. - -As soon as my new firmware was able to run a few consecutive moves, I released it for peer review. - -The forum thread http://forums.reprap.org/read.php?147,33082 has much of the history from this point on. - -Traumflug was the first to send patches, and has done a significant amount of work on a number of different parts of this firmware. -jakepoz ported it to official reprap electronics (gen3 branch) -Cefiar posted me some thermistors to sponsor addition of thermistor-reading code - -Many others have given encouragement and suggestions without which this firmware may never be what it is today. - - -############################################################################## -# # -# Architectural Overview # -# # -############################################################################## - -FiveD on Arduino is quite similar to the official firmware in some ways, and markedly different in others. FiveD on Arduino has as much modularity as I could get away with without sacrificing efficiency. - -// FIXME: make next paragraph easier to read -At startup, the code in mendel.c is run first. This initialises all the modules that need it, then starts polling the clock flags and feeding incoming serial characters to the gcode parser. The gcode parser processes each character individually, keeping track via internal state rather than buffering a line and skipping back and forth. The gcode parser converts floating values to integer or fixed-point representations as soon as it encounters a non-numeric character. It calls many module functions directly, but the most interesting part is move creation, where it passes a target position and speed to enqueue()[dda_queue.c] which adds it to the queue, and fires up dda_start()[dda.c] if the queue was empty. dda_start initialises the dda, figures out the stepper directions and first step timeout and a few other bits of housekeeping, then sets the timer for the appropriate timeout. When the timer fires, it calls dda_step()[dda.c] which sends all the step signals then figures out the next step timeout based on acceleration and speed settings. When the last step has been made, the dda "dies" (sets 'live' property to 0) after which queue_step[dda_queue.c] advances the queue read pointer and starts the next dda. - -It is necessary to keep interrupts very short on small microcontrollers, and I have endeavoured to keep them all as short as possible. Unfortunately, dda_step[dda.c] is fairly large. I simply hope that it doesn't take so much time that it interferes with the other interrupts too much. - - -############################################################################## -# # -# Interesting code sections # -# # -############################################################################## - -The serial ringbuffers are critical for good communication, but for some reason the official arduino libraries don't implement a tx queue, all but preventing sending stuff from interrupt context. As long as the queues have a length of 2^n, we can use bitwise operations rather than numerical comparison to trim the read and write pointers. The serial send function (serial_writechar[serial.c]) is necessarily careful about checking if it's in an interrupt and only waiting for space in the queue if it's not. -The dda queue is also a ringbuffer, although its implementation is harder to see as it's embedded in lots of other stuff. - -The gcode parser shows how to parse each character as it comes in, so 99% of a command can be processed before the EOL is even received. It started off as a simple state machine, which then grew and shrank and morphed until it was both smaller and more functional. - -The fixed-point stuff is fun, although we have to manually ensure that the decimal point stays in the right spot. decfloat_to_int[gcode.h] is used to convert incoming floats to integer implementations by starting off with a (very!) crude floating point implementation, then choosing appropriate scaling factors within the gcode parser itself. This allows us to do a little stuff that looks like floating-point math without the burdensome overhead of a full fp implementation. - -The PID code in heater.c is probably quite generalisable, and seems to work well when tuned. Google knows of plenty of PID tuning guides. - -############################################################################## -# # -# Resources # -# # -############################################################################## - -Forum thread: http://forums.reprap.org/read.php?147,33082 -Source Repository: http://github.com/triffid/FiveD_on_Arduino -Wiki Page: http://objects.reprap.org/wiki/FiveD_on_Arduino - -############################################################################## -# # -# File descriptions # -# # -############################################################################## - -*** analog.[ch] -This is the analog subsystem. Only used if you have a thermistor or ad595 - -*** arduino.h, arduino_[chip].h -Pin mappings and helper functions for various atmegas - -*** clock.[ch] -Regular functions that run in main loop rather than an interrupt - -*** config.h.dist, config.h -Configuration for your electronics and hardware. Copy config.h.dist to config.h, edit config.h to suit - -*** copier.[ch] -A totally untested and currently unused chunk of code for copying firmware to another identical chip - -*** dda.[ch] -A rather complex block of math that figures out when to step each axis according to speed and acceleration profiles and received moves - -*** dda_queue.[ch] -The queue of moves received from the host. - -*** debug.[ch] -Debugging aids - -*** delay.[ch] -Delay functions - -*** FiveD_on_Arduino.pde -Allows firmware to be built in arduino ide - -*** func.sh -Lots of host-side shell scripts for talking to firmware - -*** gcode_parse.[ch] -Gcode parser. Scaling of factors to internally used integer or fixed point happens here too. - -*** gcode_process.[ch] -Gcodes actually get executed here after being parsed. - -*** heater.[ch] -Heater management, including PID and PWM algorithms, and some configuration parameters - -*** intercom.[ch] -Gen3 serial link control and communication - -*** LICENSE -Gnu GPL2 license - -*** Makefile -instructions for make on how to build firmware. has a list of modules to build which may need to be updated every so often - -*** mendel.c -Firmware startup and main loop code - -*** pinio.h -A few I/O primitives - -*** README -this file - -*** sender.sh -A simple talker - -*** serial.[ch] -Serial management and buffers - -*** sermsg.[ch] -Functions for sending messages and values to host - -*** sersendf.[ch] -A small, crude printf implementation - -*** temp.[ch] -Temperature sensor management, includes some configuration parameters - -*** timer.[ch] -Timer management, used primarily by dda.c for timing steps - -*** watchdog.[ch] -Watchdog management. resets chip if firmware locks up or does something strange - diff --git a/arduino.h b/arduino.h deleted file mode 100644 index 6bd6484..0000000 --- a/arduino.h +++ /dev/null @@ -1,55 +0,0 @@ -#ifndef _ARDUINO_H -#define _ARDUINO_H - -#include - -/* - utility functions -*/ - -#ifndef MASK -#define MASK(PIN) (1 << PIN) -#endif - -/* - magic I/O routines - - now you can simply SET_OUTPUT(STEP); WRITE(STEP, 1); WRITE(STEP, 0); -*/ - -#define _READ(IO) (IO ## _RPORT & MASK(IO ## _PIN)) -#define _WRITE(IO, v) do { if (v) { IO ## _WPORT |= MASK(IO ## _PIN); } else { IO ## _WPORT &= ~MASK(IO ## _PIN); }; } while (0) -#define _TOGGLE(IO) do { IO ## _RPORT = MASK(IO ## _PIN); } while (0) - -#define _SET_INPUT(IO) do { IO ## _DDR &= ~MASK(IO ## _PIN); } while (0) -#define _SET_OUTPUT(IO) do { IO ## _DDR |= MASK(IO ## _PIN); } while (0) - -#define _GET_INPUT(IO) ((IO ## _DDR & MASK(IO ## _PIN)) == 0) -#define _GET_OUTPUT(IO) ((IO ## _DDR & MASK(IO ## _PIN)) != 0) - -// why double up on these macros? see http://gcc.gnu.org/onlinedocs/cpp/Stringification.html - -#define READ(IO) _READ(IO) -#define WRITE(IO, v) _WRITE(IO, v) -#define TOGGLE(IO) _TOGGLE(IO) - -#define SET_INPUT(IO) _SET_INPUT(IO) -#define SET_OUTPUT(IO) _SET_OUTPUT(IO) - -#define GET_INPUT(IO) _GET_INPUT(IO) -#define GET_OUTPUT(IO) _GET_OUTPUT(IO) - -/* - ports and functions - added as necessary or if I feel like it- not a comprehensive list! -*/ - -#if defined (__AVR_ATmega644__) || defined (__AVR_ATmega644P__) || defined (__AVR_ATmega644PA__) - #include "arduino_644.h" -#endif /* _AVR_ATmega{644,644P,644PA}__ */ - -#ifndef DIO0_PIN -#error pins for this chip not defined in arduino.h! If you write an appropriate pin definition and have this firmware work on your chip, please tell us via the forum thread -#endif - -#endif /* _ARDUINO_H */ diff --git a/arduino_644.h b/arduino_644.h deleted file mode 100644 index 9f1a4ab..0000000 --- a/arduino_644.h +++ /dev/null @@ -1,363 +0,0 @@ -// UART -#define RXD DIO8 -#define TXD DIO9 -#define RXD0 DIO8 -#define TXD0 DIO9 - -#define RXD1 DIO10 -#define TXD1 DIO11 - -// SPI -#define SCK DIO7 -#define MISO DIO6 -#define MOSI DIO5 -#define SS DIO4 - -// TWI (I2C) -#define SCL DIO16 -#define SDA DIO17 - -// timers and PWM -#define OC0A DIO3 -#define OC0B DIO4 -#define OC1A DIO13 -#define OC1B DIO12 -#define OC2A DIO15 -#define OC2B DIO14 - -#define DEBUG_LED DIO0 -/* -pins -*/ - -#define DIO0_PIN PINB0 -#define DIO0_RPORT PINB -#define DIO0_WPORT PORTB -#define DIO0_DDR DDRB - -#define DIO1_PIN PINB1 -#define DIO1_RPORT PINB -#define DIO1_WPORT PORTB -#define DIO1_DDR DDRB - -#define DIO2_PIN PINB2 -#define DIO2_RPORT PINB -#define DIO2_WPORT PORTB -#define DIO2_DDR DDRB - -#define DIO3_PIN PINB3 -#define DIO3_RPORT PINB -#define DIO3_WPORT PORTB -#define DIO3_DDR DDRB - -#define DIO4_PIN PINB4 -#define DIO4_RPORT PINB -#define DIO4_WPORT PORTB -#define DIO4_DDR DDRB - -#define DIO5_PIN PINB5 -#define DIO5_RPORT PINB -#define DIO5_WPORT PORTB -#define DIO5_DDR DDRB - -#define DIO6_PIN PINB6 -#define DIO6_RPORT PINB -#define DIO6_WPORT PORTB -#define DIO6_DDR DDRB - -#define DIO7_PIN PINB7 -#define DIO7_RPORT PINB -#define DIO7_WPORT PORTB -#define DIO7_DDR DDRB - -#define DIO8_PIN PIND0 -#define DIO8_RPORT PIND -#define DIO8_WPORT PORTD -#define DIO8_DDR DDRD - -#define DIO9_PIN PIND1 -#define DIO9_RPORT PIND -#define DIO9_WPORT PORTD -#define DIO9_DDR DDRD - -#define DIO10_PIN PIND2 -#define DIO10_RPORT PIND -#define DIO10_WPORT PORTD -#define DIO10_DDR DDRD - -#define DIO11_PIN PIND3 -#define DIO11_RPORT PIND -#define DIO11_WPORT PORTD -#define DIO11_DDR DDRD - -#define DIO12_PIN PIND4 -#define DIO12_RPORT PIND -#define DIO12_WPORT PORTD -#define DIO12_DDR DDRD - -#define DIO13_PIN PIND5 -#define DIO13_RPORT PIND -#define DIO13_WPORT PORTD -#define DIO13_DDR DDRD - -#define DIO14_PIN PIND6 -#define DIO14_RPORT PIND -#define DIO14_WPORT PORTD -#define DIO14_DDR DDRD - -#define DIO15_PIN PIND7 -#define DIO15_RPORT PIND -#define DIO15_WPORT PORTD -#define DIO15_DDR DDRD - -#define DIO16_PIN PINC0 -#define DIO16_RPORT PINC -#define DIO16_WPORT PORTC -#define DIO16_DDR DDRC - -#define DIO17_PIN PINC1 -#define DIO17_RPORT PINC -#define DIO17_WPORT PORTC -#define DIO17_DDR DDRC - -#define DIO18_PIN PINC2 -#define DIO18_RPORT PINC -#define DIO18_WPORT PORTC -#define DIO18_DDR DDRC - -#define DIO19_PIN PINC3 -#define DIO19_RPORT PINC -#define DIO19_WPORT PORTC -#define DIO19_DDR DDRC - -#define DIO20_PIN PINC4 -#define DIO20_RPORT PINC -#define DIO20_WPORT PORTC -#define DIO20_DDR DDRC - -#define DIO21_PIN PINC5 -#define DIO21_RPORT PINC -#define DIO21_WPORT PORTC -#define DIO21_DDR DDRC - -#define DIO22_PIN PINC6 -#define DIO22_RPORT PINC -#define DIO22_WPORT PORTC -#define DIO22_DDR DDRC - -#define DIO23_PIN PINC7 -#define DIO23_RPORT PINC -#define DIO23_WPORT PORTC -#define DIO23_DDR DDRC - -#define DIO24_PIN PINA7 -#define DIO24_RPORT PINA -#define DIO24_WPORT PORTA -#define DIO24_DDR DDRA - -#define DIO25_PIN PINA6 -#define DIO25_RPORT PINA -#define DIO25_WPORT PORTA -#define DIO25_DDR DDRA - -#define DIO26_PIN PINA5 -#define DIO26_RPORT PINA -#define DIO26_WPORT PORTA -#define DIO26_DDR DDRA - -#define DIO27_PIN PINA4 -#define DIO27_RPORT PINA -#define DIO27_WPORT PORTA -#define DIO27_DDR DDRA - -#define DIO28_PIN PINA3 -#define DIO28_RPORT PINA -#define DIO28_WPORT PORTA -#define DIO28_DDR DDRA - -#define DIO29_PIN PINA2 -#define DIO29_RPORT PINA -#define DIO29_WPORT PORTA -#define DIO29_DDR DDRA - -#define DIO30_PIN PINA1 -#define DIO30_RPORT PINA -#define DIO30_WPORT PORTA -#define DIO30_DDR DDRA - -#define DIO31_PIN PINA0 -#define DIO31_RPORT PINA -#define DIO31_WPORT PORTA -#define DIO31_DDR DDRA - -#define AIO0_PIN PINA0 -#define AIO0_RPORT PINA -#define AIO0_WPORT PORTA -#define AIO0_DDR DDRA - -#define AIO1_PIN PINA1 -#define AIO1_RPORT PINA -#define AIO1_WPORT PORTA -#define AIO1_DDR DDRA - -#define AIO2_PIN PINA2 -#define AIO2_RPORT PINA -#define AIO2_WPORT PORTA -#define AIO2_DDR DDRA - -#define AIO3_PIN PINA3 -#define AIO3_RPORT PINA -#define AIO3_WPORT PORTA -#define AIO3_DDR DDRA - -#define AIO4_PIN PINA4 -#define AIO4_RPORT PINA -#define AIO4_WPORT PORTA -#define AIO4_DDR DDRA - -#define AIO5_PIN PINA5 -#define AIO5_RPORT PINA -#define AIO5_WPORT PORTA -#define AIO5_DDR DDRA - -#define AIO6_PIN PINA6 -#define AIO6_RPORT PINA -#define AIO6_WPORT PORTA -#define AIO6_DDR DDRA - -#define AIO7_PIN PINA7 -#define AIO7_RPORT PINA -#define AIO7_WPORT PORTA -#define AIO7_DDR DDRA - -#define PA0_PIN PINA0 -#define PA0_RPORT PINA -#define PA0_WPORT PORTA -#define PA0_DDR DDRA -#define PA1_PIN PINA1 -#define PA1_RPORT PINA -#define PA1_WPORT PORTA -#define PA1_DDR DDRA -#define PA2_PIN PINA2 -#define PA2_RPORT PINA -#define PA2_WPORT PORTA -#define PA2_DDR DDRA -#define PA3_PIN PINA3 -#define PA3_RPORT PINA -#define PA3_WPORT PORTA -#define PA3_DDR DDRA -#define PA4_PIN PINA4 -#define PA4_RPORT PINA -#define PA4_WPORT PORTA -#define PA4_DDR DDRA -#define PA5_PIN PINA5 -#define PA5_RPORT PINA -#define PA5_WPORT PORTA -#define PA5_DDR DDRA -#define PA6_PIN PINA6 -#define PA6_RPORT PINA -#define PA6_WPORT PORTA -#define PA6_DDR DDRA -#define PA7_PIN PINA7 -#define PA7_RPORT PINA -#define PA7_WPORT PORTA -#define PA7_DDR DDRA - -#define PB0_PIN PINB0 -#define PB0_RPORT PINB -#define PB0_WPORT PORTB -#define PB0_DDR DDRB -#define PB1_PIN PINB1 -#define PB1_RPORT PINB -#define PB1_WPORT PORTB -#define PB1_DDR DDRB -#define PB2_PIN PINB2 -#define PB2_RPORT PINB -#define PB2_WPORT PORTB -#define PB2_DDR DDRB -#define PB3_PIN PINB3 -#define PB3_RPORT PINB -#define PB3_WPORT PORTB -#define PB3_DDR DDRB -#define PB4_PIN PINB4 -#define PB4_RPORT PINB -#define PB4_WPORT PORTB -#define PB4_DDR DDRB -#define PB5_PIN PINB5 -#define PB5_RPORT PINB -#define PB5_WPORT PORTB -#define PB5_DDR DDRB -#define PB6_PIN PINB6 -#define PB6_RPORT PINB -#define PB6_WPORT PORTB -#define PB6_DDR DDRB -#define PB7_PIN PINB7 -#define PB7_RPORT PINB -#define PB7_WPORT PORTB -#define PB7_DDR DDRB - -#define PC0_PIN PINC0 -#define PC0_RPORT PINC -#define PC0_WPORT PORTC -#define PC0_DDR DDRC -#define PC1_PIN PINC1 -#define PC1_RPORT PINC -#define PC1_WPORT PORTC -#define PC1_DDR DDRC -#define PC2_PIN PINC2 -#define PC2_RPORT PINC -#define PC2_WPORT PORTC -#define PC2_DDR DDRC -#define PC3_PIN PINC3 -#define PC3_RPORT PINC -#define PC3_WPORT PORTC -#define PC3_DDR DDRC -#define PC4_PIN PINC4 -#define PC4_RPORT PINC -#define PC4_WPORT PORTC -#define PC4_DDR DDRC -#define PC5_PIN PINC5 -#define PC5_RPORT PINC -#define PC5_WPORT PORTC -#define PC5_DDR DDRC -#define PC6_PIN PINC6 -#define PC6_RPORT PINC -#define PC6_WPORT PORTC -#define PC6_DDR DDRC -#define PC7_PIN PINC7 -#define PC7_RPORT PINC -#define PC7_WPORT PORTC -#define PC7_DDR DDRC - -#define PD0_PIN PIND0 -#define PD0_RPORT PIND -#define PD0_WPORT PORTD -#define PD0_DDR DDRD -#define PD1_PIN PIND1 -#define PD1_RPORT PIND -#define PD1_WPORT PORTD -#define PD1_DDR DDRD -#define PD2_PIN PIND2 -#define PD2_RPORT PIND -#define PD2_WPORT PORTD -#define PD2_DDR DDRD -#define PD3_PIN PIND3 -#define PD3_RPORT PIND -#define PD3_WPORT PORTD -#define PD3_DDR DDRD -#define PD4_PIN PIND4 -#define PD4_RPORT PIND -#define PD4_WPORT PORTD -#define PD4_DDR DDRD -#define PD5_PIN PIND5 -#define PD5_RPORT PIND -#define PD5_WPORT PORTD -#define PD5_DDR DDRD -#define PD6_PIN PIND6 -#define PD6_RPORT PIND -#define PD6_WPORT PORTD -#define PD6_DDR DDRD -#define PD7_PIN PIND7 -#define PD7_RPORT PIND -#define PD7_WPORT PORTD -#define PD7_DDR DDRD diff --git a/clock.c b/clock.c deleted file mode 100644 index 12be710..0000000 --- a/clock.c +++ /dev/null @@ -1,39 +0,0 @@ -#include "clock.h" - -#include "pinio.h" -#include "sersendf.h" -#include "dda_queue.h" -#include "timer.h" -#include "debug.h" -#include "watchdog.h" - -void clock_250ms() { - if (steptimeout > (30 * 4)) { - power_off(); - } - else - steptimeout++; - - ifclock(CLOCK_FLAG_1S) { - if (debug_flags & DEBUG_POSITION) { - // current position - sersendf_P(PSTR("Pos: %ld,%ld,%ld,%lu\n"), current_position.X, current_position.Y, current_position.Z, current_position.F); - - // target position - sersendf_P(PSTR("Dst: %ld,%ld,%ld,%lu\n"), movebuffer[mb_tail].endpoint.X, movebuffer[mb_tail].endpoint.Y, movebuffer[mb_tail].endpoint.Z, movebuffer[mb_tail].endpoint.F); - - // Queue - print_queue(); - } - } -} - -void clock_10ms() { - // reset watchdog - wd_reset(); - - ifclock(CLOCK_FLAG_250MS) { - clock_250ms(); - } -} - diff --git a/clock.h b/clock.h deleted file mode 100644 index daf4023..0000000 --- a/clock.h +++ /dev/null @@ -1,7 +0,0 @@ -#ifndef _CLOCK_H -#define _CLOCK_H - -void clock_250ms(void); -void clock_10ms(void); - -#endif /* _CLOCK_H */ diff --git a/config.h b/config.h index f58be5f..db05294 100644 --- a/config.h +++ b/config.h @@ -1,255 +1,65 @@ -#ifndef _CONFIG_H -#define _CONFIG_H - -/* - CONTENTS - - 1. Mechanical/Hardware - 2. Acceleration settings - 3. Pinouts - 4. Temperature sensors - 5. Heaters - 6. Communication options - 7. Miscellaneous -*/ - -/***************************************************************************\ -* 1. MECHANICAL/HARDWARE * * -\***************************************************************************/ - -/* - Microcontroller type is set to atmega644p in the Makefile. -*/ - -/* - CPU clock rate -*/ -#ifndef F_CPU - #define F_CPU 20000000L -#endif - -#define HOST - -/* - Values reflecting the gearing of your machine. - All numbers are fixed point integers, so no more than 3 digits to the right of the decimal point, please :-) -*/ - -// calculate these values appropriate for your machine -// for threaded rods, this is (steps motor per turn) / (pitch of the thread) -// for belts, this is (steps per motor turn) / (number of gear teeth) / (belt module) -// half-stepping doubles the number, quarter stepping requires * 4, etc. -#define STEPS_PER_MM_X 320.000 -#define STEPS_PER_MM_Y 320.000 -#define STEPS_PER_MM_Z 320.000 - -/* - Values depending on the capabilities of your stepper motors and other mechanics. - All numbers are integers, no decimals allowed. - - Units are mm/min -*/ - -// used for G0 rapid moves and as a cap for all other feedrates -#define MAXIMUM_FEEDRATE_X 200 -#define MAXIMUM_FEEDRATE_Y 200 -#define MAXIMUM_FEEDRATE_Z 100 - -// used when searching endstops and as default feedrate -#define SEARCH_FEEDRATE_X 50 -#define SEARCH_FEEDRATE_Y 50 -#define SEARCH_FEEDRATE_Z 50 - -/***************************************************************************\ -* 2. ACCELERATION * -* IMPORTANT: choose only one! These algorithms choose when to step, trying * -* to use more than one will have undefined and probably * -* disastrous results! * -\***************************************************************************/ - /* - acceleration, reprap style. - Each movement starts at the speed of the previous command and accelerates or decelerates linearly to reach target speed at the end of the movement. -*/ -// #define ACCELERATION_REPRAP - - -/* - acceleration and deceleration ramping. - Each movement starts at (almost) no speed, linearly accelerates to target speed and decelerates just in time to smoothly stop at the target. alternative to ACCELERATION_REPRAP -*/ -#define ACCELERATION_RAMPING - -// how fast to accelerate when using ACCELERATION_RAMPING -// smaller values give quicker acceleration -// valid range = 1 to 8,000,000; 500,000 is a good starting point -#define ACCELERATION_STEEPNESS 500000 + config.h - compile time configuration + Part of Grbl + Copyright (c) 2009-2011 Simen Svale Skogsrud -/* - temporal step algorithm - This algorithm causes the timer to fire when any axis needs to step, instead of synchronising to the axis with the most steps ala bresenham. - - This algorithm is not a type of acceleration, and I haven't worked out how to integrate acceleration with it. - However it does control step timing, so acceleration algorithms seemed appropriate + Grbl 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 3 of the License, or + (at your option) any later version. - The Bresenham algorithm is great for drawing lines, but not so good for steppers - In the case where X steps 3 times to Y's two, Y experiences massive jitter as it steps in sync with X every 2 out of 3 X steps. This is a worst-case, but the problem exists for most non-45/90 degree moves. At higher speeds, the jitter /will/ cause position loss and unnecessary vibration. - This algorithm instead calculates when a step occurs on any axis, and sets the timer to that value. + Grbl is distributed in the hope that 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. - // TODO: figure out how to add acceleration to this algorithm + You should have received a copy of the GNU General Public License + along with Grbl. If not, see . */ -// #define ACCELERATION_TEMPORAL +#ifndef config_h +#define config_h -/***************************************************************************\ -* 3. PINOUTS * -\***************************************************************************/ - -/* - Machine Pin Definitions - - make sure to avoid duplicate usage of a pin - - comment out pins not in use, as this drops the corresponding code and makes operations faster -*/ - -#include "arduino.h" - -#ifndef GEN3 - /* - user defined pins - adjust to suit your electronics - */ - - #define X_STEP_PIN DIO21 //XSTEP is PC5... ie. DIO21 - #define X_DIR_PIN DIO18 //XDIR pin is PC2. DIO18 - #define X_MIN_PIN DIO0 // XMIN is not correct, we have an XHOME on PB0 - - #define Y_STEP_PIN DIO22 // PC6 is DIO22 - #define Y_DIR_PIN DIO17 // DIO17 is PC1 - #define Y_MIN_PIN DIO1 // PB1 - - #define Z_STEP_PIN DIO23 // PC7 is DIO23 - #define Z_DIR_PIN DIO16 // DIO16 is PC0 - #define Z_MIN_PIN DIO7 // PB2 - - #define STEPPER_ENABLE_PIN DIO19 // PC3 (active low) - #define STEPPER_RESET_PIN DIO20 // PC4 (needs to be high to move) - - -#else - /* - this is the official gen3 reprap motherboard pinout - */ - #define TX_ENABLE_PIN DIO12 - #define RX_ENABLE_PIN DIO13 - - #define X_STEP_PIN DIO15 - #define X_DIR_PIN DIO18 - #define X_MIN_PIN DIO20 - #define X_MAX_PIN DIO21 - #define X_ENABLE_PIN DIO19 - - #define Y_STEP_PIN DIO23 - #define Y_DIR_PIN DIO22 - #define Y_MIN_PIN AIO6 - #define Y_MAX_PIN AIO5 - #define Y_ENABLE_PIN DIO7 - - #define Z_STEP_PIN AIO4 - #define Z_DIR_PIN AIO3 - #define Z_MIN_PIN AIO1 - #define Z_MAX_PIN AIO0 - #define Z_ENABLE_PIN AIO2 - - #define E_STEP_PIN DIO16 - #define E_DIR_PIN DIO17 - - #define SD_CARD_DETECT DIO2 - #define SD_WRITE_PROTECT DIO3 +#ifndef __AVR_ATmega328P__ +#define __AVR_ATmega328P__ #endif -/***************************************************************************\ -* 5. TEMPERATURE SENSORS * -\***************************************************************************/ - -// how many temperature sensors do you have? -#define NUM_TEMP_SENSORS 0 - -/***************************************************************************\ -* 5. HEATERS * -\***************************************************************************/ - -// number of heaters- for GEN3, set to zero as extruder manages the heater by itself -#define NUM_HEATERS 0 - -/***************************************************************************\ -* 6. COMMUNICATION OPTIONS * -\***************************************************************************/ - -/* - RepRap Host changes it's communications protocol from time to time and intentionally avoids backwards compatibility. Set this to the date the source code of your Host was fetched from RepRap's repository, which is likely also the build date. - See the discussion on the reprap-dev mailing list from 11 Oct. 2010. - - Undefine it for best human readability, set it to an old date for compatibility with hosts before August 2010 -*/ -// #define REPRAP_HOST_COMPATIBILITY 19750101 -#define REPRAP_HOST_COMPATIBILITY 20100806 -// #define REPRAP_HOST_COMPATIBILITY - -/* - Xon/Xoff flow control. - Redundant when using RepRap Host for sending GCode, but mandatory when sending GCode files with a plain terminal emulator, like GtkTerm (Linux), CoolTerm (Mac) or HyperTerminal (Windows). - Can also be set in Makefile -*/ -#define XONXOFF - +#define BAUD_RATE 9600 +// Updated default pin-assignments from 0.6 onwards +// (see bottom of file for a copy of the old config) -/***************************************************************************\ -* 7. MISCELLANEOUS OPTIONS * -\***************************************************************************/ +#define STEPPERS_ENABLE_DDR DDRC +#define STEPPERS_ENABLE_PORT PORTC +#define STEPPERS_ENABLE_BIT 3 //PC3 -/* - DEBUG - enables /heaps/ of extra output, and some extra M-codes. - WARNING: this WILL break most host-side talkers that expect particular responses from firmware such as reprap host and replicatorG - use with serial terminal or other suitable talker only. -*/ -// #define DEBUG - -/* - move buffer size, in number of moves - note that each move takes a fair chunk of ram (69 bytes as of this writing) so don't make the buffer too big - a bigger serial readbuffer may help more than increasing this unless your gcodes are more than 70 characters long on average. - however, a larger movebuffer will probably help with lots of short consecutive moves, as each move takes a bunch of math (hence time) to set up so a longer buffer allows more of the math to be done during preceding longer moves -*/ -#define MOVEBUFFER_SIZE 8 - - -/* - FiveD on Arduino implements a watchdog, which has to be reset every 250ms or it will reboot the controller. As rebooting (and letting the GCode sending application trying to continue the build with a then different Home point) is probably even worse than just hanging, and there is no better restore code in place, this is disabled for now. -*/ -// #define USE_WATCHDOG - -/* - analog subsystem stuff - REFERENCE - which analog reference to use. see analog.h for choices -*/ -#define REFERENCE REFERENCE_AVCC +#define STEPPING_DDR DDRC +#define STEPPING_PORT PORTC +#define X_STEP_BIT 5 //PC5 +#define Y_STEP_BIT 6 //PC6 +#define Z_STEP_BIT 7 //PC7 +#define X_DIRECTION_BIT 2 //PC2 +#define Y_DIRECTION_BIT 1 //PC1 +#define Z_DIRECTION_BIT 0 //PC0 -/* - this option makes the step interrupt interruptible (nested). - this should help immensely with dropped serial characters, but may also make debugging infuriating due to the complexities arising from nested interrupts -*/ -#define STEP_INTERRUPT_INTERRUPTIBLE 1 +#define LIMIT_DDR DDRA +#define LIMIT_PORT PORTA +#define X_LIMIT_BIT 1 +#define Y_LIMIT_BIT 2 +#define Z_LIMIT_BIT 3 -/* - how often we overflow and update our clock; with F_CPU=16MHz, max is < 4.096ms (TICK_TIME = 65535) -*/ -#define TICK_TIME 2 MS -#define TICK_TIME_MS (TICK_TIME / (F_CPU / 1000)) +#define SPINDLE_ENABLE_DDR DDRB +#define SPINDLE_ENABLE_PORT PORTB +#define SPINDLE_ENABLE_BIT 4 +#define SPINDLE_DIRECTION_DDR DDRB +#define SPINDLE_DIRECTION_PORT PORTB +#define SPINDLE_DIRECTION_BIT 5 -// this is the scaling of internally stored PID values. 1024L is a good value -#define PID_SCALE 1024L +// The temporal resolution of the acceleration management subsystem. Higher number +// give smoother acceleration but may impact performance +#define ACCELERATION_TICKS_PER_SECOND 40L -#endif /* _CONFIG_H */ +#endif \ No newline at end of file diff --git a/config.h.dist b/config.h.dist deleted file mode 100644 index f58be5f..0000000 --- a/config.h.dist +++ /dev/null @@ -1,255 +0,0 @@ -#ifndef _CONFIG_H -#define _CONFIG_H - -/* - CONTENTS - - 1. Mechanical/Hardware - 2. Acceleration settings - 3. Pinouts - 4. Temperature sensors - 5. Heaters - 6. Communication options - 7. Miscellaneous -*/ - -/***************************************************************************\ -* 1. MECHANICAL/HARDWARE * * -\***************************************************************************/ - -/* - Microcontroller type is set to atmega644p in the Makefile. -*/ - -/* - CPU clock rate -*/ -#ifndef F_CPU - #define F_CPU 20000000L -#endif - -#define HOST - -/* - Values reflecting the gearing of your machine. - All numbers are fixed point integers, so no more than 3 digits to the right of the decimal point, please :-) -*/ - -// calculate these values appropriate for your machine -// for threaded rods, this is (steps motor per turn) / (pitch of the thread) -// for belts, this is (steps per motor turn) / (number of gear teeth) / (belt module) -// half-stepping doubles the number, quarter stepping requires * 4, etc. -#define STEPS_PER_MM_X 320.000 -#define STEPS_PER_MM_Y 320.000 -#define STEPS_PER_MM_Z 320.000 - -/* - Values depending on the capabilities of your stepper motors and other mechanics. - All numbers are integers, no decimals allowed. - - Units are mm/min -*/ - -// used for G0 rapid moves and as a cap for all other feedrates -#define MAXIMUM_FEEDRATE_X 200 -#define MAXIMUM_FEEDRATE_Y 200 -#define MAXIMUM_FEEDRATE_Z 100 - -// used when searching endstops and as default feedrate -#define SEARCH_FEEDRATE_X 50 -#define SEARCH_FEEDRATE_Y 50 -#define SEARCH_FEEDRATE_Z 50 - -/***************************************************************************\ -* 2. ACCELERATION * -* IMPORTANT: choose only one! These algorithms choose when to step, trying * -* to use more than one will have undefined and probably * -* disastrous results! * -\***************************************************************************/ - -/* - acceleration, reprap style. - Each movement starts at the speed of the previous command and accelerates or decelerates linearly to reach target speed at the end of the movement. -*/ -// #define ACCELERATION_REPRAP - - -/* - acceleration and deceleration ramping. - Each movement starts at (almost) no speed, linearly accelerates to target speed and decelerates just in time to smoothly stop at the target. alternative to ACCELERATION_REPRAP -*/ -#define ACCELERATION_RAMPING - -// how fast to accelerate when using ACCELERATION_RAMPING -// smaller values give quicker acceleration -// valid range = 1 to 8,000,000; 500,000 is a good starting point -#define ACCELERATION_STEEPNESS 500000 - - -/* - temporal step algorithm - This algorithm causes the timer to fire when any axis needs to step, instead of synchronising to the axis with the most steps ala bresenham. - - This algorithm is not a type of acceleration, and I haven't worked out how to integrate acceleration with it. - However it does control step timing, so acceleration algorithms seemed appropriate - - The Bresenham algorithm is great for drawing lines, but not so good for steppers - In the case where X steps 3 times to Y's two, Y experiences massive jitter as it steps in sync with X every 2 out of 3 X steps. This is a worst-case, but the problem exists for most non-45/90 degree moves. At higher speeds, the jitter /will/ cause position loss and unnecessary vibration. - This algorithm instead calculates when a step occurs on any axis, and sets the timer to that value. - - // TODO: figure out how to add acceleration to this algorithm -*/ -// #define ACCELERATION_TEMPORAL - - - -/***************************************************************************\ -* 3. PINOUTS * -\***************************************************************************/ - -/* - Machine Pin Definitions - - make sure to avoid duplicate usage of a pin - - comment out pins not in use, as this drops the corresponding code and makes operations faster -*/ - -#include "arduino.h" - -#ifndef GEN3 - /* - user defined pins - adjust to suit your electronics - */ - - #define X_STEP_PIN DIO21 //XSTEP is PC5... ie. DIO21 - #define X_DIR_PIN DIO18 //XDIR pin is PC2. DIO18 - #define X_MIN_PIN DIO0 // XMIN is not correct, we have an XHOME on PB0 - - #define Y_STEP_PIN DIO22 // PC6 is DIO22 - #define Y_DIR_PIN DIO17 // DIO17 is PC1 - #define Y_MIN_PIN DIO1 // PB1 - - #define Z_STEP_PIN DIO23 // PC7 is DIO23 - #define Z_DIR_PIN DIO16 // DIO16 is PC0 - #define Z_MIN_PIN DIO7 // PB2 - - #define STEPPER_ENABLE_PIN DIO19 // PC3 (active low) - #define STEPPER_RESET_PIN DIO20 // PC4 (needs to be high to move) - - -#else - /* - this is the official gen3 reprap motherboard pinout - */ - #define TX_ENABLE_PIN DIO12 - #define RX_ENABLE_PIN DIO13 - - #define X_STEP_PIN DIO15 - #define X_DIR_PIN DIO18 - #define X_MIN_PIN DIO20 - #define X_MAX_PIN DIO21 - #define X_ENABLE_PIN DIO19 - - #define Y_STEP_PIN DIO23 - #define Y_DIR_PIN DIO22 - #define Y_MIN_PIN AIO6 - #define Y_MAX_PIN AIO5 - #define Y_ENABLE_PIN DIO7 - - #define Z_STEP_PIN AIO4 - #define Z_DIR_PIN AIO3 - #define Z_MIN_PIN AIO1 - #define Z_MAX_PIN AIO0 - #define Z_ENABLE_PIN AIO2 - - #define E_STEP_PIN DIO16 - #define E_DIR_PIN DIO17 - - #define SD_CARD_DETECT DIO2 - #define SD_WRITE_PROTECT DIO3 -#endif - -/***************************************************************************\ -* 5. TEMPERATURE SENSORS * -\***************************************************************************/ - -// how many temperature sensors do you have? -#define NUM_TEMP_SENSORS 0 - -/***************************************************************************\ -* 5. HEATERS * -\***************************************************************************/ - -// number of heaters- for GEN3, set to zero as extruder manages the heater by itself -#define NUM_HEATERS 0 - -/***************************************************************************\ -* 6. COMMUNICATION OPTIONS * -\***************************************************************************/ - -/* - RepRap Host changes it's communications protocol from time to time and intentionally avoids backwards compatibility. Set this to the date the source code of your Host was fetched from RepRap's repository, which is likely also the build date. - See the discussion on the reprap-dev mailing list from 11 Oct. 2010. - - Undefine it for best human readability, set it to an old date for compatibility with hosts before August 2010 -*/ -// #define REPRAP_HOST_COMPATIBILITY 19750101 -#define REPRAP_HOST_COMPATIBILITY 20100806 -// #define REPRAP_HOST_COMPATIBILITY - -/* - Xon/Xoff flow control. - Redundant when using RepRap Host for sending GCode, but mandatory when sending GCode files with a plain terminal emulator, like GtkTerm (Linux), CoolTerm (Mac) or HyperTerminal (Windows). - Can also be set in Makefile -*/ -#define XONXOFF - - - -/***************************************************************************\ -* 7. MISCELLANEOUS OPTIONS * -\***************************************************************************/ - -/* - DEBUG - enables /heaps/ of extra output, and some extra M-codes. - WARNING: this WILL break most host-side talkers that expect particular responses from firmware such as reprap host and replicatorG - use with serial terminal or other suitable talker only. -*/ -// #define DEBUG - -/* - move buffer size, in number of moves - note that each move takes a fair chunk of ram (69 bytes as of this writing) so don't make the buffer too big - a bigger serial readbuffer may help more than increasing this unless your gcodes are more than 70 characters long on average. - however, a larger movebuffer will probably help with lots of short consecutive moves, as each move takes a bunch of math (hence time) to set up so a longer buffer allows more of the math to be done during preceding longer moves -*/ -#define MOVEBUFFER_SIZE 8 - - -/* - FiveD on Arduino implements a watchdog, which has to be reset every 250ms or it will reboot the controller. As rebooting (and letting the GCode sending application trying to continue the build with a then different Home point) is probably even worse than just hanging, and there is no better restore code in place, this is disabled for now. -*/ -// #define USE_WATCHDOG - -/* - analog subsystem stuff - REFERENCE - which analog reference to use. see analog.h for choices -*/ -#define REFERENCE REFERENCE_AVCC - -/* - this option makes the step interrupt interruptible (nested). - this should help immensely with dropped serial characters, but may also make debugging infuriating due to the complexities arising from nested interrupts -*/ -#define STEP_INTERRUPT_INTERRUPTIBLE 1 - -/* - how often we overflow and update our clock; with F_CPU=16MHz, max is < 4.096ms (TICK_TIME = 65535) -*/ -#define TICK_TIME 2 MS -#define TICK_TIME_MS (TICK_TIME / (F_CPU / 1000)) - - -// this is the scaling of internally stored PID values. 1024L is a good value -#define PID_SCALE 1024L - -#endif /* _CONFIG_H */ diff --git a/dda.c b/dda.c deleted file mode 100644 index 786cff9..0000000 --- a/dda.c +++ /dev/null @@ -1,531 +0,0 @@ -#include "dda.h" - -#include -#include -#include - -#include "timer.h" -#include "serial.h" -#include "sermsg.h" -#include "dda_queue.h" -#include "debug.h" -#include "sersendf.h" -#include "pinio.h" -#include "config.h" - -/* - Used in distance calculation during DDA setup -*/ -#define UM_PER_STEP_X 1000L / ((uint32_t) STEPS_PER_MM_X) -#define UM_PER_STEP_Y 1000L / ((uint32_t) STEPS_PER_MM_Y) -#define UM_PER_STEP_Z 1000L / ((uint32_t) STEPS_PER_MM_Z) - -/* - step timeout -*/ - -uint8_t steptimeout = 0; - -/* - position tracking -*/ - -TARGET startpoint __attribute__ ((__section__ (".bss"))); -TARGET current_position __attribute__ ((__section__ (".bss"))); - -/* - utility functions -*/ - -// courtesy of http://www.flipcode.com/archives/Fast_Approximate_Distance_Functions.shtml -uint32_t approx_distance( uint32_t dx, uint32_t dy ) -{ - uint32_t min, max, approx; - - if ( dx < dy ) - { - min = dx; - max = dy; - } else { - min = dy; - max = dx; - } - - approx = ( max * 1007 ) + ( min * 441 ); - if ( max < ( min << 4 )) - approx -= ( max * 40 ); - - // add 512 for proper rounding - return (( approx + 512 ) >> 10 ); -} - -// courtesy of http://www.oroboro.com/rafael/docserv.php/index/programming/article/distance -uint32_t approx_distance_3( uint32_t dx, uint32_t dy, uint32_t dz ) -{ - uint32_t min, med, max, approx; - - if ( dx < dy ) - { - min = dy; - med = dx; - } else { - min = dx; - med = dy; - } - - if ( dz < min ) - { - max = med; - med = min; - min = dz; - } else if ( dz < med ) { - max = med; - med = dz; - } else { - max = dz; - } - - approx = ( max * 860 ) + ( med * 851 ) + ( min * 520 ); - if ( max < ( med << 1 )) approx -= ( max * 294 ); - if ( max < ( min << 2 )) approx -= ( max * 113 ); - if ( med < ( min << 2 )) approx -= ( med * 40 ); - - // add 512 for proper rounding - return (( approx + 512 ) >> 10 ); -} - -// this is an ultra-crude pseudo-logarithm routine, such that: -// 2 ^ msbloc(v) >= v -const uint8_t msbloc (uint32_t v) { - uint8_t i; - uint32_t c; - for (i = 31, c = 0x80000000; i; i--) { - if (v & c) - return i; - c >>= 1; - } - return 0; -} - -/* - CREATE a dda given current_position and a target, save to passed location so we can write directly into the queue -*/ - -void dda_create(DDA *dda, TARGET *target) { - uint32_t distance, c_limit, c_limit_calc; - - // initialise DDA to a known state - dda->allflags = 0; - - if (debug_flags & DEBUG_DDA) - serial_writestr_P(PSTR("\n{DDA_CREATE: [")); - - // we end at the passed target - memcpy(&(dda->endpoint), target, sizeof(TARGET)); - - dda->x_delta = labs(target->X - startpoint.X); - dda->y_delta = labs(target->Y - startpoint.Y); - dda->z_delta = labs(target->Z - startpoint.Z); - - dda->x_direction = (target->X >= startpoint.X)?1:0; - dda->y_direction = (target->Y >= startpoint.Y)?1:0; - dda->z_direction = (target->Z >= startpoint.Z)?1:0; - - if (debug_flags & DEBUG_DDA) - sersendf_P(PSTR("%ld,%ld,%ld] ["), target->X - startpoint.X, target->Y - startpoint.Y, target->Z - startpoint.Z); - - dda->total_steps = dda->x_delta; - if (dda->y_delta > dda->total_steps) - dda->total_steps = dda->y_delta; - if (dda->z_delta > dda->total_steps) - dda->total_steps = dda->z_delta; - - if (debug_flags & DEBUG_DDA) - sersendf_P(PSTR("ts:%lu"), dda->total_steps); - - if (dda->total_steps == 0) { - dda->nullmove = 1; - } - else { - // get steppers ready to go - steptimeout = 0; - power_on(); - x_enable(); - y_enable(); - if (dda->z_delta) - z_enable(); - - // since it's unusual to combine X, Y and Z changes in a single move on reprap, check if we can use simpler approximations before trying the full 3d approximation. - if (dda->z_delta == 0) - distance = approx_distance(dda->x_delta * UM_PER_STEP_X, dda->y_delta * UM_PER_STEP_Y); - else if (dda->x_delta == 0 && dda->y_delta == 0) - distance = dda->z_delta * UM_PER_STEP_Z; - else - distance = approx_distance_3(dda->x_delta * UM_PER_STEP_X, dda->y_delta * UM_PER_STEP_Y, dda->z_delta * UM_PER_STEP_Z); - - if (debug_flags & DEBUG_DDA) - sersendf_P(PSTR(",ds:%lu"), distance); - - #ifdef ACCELERATION_TEMPORAL - // bracket part of this equation in an attempt to avoid overflow: 60 * 16MHz * 5mm is >32 bits - uint32_t move_duration = distance * (60 * F_CPU / startpoint.F); - #else - dda->x_counter = dda->y_counter = dda->z_counter = - -(dda->total_steps >> 1); - - // pre-calculate move speed in millimeter microseconds per step minute for less math in interrupt context - // mm (distance) * 60000000 us/min / step (total_steps) = mm.us per step.min - // note: um (distance) * 60000 == mm * 60000000 - // so in the interrupt we must simply calculate - // mm.us per step.min / mm per min (F) = us per step - - // break this calculation up a bit and lose some precision because 300,000um * 60000 is too big for a uint32 - // calculate this with a uint64 if you need the precision, but it'll take longer so routines with lots of short moves may suffer - // 2^32/6000 is about 715mm which should be plenty - - // changed * 10 to * (F_CPU / 100000) so we can work in cpu_ticks rather than microseconds. - // timer.c setTimer() routine altered for same reason - - // changed distance * 6000 .. * F_CPU / 100000 to - // distance * 2400 .. * F_CPU / 40000 so we can move a distance of up to 1800mm without overflowing - uint32_t move_duration = ((distance * 2400) / dda->total_steps) * (F_CPU / 40000); - #endif - - // similarly, find out how fast we can run our axes. - // do this for each axis individually, as the combined speed of two or more axes can be higher than the capabilities of a single one. - c_limit = 0; - // check X axis - c_limit_calc = ( (dda->x_delta * (UM_PER_STEP_X * 2400L)) / dda->total_steps * (F_CPU / 40000) / MAXIMUM_FEEDRATE_X) << 8; - if (c_limit_calc > c_limit) - c_limit = c_limit_calc; - // check Y axis - c_limit_calc = ( (dda->y_delta * (UM_PER_STEP_Y * 2400L)) / dda->total_steps * (F_CPU / 40000) / MAXIMUM_FEEDRATE_Y) << 8; - if (c_limit_calc > c_limit) - c_limit = c_limit_calc; - // check Z axis - c_limit_calc = ( (dda->z_delta * (UM_PER_STEP_Z * 2400L)) / dda->total_steps * (F_CPU / 40000) / MAXIMUM_FEEDRATE_Z) << 8; - if (c_limit_calc > c_limit) - c_limit = c_limit_calc; - - #ifdef ACCELERATION_REPRAP - // c is initial step time in IOclk ticks - dda->c = (move_duration / startpoint.F) << 8; - if (dda->c < c_limit) - dda->c = c_limit; - dda->end_c = (move_duration / target->F) << 8; - if (dda->end_c < c_limit) - dda->end_c = c_limit; - - if (debug_flags & DEBUG_DDA) - sersendf_P(PSTR(",md:%lu,c:%lu"), move_duration, dda->c >> 8); - - if (dda->c != dda->end_c) { - uint32_t stF = startpoint.F / 4; - uint32_t enF = target->F / 4; - // now some constant acceleration stuff, courtesy of http://www.embedded.com/columns/technicalinsights/56800129?printable=true - uint32_t ssq = (stF * stF); - uint32_t esq = (enF * enF); - int32_t dsq = (int32_t) (esq - ssq) / 4; - - uint8_t msb_ssq = msbloc(ssq); - uint8_t msb_tot = msbloc(dda->total_steps); - - // the raw equation WILL overflow at high step rates, but 64 bit math routines take waay too much space - // at 65536 mm/min (1092mm/s), ssq/esq overflows, and dsq is also close to overflowing if esq/ssq is small - // but if ssq-esq is small, ssq/dsq is only a few bits - // we'll have to do it a few different ways depending on the msb locations of each - if ((msb_tot + msb_ssq) <= 30) { - // we have room to do all the multiplies first - if (debug_flags & DEBUG_DDA) - serial_writechar('A'); - dda->n = ((int32_t) (dda->total_steps * ssq) / dsq) + 1; - } - else if (msb_tot >= msb_ssq) { - // total steps has more precision - if (debug_flags & DEBUG_DDA) - serial_writechar('B'); - dda->n = (((int32_t) dda->total_steps / dsq) * (int32_t) ssq) + 1; - } - else { - // otherwise - if (debug_flags & DEBUG_DDA) - serial_writechar('C'); - dda->n = (((int32_t) ssq / dsq) * (int32_t) dda->total_steps) + 1; - } - - if (debug_flags & DEBUG_DDA) - sersendf_P(PSTR("\n{DDA:CA end_c:%lu, n:%ld, md:%lu, ssq:%lu, esq:%lu, dsq:%lu, msbssq:%u, msbtot:%u}\n"), dda->end_c >> 8, dda->n, move_duration, ssq, esq, dsq, msb_ssq, msb_tot); - - dda->accel = 1; - } - else - dda->accel = 0; - #elif defined ACCELERATION_RAMPING - // add the last bit of dda->total_steps to always round up - dda->ramp_steps = dda->total_steps / 2 + (dda->total_steps & 1); - dda->step_no = 0; - // c is initial step time in IOclk ticks - dda->c = ACCELERATION_STEEPNESS << 8; - dda->c_min = (move_duration / target->F) << 8; - if (dda->c_min < c_limit) - dda->c_min = c_limit; - dda->n = 1; - dda->ramp_state = RAMP_UP; - #elif defined ACCELERATION_TEMPORAL - dda->x_counter = dda->x_step_interval = move_duration / dda->x_delta; - dda->y_counter = dda->y_step_interval = move_duration / dda->y_delta; - dda->z_counter = dda->z_step_interval = move_duration / dda->z_delta; - - dda->c = dda->x_step_interval; - if (dda->y_step_interval < dda->c) - dda->c = dda->y_step_interval; - if (dda->z_step_interval < dda->c) - dda->c = dda->z_step_interval; - - dda->c <<= 8; - #else - dda->c = (move_duration / target->F) << 8; - if (dda->c < c_limit) - dda->c = c_limit; - #endif - } - - if (debug_flags & DEBUG_DDA) - serial_writestr_P(PSTR("] }\n")); - - // next dda starts where we finish - memcpy(&startpoint, target, sizeof(TARGET)); - // E is always relative, reset it here -} - -/* - Start a prepared DDA -*/ - -void dda_start(DDA *dda) { - // called from interrupt context: keep it simple! - if (dda->nullmove) { - // just change speed? - current_position.F = dda->endpoint.F; - // keep dda->live = 0 - } - else { -/* if (dda->waitfor_temp) { - #ifndef REPRAP_HOST_COMPATIBILITY - serial_writestr_P(PSTR("Waiting for target temp\n")); - #endif - } - else {*/ - // ensure steppers are ready to go - steptimeout = 0; - power_on(); - x_enable(); - y_enable(); - if (dda->z_delta) - z_enable(); - - // set direction outputs - x_direction(dda->x_direction); - y_direction(dda->y_direction); - z_direction(dda->z_direction); - - -// } - - // ensure this dda starts - dda->live = 1; - - // set timeout for first step - setTimer(dda->c >> 8); - } -} - -/* - STEP -*/ - -void dda_step(DDA *dda) { - // called from interrupt context! keep it as simple as possible - uint8_t did_step = 0; - - #ifdef ACCELERATION_TEMPORAL - if (dda->x_counter <= 0) { - if ((current_position.X != dda->endpoint.X) /* && - (x_max() != dda->x_direction) && (x_min() == dda->x_direction) */) { - x_step(); - if (dda->x_direction) - current_position.X++; - else - current_position.X--; - } - dda->x_counter += dda->x_step_interval; - dda->x_delta--; - } - if (dda->y_counter <= 0) { - if ((current_position.Y != dda->endpoint.Y) /* && - (y_max() != dda->y_direction) && (y_min() == dda->y_direction) */) { - y_step(); - if (dda->y_direction) - current_position.Y++; - else - current_position.Y--; - } - dda->y_counter += dda->y_step_interval; - dda->y_delta--; - } - if (dda->z_counter <= 0) { - if ((current_position.Z != dda->endpoint.Z) /* && - (z_max() != dda->z_direction) && (z_min() == dda->z_direction) */) { - z_step(); - if (dda->z_direction) - current_position.Z++; - else - current_position.Z--; - } - dda->z_counter += dda->z_step_interval; - dda->z_delta--; - } - - #else - if ((current_position.X != dda->endpoint.X) /* && - (x_max() != dda->x_direction) && (x_min() == dda->x_direction) */) { - dda->x_counter -= dda->x_delta; - if (dda->x_counter < 0) { - x_step(); - did_step = 1; - if (dda->x_direction) - current_position.X++; - else - current_position.X--; - - dda->x_counter += dda->total_steps; - } - } - - if ((current_position.Y != dda->endpoint.Y) /* && - (y_max() != dda->y_direction) && (y_min() == dda->y_direction) */) { - dda->y_counter -= dda->y_delta; - if (dda->y_counter < 0) { - y_step(); - did_step = 1; - if (dda->y_direction) - current_position.Y++; - else - current_position.Y--; - - dda->y_counter += dda->total_steps; - } - } - - if ((current_position.Z != dda->endpoint.Z) /* && - (z_max() != dda->z_direction) && (z_min() == dda->z_direction) */) { - dda->z_counter -= dda->z_delta; - if (dda->z_counter < 0) { - z_step(); - did_step = 1; - if (dda->z_direction) - current_position.Z++; - else - current_position.Z--; - - dda->z_counter += dda->total_steps; - } - } - #endif - - #if STEP_INTERRUPT_INTERRUPTIBLE - // since we have sent steps to all the motors that will be stepping and the rest of this function isn't so time critical, - // this interrupt can now be interruptible - // however we must ensure that we don't step again while computing the below, so disable *this* interrupt but allow others to fire -// disableTimerInterrupt(); - sei(); - #endif - - #ifdef ACCELERATION_REPRAP - // linear acceleration magic, courtesy of http://www.embedded.com/columns/technicalinsights/56800129?printable=true - if (dda->accel) { - if ( - ((dda->n > 0) && (dda->c > dda->end_c)) || - ((dda->n < 0) && (dda->c < dda->end_c)) - ) { - dda->c = (int32_t) dda->c - ((int32_t) (dda->c * 2) / dda->n); - dda->n += 4; - } - else if (dda->c != dda->end_c) { - dda->c = dda->end_c; - } - // else we are already at target speed - } - #endif - #ifdef ACCELERATION_RAMPING - // - algorithm courtesy of http://www.embedded.com/columns/technicalinsights/56800129?printable=true - // - for simplicity, taking even/uneven number of steps into account dropped - // - number of steps moved is always accurate, speed might be one step off - switch (dda->ramp_state) { - case RAMP_UP: - case RAMP_MAX: - if (dda->step_no >= dda->ramp_steps) { - // RAMP_UP: time to decelerate before reaching maximum speed - // RAMP_MAX: time to decelerate - dda->ramp_state = RAMP_DOWN; - dda->n = -((int32_t)2) - dda->n; - } - if (dda->ramp_state == RAMP_MAX) - break; - case RAMP_DOWN: - dda->n += 4; - // be careful of signedness! - dda->c = (int32_t)dda->c - ((int32_t)(dda->c * 2) / dda->n); - if (dda->c <= dda->c_min) { - // maximum speed reached - dda->c = dda->c_min; - dda->ramp_state = RAMP_MAX; - dda->ramp_steps = dda->total_steps - dda->step_no; - } - break; - } - dda->step_no++; - #endif - #ifdef ACCELERATION_TEMPORAL - dda->c = dda->x_counter; - if (dda->y_counter < dda->c) - dda->c = dda->y_counter; - if (dda->z_counter < dda->c) - dda->c = dda->z_counter; - - if (dda->x_delta) - dda->x_counter -= dda->c; - if (dda->y_delta) - dda->y_counter -= dda->c; - if (dda->z_delta) - dda->z_counter -= dda->c; - if ( - (dda->x_delta > 0) || - (dda->y_delta > 0) || - (dda->z_delta > 0) - did_step = 1; - - dda->c <<= 8; - #endif - - if (did_step) { - // we stepped, reset timeout - steptimeout = 0; - - // if we could do anything at all, we're still running - // otherwise, must have finished - } - else { - dda->live = 0; - // linear acceleration code doesn't alter F during a move, so we must update it here - // in theory, we *could* update F every step, but that would require a divide in interrupt context which should be avoided if at all possible - current_position.F = dda->endpoint.F; - } - - setTimer(dda->c >> 8); - - // turn off step outputs, hopefully they've been on long enough by now to register with the drivers - // if not, too bad. or insert a (very!) small delay here, or fire up a spare timer or something. - // we also hope that we don't step before the drivers register the low- limit maximum speed if you think this is a problem. - unstep(); -} diff --git a/dda.h b/dda.h deleted file mode 100644 index 88d584c..0000000 --- a/dda.h +++ /dev/null @@ -1,125 +0,0 @@ -#ifndef _DDA_H -#define _DDA_H - -#include - -#include "config.h" - -/* - enums -*/ -// wether we accelerate, run at full speed, break down, etc. -typedef enum { - RAMP_UP, - RAMP_MAX, - RAMP_DOWN -} ramp_state_t; - -/* - types -*/ - -// target is simply a point in space/time -typedef struct { - int32_t X; - int32_t Y; - int32_t Z; - uint32_t F; -} TARGET; - -// this is a digital differential analyser data struct -typedef struct { - // this is where we should finish - TARGET endpoint; - - union { - struct { - // status fields - uint8_t nullmove :1; - uint8_t live :1; - #ifdef ACCELERATION_REPRAP - uint8_t accel :1; - #endif - - // wait for temperature to stabilise flag - uint8_t waitfor_temp :1; - - // directions - uint8_t x_direction :1; - uint8_t y_direction :1; - uint8_t z_direction :1; - }; - uint8_t allflags; // used for clearing all flags - }; - - // distances - uint32_t x_delta; - uint32_t y_delta; - uint32_t z_delta; - - // bresenham counters - int32_t x_counter; - int32_t y_counter; - int32_t z_counter; - - // total number of steps: set to max(x_delta, y_delta, z_delta) - uint32_t total_steps; - - // linear acceleration variables: c and end_c are 24.8 fixed point timer values, n is the tracking variable - uint32_t c; - #ifdef ACCELERATION_REPRAP - uint32_t end_c; - int32_t n; - #endif - #ifdef ACCELERATION_RAMPING - // start of down-ramp, intitalized with total_steps / 2 - uint32_t ramp_steps; - // counts actual steps done - uint32_t step_no; - // 24.8 fixed point timer value, maximum speed - uint32_t c_min; - // tracking variable - int32_t n; - ramp_state_t ramp_state; - #endif - #ifdef ACCELERATION_TEMPORAL - uint32_t x_step_interval; - uint32_t y_step_interval; - uint32_t z_step_interval; - #endif -} DDA; - -/* - variables -*/ - -// steptimeout is set to zero when we step, and increases over time so we can turn the motors off when they've been idle for a while -extern uint8_t steptimeout; - -// startpoint holds the endpoint of the most recently created DDA, so we know where the next one created starts -// could also be called last_endpoint -extern TARGET startpoint; - -// current_position holds the machine's current position. this is only updated when we step, or when G92 (set home) is received. -extern TARGET current_position; - -/* - methods -*/ - -uint32_t approx_distance( uint32_t dx, uint32_t dy ) __attribute__ ((hot)); -uint32_t approx_distance_3( uint32_t dx, uint32_t dy, uint32_t dz ) __attribute__ ((hot)); - -// const because return value is always the same given the same v -const uint8_t msbloc (uint32_t v) __attribute__ ((const)); - -// create a DDA -void dda_create(DDA *dda, TARGET *target); - -// start a created DDA (called from timer interrupt) -void dda_start(DDA *dda) __attribute__ ((hot)); - -// DDA takes one step (called from timer interrupt) -void dda_step(DDA *dda) __attribute__ ((hot)); - -#endif /* _DDA_H */ diff --git a/dda_queue.c b/dda_queue.c deleted file mode 100644 index b13cabc..0000000 --- a/dda_queue.c +++ /dev/null @@ -1,130 +0,0 @@ -#include "dda_queue.h" - -#include -#include - -#include "config.h" -#include "timer.h" -#include "serial.h" -#include "sermsg.h" -#include "delay.h" -#include "sersendf.h" -#include "clock.h" - -uint8_t mb_head = 0; -uint8_t mb_tail = 0; -DDA movebuffer[MOVEBUFFER_SIZE] __attribute__ ((__section__ (".bss"))); - -uint8_t queue_full() { - return (((mb_tail - mb_head - 1) & (MOVEBUFFER_SIZE - 1)) == 0)?255:0; -} - -uint8_t queue_empty() { - return ((mb_tail == mb_head) && (movebuffer[mb_tail].live == 0))?255:0; -} - -// ------------------------------------------------------- -// This is the one function called by the timer interrupt. -// It calls a few other functions, though. -// ------------------------------------------------------- -void queue_step() { - // do our next step - if (movebuffer[mb_tail].live) { - setTimer(movebuffer[mb_tail].c >> 8); - movebuffer[mb_tail].live = 0; - - #if STEP_INTERRUPT_INTERRUPTIBLE - sei(); - #endif - } - else { - // NOTE: dda_step makes this interrupt interruptible after steps have been sent but before new speed is calculated. - dda_step(&(movebuffer[mb_tail])); - } - - - // fall directly into dda_start instead of waiting for another step - // the dda dies not directly after its last step, but when the timer fires and there's no steps to do - if (movebuffer[mb_tail].live == 0) - next_move(); -} - -void enqueue(TARGET *t) { - // don't call this function when the queue is full, but just in case, wait for a move to complete and free up the space for the passed target - while (queue_full()) - delay(WAITING_DELAY); - - uint8_t h = mb_head + 1; - h &= (MOVEBUFFER_SIZE - 1); - - if (t != NULL) { - dda_create(&movebuffer[h], t); - } - else { - // it's a wait for temp - movebuffer[h].waitfor_temp = 1; - movebuffer[h].nullmove = 0; - #if (F_CPU & 0xFF000000) == 0 - // set "step" timeout to 1 second - movebuffer[h].c = F_CPU << 8; - #else - // set "step" timeout to maximum - movebuffer[h].c = 0xFFFFFF00; - #endif - } - - mb_head = h; - - // fire up in case we're not running yet - if (movebuffer[mb_tail].live == 0) - next_move(); -} - -// sometimes called from normal program execution, sometimes from interrupt context -void next_move() { - if (queue_empty() == 0) { - // next item - uint8_t t = mb_tail + 1; - t &= (MOVEBUFFER_SIZE - 1); - if (movebuffer[t].waitfor_temp) { - #ifndef REPRAP_HOST_COMPATIBILITY - serial_writestr_P(PSTR("Waiting for target temp\n")); - #endif - movebuffer[t].live = 1; - setTimer(movebuffer[t].c >> 8); - } - else { - dda_start(&movebuffer[t]); - } - mb_tail = t; - } - else - setTimer(0); -} - -void print_queue() { - sersendf_P(PSTR("Q%d/%d%c\n"), mb_tail, mb_head, (queue_full()?'F':(queue_empty()?'E':' '))); -} - -void queue_flush() { - // save interrupt flag - uint8_t sreg = SREG; - - // disable interrupts - cli(); - - // flush queue - mb_tail = mb_head; - movebuffer[mb_head].live = 0; - - // restore interrupt flag - SREG = sreg; -} - -void queue_wait() { - for (;queue_empty() == 0;) { - ifclock(CLOCK_FLAG_10MS) { - clock_10ms(); - } - } -} diff --git a/dda_queue.h b/dda_queue.h deleted file mode 100644 index 26f0aa2..0000000 --- a/dda_queue.h +++ /dev/null @@ -1,42 +0,0 @@ -#ifndef _DDA_QUEUE -#define _DDA_QUEUE - -#include "dda.h" - -/* - variables -*/ - -// this is the ringbuffer that holds the current and pending moves. -extern uint8_t mb_head; -extern uint8_t mb_tail; -extern DDA movebuffer[MOVEBUFFER_SIZE]; - -/* - methods -*/ - -// queue status methods -uint8_t queue_full(void); -uint8_t queue_empty(void); - -// take one step -void queue_step(void); - -// add a new target to the queue -// t == NULL means add a wait for target temp to the queue -void enqueue(TARGET *t); - -// called from step timer when current move is complete -void next_move(void) __attribute__ ((hot)); - -// print queue status -void print_queue(void); - -// flush the queue for eg; emergency stop -void queue_flush(void); - -// wait for queue to empty -void queue_wait(void); - -#endif /* _DDA_QUEUE */ diff --git a/debug.c b/debug.c deleted file mode 100644 index ad81914..0000000 --- a/debug.c +++ /dev/null @@ -1,3 +0,0 @@ -#include "debug.h" - -volatile uint8_t debug_flags; diff --git a/debug.h b/debug.h deleted file mode 100644 index e96e268..0000000 --- a/debug.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef _DEBUG_H -#define _DEBUG_H - -#include - -#ifdef DEBUG - #define DEBUG_PID 1 - #define DEBUG_DDA 2 - #define DEBUG_POSITION 4 -#else - // by setting these to zero, the compiler should optimise the relevant code out - #define DEBUG_PID 0 - #define DEBUG_DDA 0 - #define DEBUG_POSITION 0 -#endif - -#define DEBUG_ECHO 128 - -extern volatile uint8_t debug_flags; - -#endif /* _DEBUG_H */ diff --git a/delay.c b/delay.c deleted file mode 100644 index fb3e2bb..0000000 --- a/delay.c +++ /dev/null @@ -1,50 +0,0 @@ -#include "delay.h" - -#include "watchdog.h" - -// delay( microseconds ) -void delay(uint32_t delay) { - wd_reset(); - while (delay > 65535) { - delayMicrosecondsInterruptible(65533); - delay -= 65535; - wd_reset(); - } - delayMicrosecondsInterruptible(delay & 0xFFFF); - wd_reset(); -} - -// delay_ms( milliseconds ) -void delay_ms(uint32_t delay) { - wd_reset(); - while (delay > 65) { - delayMicrosecondsInterruptible(64999); - delay -= 65; - wd_reset(); - } - delayMicrosecondsInterruptible(delay * 1000); - wd_reset(); -} - -void delayMicrosecondsInterruptible(uint16_t us) -{ - // for a one-microsecond delay, simply return. the overhead - // of the function call yields a delay of approximately 1 1/8 us. - if (--us == 0) - return; - - // the following loop takes a quarter of a microsecond (4 cycles) - // per iteration, so execute it four times for each microsecond of - // delay requested. - us <<= 2; - - // account for the time taken in the preceeding commands. - us -= 2; - - // busy wait - __asm__ __volatile__ ("1: sbiw %0,1" "\n\t" // 2 cycles - "brne 1b" : - "=w" (us) : - "0" (us) // 2 cycles - ); -} diff --git a/delay.h b/delay.h deleted file mode 100644 index 83aaf36..0000000 --- a/delay.h +++ /dev/null @@ -1,15 +0,0 @@ -#ifndef _DELAY_H -#define _DELAY_H - -#include - -#define WAITING_DELAY 10 MS - -void delay(uint32_t delay); - -void delay_ms(uint32_t delay); - -#define delay_us(d) delayMicrosecondsInterruptible(d) -void delayMicrosecondsInterruptible(unsigned int us); - -#endif /* _DELAY_H */ diff --git a/eeprom.c b/eeprom.c new file mode 100644 index 0000000..29a85b2 --- /dev/null +++ b/eeprom.c @@ -0,0 +1,151 @@ +// This file has been prepared for Doxygen automatic documentation generation. +/*! \file ******************************************************************** +* +* Atmel Corporation +* +* \li File: eeprom.c +* \li Compiler: IAR EWAAVR 3.10c +* \li Support mail: avr@atmel.com +* +* \li Supported devices: All devices with split EEPROM erase/write +* capabilities can be used. +* The example is written for ATmega48. +* +* \li AppNote: AVR103 - Using the EEPROM Programming Modes. +* +* \li Description: Example on how to use the split EEPROM erase/write +* capabilities in e.g. ATmega48. All EEPROM +* programming modes are tested, i.e. Erase+Write, +* Erase-only and Write-only. +* +* $Revision: 1.6 $ +* $Date: Friday, February 11, 2005 07:16:44 UTC $ +****************************************************************************/ +#include +#include + +/* These EEPROM bits have different names on different devices. */ +#ifndef EEPE + #define EEPE EEWE //!< EEPROM program/write enable. + #define EEMPE EEMWE //!< EEPROM master program/write enable. +#endif + +/* These two are unfortunately not defined in the device include files. */ +#define EEPM1 5 //!< EEPROM Programming Mode Bit 1. +#define EEPM0 4 //!< EEPROM Programming Mode Bit 0. + +/* Define to reduce code size. */ +#define EEPROM_IGNORE_SELFPROG //!< Remove SPM flag polling. + +/*! \brief Read byte from EEPROM. + * + * This function reads one byte from a given EEPROM address. + * + * \note The CPU is halted for 4 clock cycles during EEPROM read. + * + * \param addr EEPROM address to read from. + * \return The byte read from the EEPROM address. + */ +unsigned char eeprom_get_char( unsigned int addr ) +{ + do {} while( EECR & (1< 0; size--) { + checksum = (checksum << 1) || (checksum >> 7); + checksum += *source; + eeprom_put_char(destination++, *(source++)); + } + eeprom_put_char(destination, checksum); +} + +int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size) { + unsigned char data, checksum = 0; + for(; size > 0; size--) { + data = eeprom_get_char(source++); + checksum = (checksum << 1) || (checksum >> 7); + checksum += data; + *(destination++) = data; + } + return(checksum == eeprom_get_char(source)); +} + +// end of file diff --git a/eeprom.h b/eeprom.h new file mode 100644 index 0000000..1769903 --- /dev/null +++ b/eeprom.h @@ -0,0 +1,9 @@ +#ifndef eeprom_h +#define eeprom_h + +char eeprom_get_char(unsigned int addr); +void eeprom_put_char(unsigned int addr, char new_value); +void memcpy_to_eeprom_with_checksum(unsigned int destination, char *source, unsigned int size); +int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size); + +#endif diff --git a/fuses.h b/fuses.h deleted file mode 100644 index 829d54e..0000000 --- a/fuses.h +++ /dev/null @@ -1,11 +0,0 @@ -#ifdef FUSES - #if defined (__AVR_ATmega644__) || defined (__AVR_ATmega644P__) - FUSES = { - .low = FUSE_CKSEL3 & FUSE_SUT0, - .high = FUSE_SPIEN & FUSE_BOOTSZ0 & FUSE_BOOTSZ1, - .extended = EFUSE_DEFAULT, - }; - #else - #warning No fuse definitions for this chip in fuses.h! - #endif -#endif /* FUSES */ \ No newline at end of file diff --git a/gcode.c b/gcode.c new file mode 100644 index 0000000..7efd8fc --- /dev/null +++ b/gcode.c @@ -0,0 +1,467 @@ +/* + gcode.c - rs274/ngc parser. + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + +/* This code is inspired by the Arduino GCode Interpreter by Mike Ellery and the NIST RS274/NGC Interpreter + by Kramer, Proctor and Messina. */ + +#include "gcode.h" +#include +#include +#include "nuts_bolts.h" +#include +#include "settings.h" +#include "motion_control.h" +#include "spindle_control.h" +#include "errno.h" +#include "serial_protocol.h" + +#define MM_PER_INCH (25.4) + +#define NEXT_ACTION_DEFAULT 0 +#define NEXT_ACTION_DWELL 1 +#define NEXT_ACTION_GO_HOME 2 + +#define MOTION_MODE_SEEK 0 // G0 +#define MOTION_MODE_LINEAR 1 // G1 +#define MOTION_MODE_CW_ARC 2 // G2 +#define MOTION_MODE_CCW_ARC 3 // G3 +#define MOTION_MODE_CANCEL 4 // G80 + +#define PATH_CONTROL_MODE_EXACT_PATH 0 +#define PATH_CONTROL_MODE_EXACT_STOP 1 +#define PATH_CONTROL_MODE_CONTINOUS 2 + +#define PROGRAM_FLOW_RUNNING 0 +#define PROGRAM_FLOW_PAUSED 1 +#define PROGRAM_FLOW_COMPLETED 2 + +#define SPINDLE_DIRECTION_CW 0 +#define SPINDLE_DIRECTION_CCW 1 + +typedef struct { + uint8_t status_code; + + uint8_t motion_mode; /* {G0, G1, G2, G3, G80} */ + uint8_t inverse_feed_rate_mode; /* G93, G94 */ + uint8_t inches_mode; /* 0 = millimeter mode, 1 = inches mode {G20, G21} */ + uint8_t absolute_mode; /* 0 = relative motion, 1 = absolute motion {G90, G91} */ + uint8_t program_flow; + int spindle_direction; + double feed_rate, seek_rate; /* Millimeters/second */ + double position[3]; /* Where the interpreter considers the tool to be at this point in the code */ + uint8_t tool; + int16_t spindle_speed; /* RPM/100 */ + uint8_t plane_axis_0, + plane_axis_1, + plane_axis_2; // The axes of the selected plane +} parser_state_t; +static parser_state_t gc; + +#define FAIL(status) gc.status_code = status; + +int read_double(char *line, // <- string: line of RS274/NGC code being processed + int *char_counter, // <- pointer to a counter for position on the line + double *double_ptr); // <- pointer to double to be read + +int next_statement(char *letter, double *double_ptr, char *line, int *char_counter); + + +void select_plane(uint8_t axis_0, uint8_t axis_1, uint8_t axis_2) +{ + gc.plane_axis_0 = axis_0; + gc.plane_axis_1 = axis_1; + gc.plane_axis_2 = axis_2; +} + +void gc_init() { + memset(&gc, 0, sizeof(gc)); + gc.feed_rate = settings.default_feed_rate/60; + gc.seek_rate = settings.default_seek_rate/60; + select_plane(X_AXIS, Y_AXIS, Z_AXIS); + gc.absolute_mode = TRUE; +} + +inline float to_millimeters(double value) { + return(gc.inches_mode ? (value * MM_PER_INCH) : value); +} + +// Find the angle in radians of deviance from the positive y axis. negative angles to the left of y-axis, +// positive to the right. +double theta(double x, double y) +{ + double theta = atan(x/fabs(y)); + if (y>0) { + return(theta); + } else { + if (theta>0) + { + return(M_PI-theta); + } else { + return(-M_PI-theta); + } + } +} + +// Executes one line of 0-terminated G-Code. The line is assumed to contain only uppercase +// characters and signed floating point values (no whitespace). +uint8_t gc_execute_line(char *line) { + int char_counter = 0; + char letter; + double value; + double unit_converted_value; + double inverse_feed_rate = -1; // negative inverse_feed_rate means no inverse_feed_rate specified + int radius_mode = FALSE; + + uint8_t absolute_override = FALSE; /* 1 = absolute motion for this block only {G53} */ + uint8_t next_action = NEXT_ACTION_DEFAULT; /* The action that will be taken by the parsed line */ + + double target[3], offset[3]; + + double p = 0, r = 0; + int int_value; + + clear_vector(target); + clear_vector(offset); + + gc.status_code = GCSTATUS_OK; + + // Disregard comments and block delete + if (line[0] == '(') { return(gc.status_code); } + if (line[0] == '/') { char_counter++; } // ignore block delete + + // If the line starts with an '$' it is a configuration-command + if (line[0] == '$') { + // Parameter lines are on the form '$4=374.3' or '$' to dump current settings + char_counter = 1; + if(line[char_counter] == 0) { settings_dump(); return(GCSTATUS_OK); } + read_double(line, &char_counter, &p); + if(line[char_counter++] != '=') { return(GCSTATUS_UNSUPPORTED_STATEMENT); } + read_double(line, &char_counter, &value); + if(line[char_counter] != 0) { return(GCSTATUS_UNSUPPORTED_STATEMENT); } + settings_store_setting(p, value); + return(gc.status_code); + } + + /* We'll handle this as g-code. First: parse all statements */ + + // Pass 1: Commands + while(next_statement(&letter, &value, line, &char_counter)) { + int_value = trunc(value); + switch(letter) { + case 'G': + switch(int_value) { + case 0: gc.motion_mode = MOTION_MODE_SEEK; break; + case 1: gc.motion_mode = MOTION_MODE_LINEAR; break; +#ifdef __AVR_ATmega328P__ + case 2: gc.motion_mode = MOTION_MODE_CW_ARC; break; + case 3: gc.motion_mode = MOTION_MODE_CCW_ARC; break; +#endif + case 4: next_action = NEXT_ACTION_DWELL; break; + case 17: select_plane(X_AXIS, Y_AXIS, Z_AXIS); break; + case 18: select_plane(X_AXIS, Z_AXIS, Y_AXIS); break; + case 19: select_plane(Y_AXIS, Z_AXIS, X_AXIS); break; + case 20: gc.inches_mode = TRUE; break; + case 21: gc.inches_mode = FALSE; break; + case 28: case 30: next_action = NEXT_ACTION_GO_HOME; break; + case 53: absolute_override = TRUE; break; + case 80: gc.motion_mode = MOTION_MODE_CANCEL; break; + case 90: gc.absolute_mode = TRUE; break; + case 91: gc.absolute_mode = FALSE; break; + case 93: gc.inverse_feed_rate_mode = TRUE; break; + case 94: gc.inverse_feed_rate_mode = FALSE; break; + default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT); + } + break; + + case 'M': + switch(int_value) { + case 0: case 1: gc.program_flow = PROGRAM_FLOW_PAUSED; break; + case 2: case 30: case 60: gc.program_flow = PROGRAM_FLOW_COMPLETED; break; + case 3: gc.spindle_direction = 1; break; + case 4: gc.spindle_direction = -1; break; + case 5: gc.spindle_direction = 0; break; + default: FAIL(GCSTATUS_UNSUPPORTED_STATEMENT); + } + break; + case 'T': gc.tool = trunc(value); break; + } + if(gc.status_code) { break; } + } + + // If there were any errors parsing this line, we will return right away with the bad news + if (gc.status_code) { return(gc.status_code); } + + char_counter = 0; + clear_vector(offset); + memcpy(target, gc.position, sizeof(target)); // i.e. target = gc.position + + // Pass 2: Parameters + while(next_statement(&letter, &value, line, &char_counter)) { + int_value = trunc(value); + unit_converted_value = to_millimeters(value); + switch(letter) { + case 'F': + if (gc.inverse_feed_rate_mode) { + inverse_feed_rate = unit_converted_value; // seconds per motion for this motion only + } else { + if (gc.motion_mode == MOTION_MODE_SEEK) { + gc.seek_rate = unit_converted_value/60; + } else { + gc.feed_rate = unit_converted_value/60; // millimeters pr second + } + } + break; + case 'I': case 'J': case 'K': offset[letter-'I'] = unit_converted_value; break; + case 'P': p = value; break; + case 'R': r = unit_converted_value; radius_mode = TRUE; break; + case 'S': gc.spindle_speed = value; break; + case 'X': case 'Y': case 'Z': + if (gc.absolute_mode || absolute_override) { + target[letter - 'X'] = unit_converted_value; + } else { + target[letter - 'X'] += unit_converted_value; + } + break; + } + } + + // If there were any errors parsing this line, we will return right away with the bad news + if (gc.status_code) { return(gc.status_code); } + + // Update spindle state + if (gc.spindle_direction) { + spindle_run(gc.spindle_direction, gc.spindle_speed); + } else { + spindle_stop(); + } + + // Perform any physical actions + switch (next_action) { + case NEXT_ACTION_GO_HOME: mc_go_home(); break; + case NEXT_ACTION_DWELL: mc_dwell(trunc(p*1000)); break; + case NEXT_ACTION_DEFAULT: + switch (gc.motion_mode) { + case MOTION_MODE_CANCEL: break; + case MOTION_MODE_SEEK: + mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, FALSE); + break; + case MOTION_MODE_LINEAR: + mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], + (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); + break; +#ifdef __AVR_ATmega328P__ + case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: + if (radius_mode) { + /* + We need 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 our 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 + + */ + + // Calculate the change in position along each selected axis + double x = target[gc.plane_axis_0]-gc.position[gc.plane_axis_0]; + double y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; + + clear_vector(offset); + double h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d) + // If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any + // real CNC, and thus - for practical reasons - we will terminate promptly: + if(isnan(h_x2_div_d)) { FAIL(GCSTATUS_FLOATING_POINT_ERROR); return(gc.status_code); } + // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) + if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } + + /* 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 this center | Clockwise circles with this center will have + will have > 180 deg of angular travel | < 180 deg of angular travel, which is a good thing! + \ | / + center of arc when h_x2_div_d is positive -> x <----- | -----> x <- center of arc when h_x2_div_d is negative + | + | + + C <-- Current position */ + + + // 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 (r < 0) { h_x2_div_d = -h_x2_div_d; } + // Complete the operation by calculating the actual center of the arc + offset[gc.plane_axis_0] = (x-(y*h_x2_div_d))/2; + offset[gc.plane_axis_1] = (y+(x*h_x2_div_d))/2; + } + + /* + This segment 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)) + + */ + + // calculate the theta (angle) of the current point + double theta_start = theta(-offset[gc.plane_axis_0], -offset[gc.plane_axis_1]); + // calculate the theta (angle) of the target point + double theta_end = theta(target[gc.plane_axis_0] - offset[gc.plane_axis_0] - gc.position[gc.plane_axis_0], + target[gc.plane_axis_1] - offset[gc.plane_axis_1] - gc.position[gc.plane_axis_1]); + // ensure that the difference is positive so that we have clockwise travel + if (theta_end < theta_start) { theta_end += 2*M_PI; } + double angular_travel = theta_end-theta_start; + // Invert angular motion if the g-code wanted a counterclockwise arc + if (gc.motion_mode == MOTION_MODE_CCW_ARC) { + angular_travel = angular_travel-2*M_PI; + } + // Find the radius + double radius = hypot(offset[gc.plane_axis_0], offset[gc.plane_axis_1]); + // Calculate the motion along the depth axis of the helix + double depth = target[gc.plane_axis_2]-gc.position[gc.plane_axis_2]; + // Trace the arc + mc_arc(theta_start, angular_travel, radius, depth, gc.plane_axis_0, gc.plane_axis_1, gc.plane_axis_2, + (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode, + gc.position); + // Finish off with a line to make sure we arrive exactly where we think we are + mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], + (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.feed_rate, gc.inverse_feed_rate_mode); + break; +#endif + } + } + + // As far as the parser is concerned, the position is now == target. In reality the + // motion control system might still be processing the action and the real tool position + // in any intermediate location. + memcpy(gc.position, target, sizeof(double)*3); // gc.position[] = target[]; + return(gc.status_code); +} + +// Parses the next statement and leaves the counter on the first character following +// the statement. Returns 1 if there was a statements, 0 if end of string was reached +// or there was an error (check state.status_code). +int next_statement(char *letter, double *double_ptr, char *line, int *char_counter) { + if (line[*char_counter] == 0) { + return(0); // No more statements + } + + *letter = line[*char_counter]; + if((*letter < 'A') || (*letter > 'Z')) { + FAIL(GCSTATUS_EXPECTED_COMMAND_LETTER); + return(0); + } + (*char_counter)++; + if (!read_double(line, char_counter, double_ptr)) { + return(0); + }; + return(1); +} + +int read_double(char *line, //!< string: line of RS274/NGC code being processed + int *char_counter, //!< pointer to a counter for position on the line + double *double_ptr) //!< pointer to double to be read +{ + char *start = line + *char_counter; + char *end; + + *double_ptr = strtod(start, &end); + if(end == start) { + FAIL(GCSTATUS_BAD_NUMBER_FORMAT); + return(0); + }; + + *char_counter = end - line; + return(1); +} + +/* + Intentionally not supported: + + - Canned cycles + - Tool radius compensation + - A,B,C-axes + - Multiple coordinate systems + - Evaluation of expressions + - Variables + - Multiple home locations + - Probing + - Override control + + group 0 = {G10, G28, G30, G92, G92.1, G92.2, G92.3} (Non modal G-codes) + group 8 = {M7, M8, M9} coolant (special case: M7 and M8 may be active at the same time) + group 9 = {M48, M49} enable/disable feed and speed override switches + group 12 = {G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3} coordinate system selection + group 13 = {G61, G61.1, G64} path control mode +*/ + diff --git a/gcode.h b/gcode.h new file mode 100644 index 0000000..8adb836 --- /dev/null +++ b/gcode.h @@ -0,0 +1,38 @@ +/* + gcode.c - rs274/ngc parser. + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + + +#ifndef gcode_h +#define gcode_h +#include + +#define GCSTATUS_OK 0 +#define GCSTATUS_BAD_NUMBER_FORMAT 1 +#define GCSTATUS_EXPECTED_COMMAND_LETTER 2 +#define GCSTATUS_UNSUPPORTED_STATEMENT 3 +#define GCSTATUS_FLOATING_POINT_ERROR 4 + +// Initialize the parser +void gc_init(); + +// Execute one block of rs275/ngc/g-code +uint8_t gc_execute_line(char *line); + +#endif diff --git a/gcode_parse.c b/gcode_parse.c deleted file mode 100644 index eb2120f..0000000 --- a/gcode_parse.c +++ /dev/null @@ -1,361 +0,0 @@ -#include "gcode_parse.h" - -#include - -#include "serial.h" -#include "sermsg.h" -#include "dda_queue.h" -#include "debug.h" -#include "sersendf.h" - -#include "gcode_process.h" - -/* - Switch user friendly values to coding friendly values - - This also affects the possible build volume. We have +/- 2^31 numbers available and as we internally measure position in steps and use a precision factor of 1000, this translates into a possible range of - - 2^31 mm / STEPS_PER_MM_x / 1000 - - for each axis. For a M6 threaded rod driven machine and 1/16 microstepping this evaluates to - - 2^31 mm / 200 / 16 / 1000 = 671 mm, - - which is about the worst case we have. All other machines have a bigger build volume. -*/ - -#define STEPS_PER_M_X ((uint32_t) ((STEPS_PER_MM_X * 1000.0) + 0.5)) -#define STEPS_PER_M_Y ((uint32_t) ((STEPS_PER_MM_Y * 1000.0) + 0.5)) -#define STEPS_PER_M_Z ((uint32_t) ((STEPS_PER_MM_Z * 1000.0) + 0.5)) - -/* - mm -> inch conversion -*/ - -#define STEPS_PER_IN_X ((uint32_t) ((25.4 * STEPS_PER_MM_X) + 0.5)) -#define STEPS_PER_IN_Y ((uint32_t) ((25.4 * STEPS_PER_MM_Y) + 0.5)) -#define STEPS_PER_IN_Z ((uint32_t) ((25.4 * STEPS_PER_MM_Z) + 0.5)) - -uint8_t last_field = 0; - -#define crc(a, b) (a ^ b) - -decfloat read_digit __attribute__ ((__section__ (".bss"))); - -GCODE_COMMAND next_target __attribute__ ((__section__ (".bss"))); - -/* - utility functions -*/ -extern const uint32_t powers[]; // defined in sermsg.c -const int32_t rounding[DECFLOAT_EXP_MAX] = {0, 5, 50, 500, 5000, 50000, 500000}; - -static int32_t decfloat_to_int(decfloat *df, int32_t multiplicand, uint32_t denominator) { - int32_t r = df->mantissa; - uint8_t e = df->exponent; - - // e=1 means we've seen a decimal point but no digits after it, and e=2 means we've seen a decimal point with one digit so it's too high by one if not zero - if (e) - e--; - - int32_t rnew1 = r * (multiplicand / denominator); - if (e) - { - int32_t rnew2 = r * (multiplicand % denominator) / denominator; - r = rnew1 + rnew2; - - r = (r + rounding[e]) / powers[e]; - } - else - { - int32_t rnew2 = (r * (multiplicand % denominator) + (denominator / 2)) / denominator; - r = rnew1 + rnew2; - } - - return df->sign ? -r : r; -} - -/***************************************************************************\ -* Character Received - add it to our command * -\***************************************************************************/ - -void gcode_parse_char(uint8_t c) { - #ifdef ASTERISK_IN_CHECKSUM_INCLUDED - if (next_target.seen_checksum == 0) - next_target.checksum_calculated = crc(next_target.checksum_calculated, c); - #endif - - // uppercase - if (c >= 'a' && c <= 'z') - c &= ~32; - - // process previous field - if (last_field) { - // check if we're seeing a new field or end of line - // any character will start a new field, even invalid/unknown ones - if ((c >= 'A' && c <= 'Z') || c == '*' || (c == 10) || (c == 13)) { - switch (last_field) { - case 'G': - next_target.G = read_digit.mantissa; - if (debug_flags & DEBUG_ECHO) - serwrite_uint8(next_target.G); - break; - case 'M': - next_target.M = read_digit.mantissa; - if (debug_flags & DEBUG_ECHO) - serwrite_uint8(next_target.M); - break; - case 'X': - if (next_target.option_inches) - next_target.target.X = decfloat_to_int(&read_digit, STEPS_PER_IN_X, 1); - else - next_target.target.X = decfloat_to_int(&read_digit, STEPS_PER_M_X, 1000); - if (debug_flags & DEBUG_ECHO) - serwrite_int32(next_target.target.X); - break; - case 'Y': - if (next_target.option_inches) - next_target.target.Y = decfloat_to_int(&read_digit, STEPS_PER_IN_Y, 1); - else - next_target.target.Y = decfloat_to_int(&read_digit, STEPS_PER_M_Y, 1000); - if (debug_flags & DEBUG_ECHO) - serwrite_int32(next_target.target.Y); - break; - case 'Z': - if (next_target.option_inches) - next_target.target.Z = decfloat_to_int(&read_digit, STEPS_PER_IN_Z, 1); - else - next_target.target.Z = decfloat_to_int(&read_digit, STEPS_PER_M_Z, 1000); - if (debug_flags & DEBUG_ECHO) - serwrite_int32(next_target.target.Z); - break; - case 'F': - // just use raw integer, we need move distance and n_steps to convert it to a useful value, so wait until we have those to convert it - if (next_target.option_inches) - next_target.target.F = decfloat_to_int(&read_digit, 254, 10); - else - next_target.target.F = decfloat_to_int(&read_digit, 1, 1); - if (debug_flags & DEBUG_ECHO) - serwrite_uint32(next_target.target.F); - break; - case 'S': - // if this is temperature, multiply by 4 to convert to quarter-degree units - // cosmetically this should be done in the temperature section, - // but it takes less code, less memory and loses no precision if we do it here instead - if ((next_target.M == 104) || (next_target.M == 109)) - next_target.S = decfloat_to_int(&read_digit, 4, 1); - // if this is heater PID stuff, multiply by PID_SCALE because we divide by PID_SCALE later on - else if ((next_target.M >= 130) && (next_target.M <= 132)) - next_target.S = decfloat_to_int(&read_digit, PID_SCALE, 1); - else - next_target.S = decfloat_to_int(&read_digit, 1, 1); - if (debug_flags & DEBUG_ECHO) - serwrite_uint16(next_target.S); - break; - case 'P': - // if this is dwell, multiply by 1000 to convert seconds to milliseconds - if (next_target.G == 4) - next_target.P = decfloat_to_int(&read_digit, 1000, 1); - else - next_target.P = decfloat_to_int(&read_digit, 1, 1); - if (debug_flags & DEBUG_ECHO) - serwrite_uint16(next_target.P); - break; - case 'T': - next_target.T = read_digit.mantissa; - if (debug_flags & DEBUG_ECHO) - serwrite_uint8(next_target.T); - break; - case 'N': - next_target.N = decfloat_to_int(&read_digit, 1, 1); - if (debug_flags & DEBUG_ECHO) - serwrite_uint32(next_target.N); - break; - case '*': - next_target.checksum_read = decfloat_to_int(&read_digit, 1, 1); - if (debug_flags & DEBUG_ECHO) - serwrite_uint8(next_target.checksum_read); - break; - } - // reset for next field - last_field = 0; - read_digit.sign = read_digit.mantissa = read_digit.exponent = 0; - } - } - - // skip comments - if (next_target.seen_semi_comment == 0 && next_target.seen_parens_comment == 0) { - // new field? - if ((c >= 'A' && c <= 'Z') || c == '*') { - last_field = c; - if (debug_flags & DEBUG_ECHO) - serial_writechar(c); - } - - // process character - switch (c) { - // each currently known command is either G or M, so preserve previous G/M unless a new one has appeared - // FIXME: same for T command - case 'G': - next_target.seen_G = 1; - next_target.seen_M = 0; - next_target.M = 0; - break; - case 'M': - next_target.seen_M = 1; - next_target.seen_G = 0; - next_target.G = 0; - break; - case 'X': - next_target.seen_X = 1; - break; - case 'Y': - next_target.seen_Y = 1; - break; - case 'Z': - next_target.seen_Z = 1; - break; - case 'F': - next_target.seen_F = 1; - break; - case 'S': - next_target.seen_S = 1; - break; - case 'P': - next_target.seen_P = 1; - break; - case 'T': - next_target.seen_T = 1; - break; - case 'N': - next_target.seen_N = 1; - break; - case '*': - next_target.seen_checksum = 1; - break; - - // comments - case ';': - next_target.seen_semi_comment = 1; - break; - case '(': - next_target.seen_parens_comment = 1; - break; - - // now for some numeracy - case '-': - read_digit.sign = 1; - // force sign to be at start of number, so 1-2 = -2 instead of -12 - read_digit.exponent = 0; - read_digit.mantissa = 0; - break; - case '.': - if (read_digit.exponent == 0) - read_digit.exponent = 1; - break; - #ifdef DEBUG - case ' ': - case '\t': - case 10: - case 13: - // ignore - break; - #endif - - default: - // can't do ranges in switch..case, so process actual digits here. - if ( c >= '0' - && c <= '9' - && read_digit.mantissa < (DECFLOAT_MANT_MAX / 10) - && read_digit.exponent < DECFLOAT_EXP_MAX ) { - // this is simply mantissa = (mantissa * 10) + atoi(c) in different clothes - read_digit.mantissa = (read_digit.mantissa << 3) + (read_digit.mantissa << 1) + (c - '0'); - if (read_digit.exponent) - read_digit.exponent++; - } - #ifdef DEBUG - else { - // invalid - serial_writechar('?'); - serial_writechar(c); - serial_writechar('?'); - } - #endif - } - } else if ( next_target.seen_parens_comment == 1 && c == ')') - next_target.seen_parens_comment = 0; // recognize stuff after a (comment) - - - #ifndef ASTERISK_IN_CHECKSUM_INCLUDED - if (next_target.seen_checksum == 0) - next_target.checksum_calculated = crc(next_target.checksum_calculated, c); - #endif - - // end of line - if ((c == 10) || (c == 13)) { - if (debug_flags & DEBUG_ECHO) - serial_writechar(c); - - if ( - #ifdef REQUIRE_LINENUMBER - (next_target.N >= next_target.N_expected) && (next_target.seen_N == 1) - #else - 1 - #endif - ) { - if ( - #ifdef REQUIRE_CHECKSUM - ((next_target.checksum_calculated == next_target.checksum_read) && (next_target.seen_checksum == 1)) - #else - ((next_target.checksum_calculated == next_target.checksum_read) || (next_target.seen_checksum == 0)) - #endif - ) { - // process - serial_writestr_P(PSTR("ok ")); - process_gcode_command(); - serial_writestr_P(PSTR("\n")); - - // expect next line number - if (next_target.seen_N == 1) - next_target.N_expected = next_target.N + 1; - } - else { - sersendf_P(PSTR("rs %ld Expected checksum %d\n"), next_target.N_expected, next_target.checksum_calculated); - request_resend(); - } - } - else { - sersendf_P(PSTR("rs %ld Expected line number %ld\n"), next_target.N_expected, next_target.N_expected); - request_resend(); - } - - // reset variables - next_target.seen_X = next_target.seen_Y = next_target.seen_Z = \ - next_target.seen_F = next_target.seen_S = \ - next_target.seen_P = next_target.seen_T = next_target.seen_N = \ - next_target.seen_M = next_target.seen_checksum = next_target.seen_semi_comment = \ - next_target.seen_parens_comment = next_target.checksum_read = \ - next_target.checksum_calculated = 0; - last_field = 0; - read_digit.sign = read_digit.mantissa = read_digit.exponent = 0; - - // assume a G1 by default - next_target.seen_G = 1; - next_target.G = 1; - - if (next_target.option_relative) { - next_target.target.X = next_target.target.Y = next_target.target.Z = 0; - } - } -} - -/***************************************************************************\ -* Request a resend of the current line - used from various places. * -* Relies on the global variable next_target.N being valid. * -\***************************************************************************/ - -void request_resend(void) { - serial_writestr_P(PSTR("rs ")); - serwrite_uint8(next_target.N); - serial_writechar('\n'); -} diff --git a/gcode_parse.h b/gcode_parse.h deleted file mode 100644 index 1ed5415..0000000 --- a/gcode_parse.h +++ /dev/null @@ -1,82 +0,0 @@ -#ifndef _GCODE_PARSE_H -#define _GCODE_PARSE_H - -#include - -#include "dda.h" - -// wether the asterisk (checksum-command) is included for checksum calculation -// undefined for RepRap host software -//#define ASTERISK_IN_CHECKSUM_INCLUDED - -// wether to insist on N line numbers -// if not defined, N's are completely ignored -//#define REQUIRE_LINENUMBER - -// wether to insist on a checksum -//#define REQUIRE_CHECKSUM - -// this is a very crude decimal-based floating point structure. -// a real floating point would at least have signed exponent. -// max (DECFLOAT_EXP_MAX - 1) digits after decimal point, because point is -// counted as well -#define DECFLOAT_EXP_WIDTH 3 -#define DECFLOAT_EXP_MAX ((1 << DECFLOAT_EXP_WIDTH) - 1) -#define DECFLOAT_MANT_WIDTH (32 - 1 - DECFLOAT_EXP_WIDTH) -#define DECFLOAT_MANT_MAX (((uint32_t)1 << DECFLOAT_MANT_WIDTH) - 1) -typedef struct { - uint32_t sign :1; - uint32_t mantissa :DECFLOAT_MANT_WIDTH; - uint32_t exponent :DECFLOAT_EXP_WIDTH; -} decfloat; - -// this holds all the possible data from a received command -typedef struct { - union { - struct { - uint8_t seen_G :1; - uint8_t seen_M :1; - uint8_t seen_X :1; - uint8_t seen_Y :1; - uint8_t seen_Z :1; - uint8_t seen_F :1; - uint8_t seen_S :1; - - uint8_t seen_P :1; - uint8_t seen_T :1; - uint8_t seen_N :1; - uint8_t seen_checksum :1; - uint8_t seen_semi_comment :1; - uint8_t seen_parens_comment :1; - uint8_t option_relative :1; - uint8_t option_inches :1; - }; - uint16_t flags; - }; - - uint8_t G; - uint8_t M; - TARGET target; - - int16_t S; - uint16_t P; - - uint8_t T; - - uint32_t N; - uint32_t N_expected; - - uint8_t checksum_read; - uint8_t checksum_calculated; -} GCODE_COMMAND; - -// the command being processed -extern GCODE_COMMAND next_target; - -// accept the next character and process it -void gcode_parse_char(uint8_t c); - -// uses the global variable next_target.N -void request_resend(void); - -#endif /* _GCODE_PARSE_H */ diff --git a/gcode_process.c b/gcode_process.c deleted file mode 100644 index 9469bda..0000000 --- a/gcode_process.c +++ /dev/null @@ -1,293 +0,0 @@ -#include "gcode_process.h" - -#include - -#include "gcode_parse.h" - -#include "dda_queue.h" -#include "delay.h" -#include "serial.h" -#include "sermsg.h" -#include "timer.h" -#include "sersendf.h" -#include "pinio.h" -#include "debug.h" -#include "clock.h" -#include "watchdog.h" - -// the current tool -uint8_t tool; -// the tool to be changed when we get an M6 -uint8_t next_tool; - - -/* -public functions -*/ - -void zero_x(void) { - TARGET t = startpoint; - t.X = 0; - t.F = SEARCH_FEEDRATE_X; - enqueue(&t); -} - -void zero_y(void) { - TARGET t = startpoint; - t.Y = 0; - t.F = SEARCH_FEEDRATE_X; - enqueue(&t); -} - -void zero_z(void) { - TARGET t = startpoint; - t.Z = 0; - t.F = SEARCH_FEEDRATE_Z; - enqueue(&t); -} - -/**************************************************************************** -* * -* Command Received - process it * -* * -****************************************************************************/ - -void process_gcode_command() { - uint32_t backup_f; - - // convert relative to absolute - if (next_target.option_relative) { - next_target.target.X += startpoint.X; - next_target.target.Y += startpoint.Y; - next_target.target.Z += startpoint.Z; - } - - if (next_target.seen_T) { - next_tool = next_target.T; - } - - if (next_target.seen_G) { - uint8_t axisSelected = 0; - switch (next_target.G) { - // G0 - rapid, unsynchronised motion - // since it would be a major hassle to force the dda to not synchronise, just provide a fast feedrate and hope it's close enough to what host expects - case 0: - backup_f = next_target.target.F; - next_target.target.F = MAXIMUM_FEEDRATE_X * 2; - enqueue(&next_target.target); - next_target.target.F = backup_f; - break; - - // G1 - synchronised motion - case 1: - enqueue(&next_target.target); - break; - - // G2 - Arc Clockwise - // unimplemented - - // G3 - Arc Counter-clockwise - // unimplemented - - // G4 - Dwell - case 4: - // wait for all moves to complete - queue_wait(); - // delay - for (;next_target.P > 0;next_target.P--) { - ifclock(CLOCK_FLAG_10MS) { - clock_10ms(); - } - delay_ms(1); - } - break; - - // G20 - inches as units - case 20: - next_target.option_inches = 1; - break; - - // G21 - mm as units - case 21: - next_target.option_inches = 0; - break; - - // G30 - go home via point - case 30: - enqueue(&next_target.target); - // no break here, G30 is move and then go home - - // G28 - go home - case 28: - queue_wait(); - - if (next_target.seen_X) { - zero_x(); - axisSelected = 1; - } - if (next_target.seen_Y) { - zero_y(); - axisSelected = 1; - } - if (next_target.seen_Z) { - zero_z(); - axisSelected = 1; - } - - break; - - // G90 - absolute positioning - case 90: - next_target.option_relative = 0; - break; - - // G91 - relative positioning - case 91: - next_target.option_relative = 1; - break; - - // G92 - set home - case 92: - // wait for queue to empty - queue_wait(); - - if (next_target.seen_X) { - startpoint.X = current_position.X = next_target.target.X; - axisSelected = 1; - } - if (next_target.seen_Y) { - startpoint.Y = current_position.Y = next_target.target.Y; - axisSelected = 1; - } - if (next_target.seen_Z) { - startpoint.Z = current_position.Z = next_target.target.Z; - axisSelected = 1; - } - if (axisSelected == 0) { - startpoint.X = current_position.X = - startpoint.Y = current_position.Y = - startpoint.Z = current_position.Z = 0; - } - break; - - // unknown gcode: spit an error - default: - sersendf_P(PSTR("E: Bad G-code %d"), next_target.G); - // newline is sent from gcode_parse after we return - } - } - else if (next_target.seen_M) { - switch (next_target.M) { - // M2- program end - case 2: - timer_stop(); - queue_flush(); - x_disable(); - y_disable(); - z_disable(); - power_off(); - for (;;) - wd_reset(); - break; - - // M6- tool change - case 6: - tool = next_tool; - break; - - // M110- set line number - case 110: - next_target.N_expected = next_target.S - 1; - break; - // M111- set debug level - #ifdef DEBUG - case 111: - debug_flags = next_target.S; - break; - #endif - // M112- immediate stop - case 112: - timer_stop(); - queue_flush(); - power_off(); - break; - // M113- extruder PWM - // M114- report XYZEF to host - case 114: - sersendf_P(PSTR("X:%ld,Y:%ld,Z:%ld,F:%ld"), current_position.X, current_position.Y, current_position.Z, current_position.F); - // newline is sent from gcode_parse after we return - break; - // M115- capabilities string - case 115: - sersendf_P(PSTR("FIRMWARE_NAME:FiveD_on_Arduino FIRMWARE_URL:http%%3A//github.com/triffid/FiveD_on_Arduino/ PROTOCOL_VERSION:1.0 MACHINE_TYPE:Mendel EXTRUDER_COUNT:%d TEMP_SENSOR_COUNT:%d HEATER_COUNT:%d"), 1, 0, 0); - // newline is sent from gcode_parse after we return - break; - - // M190- power on - case 190: - power_on(); - x_enable(); - y_enable(); - z_enable(); - steptimeout = 0; - break; - // M191- power off - case 191: - x_disable(); - y_disable(); - z_disable(); - power_off(); - break; - - #ifdef DEBUG - // M140- echo off - case 140: - debug_flags &= ~DEBUG_ECHO; - serial_writestr_P(PSTR("Echo off")); - // newline is sent from gcode_parse after we return - break; - // M141- echo on - case 141: - debug_flags |= DEBUG_ECHO; - serial_writestr_P(PSTR("Echo on")); - // newline is sent from gcode_parse after we return - break; - - // DEBUG: return current position, end position, queue - case 250: - sersendf_P(PSTR("{X:%ld,Y:%ld,Z:%ld,F:%lu,c:%lu}\t{X:%ld,Y:%ld,Z:%ld,F:%lu,c:%lu}\t"), current_position.X, current_position.Y, current_position.Z, current_position.F, movebuffer[mb_tail].c, movebuffer[mb_tail].endpoint.X, movebuffer[mb_tail].endpoint.Y, movebuffer[mb_tail].endpoint.Z, movebuffer[mb_tail].endpoint.F, - #ifdef ACCELERATION_REPRAP - movebuffer[mb_tail].end_c - #else - movebuffer[mb_tail].c - #endif - ); - - print_queue(); - break; - - // DEBUG: read arbitrary memory location - case 253: - if (next_target.seen_P == 0) - next_target.P = 1; - for (; next_target.P; next_target.P--) { - serwrite_hex8(*(volatile uint8_t *)(next_target.S)); - next_target.S++; - } - // newline is sent from gcode_parse after we return - break; - - // DEBUG: write arbitrary memory locatiom - case 254: - sersendf_P(PSTR("%x:%x->%x"), next_target.S, *(volatile uint8_t *)(next_target.S), next_target.P); - (*(volatile uint8_t *)(next_target.S)) = next_target.P; - // newline is sent from gcode_parse after we return - break; - #endif /* DEBUG */ - // unknown mcode: spit an error - default: - sersendf_P(PSTR("E: Bad M-code %d"), next_target.M); - // newline is sent from gcode_parse after we return - } // switch (next_target.M) - } // else if (next_target.seen_M) -} // process_gcode_command() diff --git a/gcode_process.h b/gcode_process.h deleted file mode 100644 index 8023b64..0000000 --- a/gcode_process.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef _GCODE_PROCESS_H -#define _GCODE_PROCESS_H - -#include "gcode_parse.h" - -// the current tool -extern uint8_t tool; -// the tool to be changed when we get an M6 -extern uint8_t next_tool; - -void zero_x(void); -void zero_y(void); -void zero_z(void); - -// this is where we construct a move without a gcode command, useful for gcodes which require multiple moves eg; homing -void SpecialMoveE(int32_t e, uint32_t f); - -// when we have a whole line, feed it to this -void process_gcode_command(void); - -#endif /* _GCODE_PROCESS_H */ diff --git a/main.c b/main.c new file mode 100644 index 0000000..61069cd --- /dev/null +++ b/main.c @@ -0,0 +1,55 @@ +/* + main.c - An embedded CNC Controller with rs274/ngc (g-code) support + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + +#include +#include +#include +#include "planner.h" +#include "stepper.h" +#include "spindle_control.h" +#include "motion_control.h" +#include "gcode.h" +#include "serial_protocol.h" +#include "nuts_bolts.h" +#include "settings.h" +#include "wiring_serial.h" +#include "config.h" +#include +#include + +// #ifndef __AVR_ATmega328P__ +// # error "As of version 0.6 Grbl only supports atmega328p. If you want to run Grbl on an 168 check out 0.51 ('git co v0_51')" +// #endif + +int main(void) +{ + sp_init(); + settings_init(); + plan_init(); + st_init(); + //spindle_init(); + gc_init(); + + for(;;){ + sleep_mode(); // Wait for it ... + sp_process(); // ... process the serial protocol + } + return 0; /* never reached */ +} diff --git a/mendel.c b/mendel.c deleted file mode 100644 index baf07a3..0000000 --- a/mendel.c +++ /dev/null @@ -1,157 +0,0 @@ - -#include -#include - -#include "config.h" -#include "fuses.h" - -#include "serial.h" -#include "dda_queue.h" -#include "dda.h" -#include "gcode_parse.h" -#include "timer.h" -#include "sermsg.h" -#include "debug.h" -#include "sersendf.h" -#include "pinio.h" -#include "arduino.h" -#include "clock.h" -#include "watchdog.h" - -void io_init(void) { - // disable modules we don't use - #ifdef PRR - PRR = MASK(PRTWI) | MASK(PRADC) | MASK(PRSPI); - #elif defined PRR0 - PRR0 = MASK(PRTWI) | MASK(PRADC) | MASK(PRSPI); - #ifdef PRR1 - // don't use USART2 or USART3- leave USART1 for GEN3 and derivatives - PRR1 = MASK(PRUSART3) | MASK(PRUSART2); - #endif - #endif - ACSR = MASK(ACD); - - // setup I/O pins - WRITE(X_STEP_PIN, 0); SET_OUTPUT(X_STEP_PIN); - WRITE(X_DIR_PIN, 0); SET_OUTPUT(X_DIR_PIN); - WRITE(X_MIN_PIN, 1); SET_INPUT(X_MIN_PIN); - #ifdef X_MAX_PIN - WRITE(X_MAX_PIN, 1); SET_INPUT(X_MAX_PIN); - #endif - #ifdef X_ENABLE_PIN - WRITE(X_ENABLE_PIN, 1); SET_OUTPUT(X_ENABLE_PIN); - #endif - - WRITE(Y_STEP_PIN, 0); SET_OUTPUT(Y_STEP_PIN); - WRITE(Y_DIR_PIN, 0); SET_OUTPUT(Y_DIR_PIN); - WRITE(Y_MIN_PIN, 1); SET_INPUT(Y_MIN_PIN); - #ifdef Y_MAX_PIN - WRITE(Y_MAX_PIN, 1); SET_INPUT(Y_MAX_PIN); - #endif - #ifdef Y_ENABLE_PIN - WRITE(Y_ENABLE_PIN, 1); SET_OUTPUT(Y_ENABLE_PIN); - #endif - - WRITE(Z_STEP_PIN, 0); SET_OUTPUT(Z_STEP_PIN); - WRITE(Z_DIR_PIN, 0); SET_OUTPUT(Z_DIR_PIN); - WRITE(Z_MIN_PIN, 1); SET_INPUT(Z_MIN_PIN); - #ifdef Z_MAX_PIN - WRITE(Z_MAX_PIN, 1); SET_INPUT(Z_MAX_PIN); - #endif - #ifdef Z_ENABLE_PIN - WRITE(Z_ENABLE_PIN, 1); SET_OUTPUT(Z_ENABLE_PIN); - #endif - - // setup PWM timers: fast PWM, no prescaler - TCCR0A = MASK(WGM01) | MASK(WGM00); - TCCR0B = MASK(CS00); - TIMSK0 = 0; - OCR0A = 0; - OCR0B = 0; - - TCCR2A = MASK(WGM21) | MASK(WGM20); - TCCR2B = MASK(CS20); - TIMSK2 = 0; - OCR2A = 0; - OCR2B = 0; - - #ifdef TCCR3A - TCCR3A = MASK(WGM31) | MASK(WGM30); - TCCR3B = MASK(CS30); - TIMSK3 = 0; - OCR3A = 0; - OCR3B = 0; - #endif - - #ifdef TCCR4A - TCCR4A = MASK(WGM41) | MASK(WGM40); - TCCR4B = MASK(CS40); - TIMSK4 = 0; - OCR4A = 0; - OCR4B = 0; - #endif - - #ifdef TCCR5A - TCCR5A = MASK(WGM51) | MASK(WGM50); - TCCR5B = MASK(CS50); - TIMSK5 = 0; - OCR5A = 0; - OCR5B = 0; - #endif - - #ifdef STEPPER_ENABLE_PIN - power_off(); - #endif - - // setup SPI - WRITE(SCK, 0); SET_OUTPUT(SCK); - WRITE(MOSI, 1); SET_OUTPUT(MOSI); - WRITE(MISO, 1); SET_INPUT(MISO); - WRITE(SS, 1); SET_OUTPUT(SS); -} - -void init(void) { - // set up watchdog - wd_init(); - - // set up serial - serial_init(); - - // set up inputs and outputs - io_init(); - - // set up timers - timer_init(); - - // set up default feedrate - current_position.F = startpoint.F = next_target.target.F = SEARCH_FEEDRATE_Z; - - // enable interrupts - sei(); - - // reset watchdog - wd_reset(); - - // say hi to host - serial_writestr_P(PSTR("Start\nok\n")); - -} - -int main (void) -{ - init(); - - // main loop - for (;;) - { - // if queue is full, no point in reading chars- host will just have to wait - if ((serial_rxchars() != 0) && (queue_full() == 0)) { - uint8_t c = serial_popchar(); - gcode_parse_char(c); - } - - ifclock(CLOCK_FLAG_10MS) { - clock_10ms(); - } - } -} diff --git a/motion_control.c b/motion_control.c new file mode 100644 index 0000000..11c7dec --- /dev/null +++ b/motion_control.c @@ -0,0 +1,86 @@ +/* + motion_control.c - high level interface for issuing motion commands + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + +#include +#include "settings.h" +#include "motion_control.h" +#include +#include +#include +#include "nuts_bolts.h" +#include "stepper.h" +#include "planner.h" +#include "wiring_serial.h" + + +void mc_dwell(uint32_t milliseconds) +{ + st_synchronize(); + _delay_ms(milliseconds); +} + +// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc, +// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the +// circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining +// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion. +// position is a pointer to a vector representing the current position in millimeters. + +#ifdef __AVR_ATmega328P__ +// The arc is approximated by generating a huge number of tiny, linear segments. The length of each +// segment is configured in settings.mm_per_arc_segment. +void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, + int axis_linear, double feed_rate, int invert_feed_rate, double *position) +{ + int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); + plan_set_acceleration_manager_enabled(FALSE); // disable acceleration management for the duration of the arc + double millimeters_of_travel = hypot(angular_travel*radius, labs(linear_travel)); + if (millimeters_of_travel == 0.0) { return; } + uint16_t segments = ceil(millimeters_of_travel/settings.mm_per_arc_segment); + // Multiply inverse feed_rate to compensate for the fact that this movement is approximated + // by a number of discrete segments. The inverse feed_rate should be correct for the sum of + // all segments. + if (invert_feed_rate) { feed_rate *= segments; } + // The angular motion for each segment + double theta_per_segment = angular_travel/segments; + // The linear motion for each segment + double linear_per_segment = linear_travel/segments; + // Compute the center of this circle + double center_x = position[axis_1]-sin(theta)*radius; + double center_y = position[axis_2]-cos(theta)*radius; + // a vector to track the end point of each segment + double target[3]; + int i; + // Initialize the linear axis + target[axis_linear] = position[axis_linear]; + for (i=0; i<=segments; i++) { + target[axis_linear] += linear_per_segment; + theta += theta_per_segment; + target[axis_1] = center_x+sin(theta)*radius; + target[axis_2] = center_y+cos(theta)*radius; + plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate); + } + plan_set_acceleration_manager_enabled(acceleration_manager_was_enabled); +} +#endif + +void mc_go_home() +{ + st_go_home(); +} diff --git a/motion_control.h b/motion_control.h new file mode 100644 index 0000000..90a05da --- /dev/null +++ b/motion_control.h @@ -0,0 +1,49 @@ +/* + motion_control.h - high level interface for issuing motion commands + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + +#ifndef motion_control_h +#define motion_control_h + +#include +#include "planner.h" + +// Execute linear motion in absolute millimeter coordinates. Feed rate given in millimeters/second +// unless invert_feed_rate is true. Then the feed_rate means that the motion should be completed in +// (1 minute)/feed_rate time. +#define mc_line(x, y, z, feed_rate, invert_feed_rate) plan_buffer_line(x, y, z, feed_rate, invert_feed_rate) +// NOTE: Although this function structurally belongs in this module, there is nothing to do but +// to forward the request to the planner. For efficiency the function is implemented with a macro. + +#ifdef __AVR_ATmega328P__ +// Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc, +// positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the +// circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining +// axis in axis_l which will be the axis for linear travel if you are tracing a helical motion. +void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, + int axis_linear, double feed_rate, int invert_feed_rate, double *position); +#endif + +// Dwell for a couple of time units +void mc_dwell(uint32_t milliseconds); + +// Send the tool home (not implemented) +void mc_go_home(); + +#endif diff --git a/nuts_bolts.h b/nuts_bolts.h new file mode 100644 index 0000000..ca4ecf5 --- /dev/null +++ b/nuts_bolts.h @@ -0,0 +1,35 @@ +/* + motion_control.h - cartesian robot controller. + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + +#ifndef nuts_bolts_h +#define nuts_bolts_h +#include + +#define FALSE 0 +#define TRUE 1 + +#define X_AXIS 0 +#define Y_AXIS 1 +#define Z_AXIS 2 + +#define clear_vector(a) memset(a, 0, sizeof(a)) +#define max(a,b) (((a) > (b)) ? (a) : (b)) + +#endif diff --git a/pinio.c b/pinio.c deleted file mode 100644 index 8d89ed1..0000000 --- a/pinio.c +++ /dev/null @@ -1,20 +0,0 @@ -#include "pinio.h" - -void power_off() { - #ifdef X_ENABLE_PIN - x_disable(); - #endif - #ifdef Y_ENABLE_PIN - y_disable(); - #endif - #ifdef Z_ENABLE_PIN - z_disable(); - #endif - - #ifdef STEPPER_ENABLE_PIN - WRITE(STEPPER_ENABLE_PIN, STEPPER_ENABLE_INVERT ^ 1) - #endif - #ifdef PS_ON_PIN - SET_INPUT(PS_ON_PIN); - #endif -} diff --git a/pinio.h b/pinio.h deleted file mode 100644 index 1c58de2..0000000 --- a/pinio.h +++ /dev/null @@ -1,138 +0,0 @@ -#ifndef _PINIO_H -#define _PINIO_H - -#ifndef X_INVERT_DIR - #define X_INVERT_DIR 0 -#endif -#ifndef X_INVERT_MIN - #define X_INVERT_MIN 0 -#endif -#ifndef X_INVERT_MAX - #define X_INVERT_MAX 0 -#endif -#ifndef X_INVERT_ENABLE - #define X_INVERT_ENABLE 0 -#endif - -#ifndef Y_INVERT_DIR - #define Y_INVERT_DIR 0 -#endif -#ifndef Y_INVERT_MIN - #define Y_INVERT_MIN 0 -#endif -#ifndef Y_INVERT_MAX - #define Y_INVERT_MAX 0 -#endif -#ifndef Y_INVERT_ENABLE - #define Y_INVERT_ENABLE 0 -#endif - -#ifndef Z_INVERT_DIR - #define Z_INVERT_DIR 0 -#endif -#ifndef Z_INVERT_MIN - #define Z_INVERT_MIN 0 -#endif -#ifndef Z_INVERT_MAX - #define Z_INVERT_MAX 0 -#endif -#ifndef Z_INVERT_ENABLE - #define Z_INVERT_ENABLE 0 -#endif - -#ifndef STEPPER_ENABLE_INVERT - #define STEPPER_ENABLE_INVERT 0 -#endif - -/* -Power -*/ - -#ifdef STEPPER_ENABLE_PIN - #define power_on() do { WRITE(STEPPER_ENABLE_PIN, STEPPER_ENABLE_INVERT); SET_OUTPUT(STEPPER_ENABLE_PIN); } while (0) -#elif defined PS_ON_PIN - #define power_on() do { WRITE(PS_ON_PIN, 0); SET_OUTPUT(PS_ON_PIN); } while (0) -#else - #define power_on() do { } while (0) -#endif - -void power_off(void); - -/* -X Stepper -*/ - -#define _x_step(st) WRITE(X_STEP_PIN, st) -#define x_step() _x_step(1); -#define x_direction(dir) WRITE(X_DIR_PIN, dir ^ X_INVERT_DIR) -#define x_min() (READ(X_MIN_PIN)?(X_INVERT_MIN ^ 1):X_INVERT_DIR) -#ifdef X_MAX_PIN - #define x_max() (READ(X_MAX_PIN)?(X_INVERT_MAX ^ 1):X_INVERT_MAX) -#else - #define x_max() (0) -#endif - -/* -Y Stepper -*/ - -#define _y_step(st) WRITE(Y_STEP_PIN, st) -#define y_step() _y_step(1); -#define y_direction(dir) WRITE(Y_DIR_PIN, dir ^ Y_INVERT_DIR) -#define y_min() (READ(Y_MIN_PIN)?(Y_INVERT_MIN ^ 1):Y_INVERT_MIN) -#ifdef Y_MAX_PIN - #define y_max() (READ(Y_MAX_PIN)?(Y_INVERT_MAX ^ 1):Y_INVERT_MAX) -#else - #define y_max() (0) -#endif - -/* -Z Stepper -*/ - -#define _z_step(st) WRITE(Z_STEP_PIN, st) -#define z_step() _z_step(1); -#define z_direction(dir) WRITE(Z_DIR_PIN, dir ^ Z_INVERT_DIR) -#define z_min() (READ(Z_MIN_PIN)?(Z_INVERT_MIN ^ 1):Z_INVERT_MIN) -#ifdef Z_MAX_PIN - #define z_max() (READ(Z_MAX_PIN)?(Z_INVERT_MAX ^ 1):Z_INVERT_MAX) -#else - #define z_max() (0) -#endif - -/* -End Step - All Steppers -(so we don't have to delay in interrupt context) -*/ - - #define unstep() do { _x_step(0); _y_step(0); _z_step(0); } while (0) - -/* -Stepper Enable Pins -*/ - -#ifdef X_ENABLE_PIN - #define x_enable() do { WRITE(X_ENABLE_PIN, X_INVERT_ENABLE); SET_OUTPUT(X_ENABLE_PIN); } while (0) - #define x_disable() do { WRITE(X_ENABLE_PIN, X_INVERT_ENABLE ^ 1); SET_OUTPUT(X_ENABLE_PIN); } while (0) -#else - #define x_enable() do { } while (0) - #define x_disable() do { } while (0) -#endif - -#ifdef Y_ENABLE_PIN - #define y_enable() do { WRITE(Y_ENABLE_PIN, Y_INVERT_ENABLE); SET_OUTPUT(Y_ENABLE_PIN); } while (0) - #define y_disable() do { WRITE(Y_ENABLE_PIN, Y_INVERT_ENABLE ^ 1); SET_OUTPUT(Y_ENABLE_PIN); } while (0) -#else - #define y_enable() do { } while (0) - #define y_disable() do { } while (0) -#endif - -#ifdef Z_ENABLE_PIN - #define z_enable() do { WRITE(Z_ENABLE_PIN, Z_INVERT_ENABLE); SET_OUTPUT(Z_ENABLE_PIN); } while (0) - #define z_disable() do { WRITE(Z_ENABLE_PIN, Z_INVERT_ENABLE ^ 1); SET_OUTPUT(Z_ENABLE_PIN); } while (0) -#else - #define z_enable() do { } while (0) - #define z_disable() do { } while (0) -#endif - -#endif /* _PINIO_H */ diff --git a/planner.c b/planner.c new file mode 100644 index 0000000..9dbe363 --- /dev/null +++ b/planner.c @@ -0,0 +1,421 @@ +/* + planner.c - buffers movement commands and manages the acceleration profile plan + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + +/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */ + +/* + Reasoning behind the mathematics in this module (in the key of 'Mathematica'): + + s == speed, a == acceleration, t == time, d == distance + + Basic definitions: + + Speed[s_, a_, t_] := s + (a*t) + Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t] + + Distance to reach a specific speed with a constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t] + d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance() + + Speed after a given distance of travel with constant acceleration: + + Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t] + m -> Sqrt[2 a d + s^2] + + DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2] + + When to start braking (di) to reach a specified destionation speed (s2) after accelerating + from initial speed s1 without ever stopping at a plateau: + + Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di] + di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance() + + IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a) +*/ + + +#include +#include +#include + +#include "planner.h" +#include "nuts_bolts.h" +#include "stepper.h" +#include "settings.h" +#include "config.h" +#include "wiring_serial.h" + +// The number of linear motions that can be in the plan at any give time +#ifdef __AVR_ATmega328P__ +#define BLOCK_BUFFER_SIZE 20 +#else +#define BLOCK_BUFFER_SIZE 5 +#endif + +static block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions +static volatile int block_buffer_head; // Index of the next block to be pushed +static volatile int block_buffer_tail; // Index of the block to process now + +// The current position of the tool in absolute steps +static int32_t position[3]; + +static uint8_t acceleration_manager_enabled; // Acceleration management active? + +#define ONE_MINUTE_OF_MICROSECONDS 60000000.0 + +// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the +// given acceleration: +inline double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) { + return( + (target_rate*target_rate-initial_rate*initial_rate)/ + (2L*acceleration) + ); +} + +// This function gives you the point at which you must start braking (at the rate of -acceleration) if +// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after +// a total travel of distance. This can be used to compute the intersection point between acceleration and +// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed) + +/* + <- some maximum rate we don't care about + /|\ + / | \ + / | + <- final_rate + / | | + initial_rate -> +----+--+ + ^ ^ + | | + intersection_distance distance */ + +inline double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) { + return( + (2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/ + (4*acceleration) + ); +} + + +// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. +// The factors represent a factor of braking and must be in the range 0.0-1.0. + +/* + +--------+ <- nominal_rate + / \ + nominal_rate*entry_factor -> + \ + | + <- nominal_rate*exit_factor + +-------------+ + time --> +*/ + +void calculate_trapezoid_for_block(block_t *block, double entry_factor, double exit_factor) { + block->initial_rate = ceil(block->nominal_rate*entry_factor); + block->final_rate = ceil(block->nominal_rate*exit_factor); + int32_t acceleration_per_minute = block->rate_delta*ACCELERATION_TICKS_PER_SECOND*60.0; + int32_t accelerate_steps = + ceil(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_minute)); + int32_t decelerate_steps = + floor(estimate_acceleration_distance(block->nominal_rate, block->final_rate, -acceleration_per_minute)); + + // Calculate the size of Plateau of Nominal Rate. + int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps; + + // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will + // have to use intersection_distance() to calculate when to abort acceleration and start braking + // in order to reach the final_rate exactly at the end of this block. + if (plateau_steps < 0) { + accelerate_steps = ceil( + intersection_distance(block->initial_rate, block->final_rate, acceleration_per_minute, block->step_event_count)); + plateau_steps = 0; + } + + block->accelerate_until = accelerate_steps; + block->decelerate_after = accelerate_steps+plateau_steps; +} + +// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the +// acceleration within the allotted distance. +inline double max_allowable_speed(double acceleration, double target_velocity, double distance) { + return( + sqrt(target_velocity*target_velocity-2*acceleration*60*60*distance) + ); +} + +// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks. +// This method will calculate the junction jerk as the euclidean distance between the nominal +// velocities of the respective blocks. +inline double junction_jerk(block_t *before, block_t *after) { + return(sqrt( + pow(before->speed_x-after->speed_x, 2)+ + pow(before->speed_y-after->speed_y, 2)+ + pow(before->speed_z-after->speed_z, 2)) + ); +} + +// Calculate a braking factor to reach baseline speed which is max_jerk/2, e.g. the +// speed under which you cannot exceed max_jerk no matter what you do. +double factor_for_safe_speed(block_t *block) { + return(settings.max_jerk/block->nominal_speed); +} + +// The kernel called by planner_recalculate() when scanning the plan from last to first entry. +void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { + if(!current) { return; } + + double entry_factor = 1.0; + double exit_factor; + if (next) { + exit_factor = next->entry_factor; + } else { + exit_factor = factor_for_safe_speed(current); + } + + // Calculate the entry_factor for the current block. + if (previous) { + // Reduce speed so that junction_jerk is within the maximum allowed + double jerk = junction_jerk(previous, current); + if (jerk > settings.max_jerk) { + entry_factor = (settings.max_jerk/jerk); + } + // If the required deceleration across the block is too rapid, reduce the entry_factor accordingly. + if (entry_factor > exit_factor) { + double max_entry_speed = max_allowable_speed(-settings.acceleration,current->nominal_speed*exit_factor, + current->millimeters); + double max_entry_factor = max_entry_speed/current->nominal_speed; + if (max_entry_factor < entry_factor) { + entry_factor = max_entry_factor; + } + } + } else { + entry_factor = factor_for_safe_speed(current); + } + + // Store result + current->entry_factor = entry_factor; +} + +// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This +// implements the reverse pass. +void planner_reverse_pass() { + auto int8_t block_index = block_buffer_head; + block_t *block[3] = {NULL, NULL, NULL}; + while(block_index != block_buffer_tail) { + block_index--; + if(block_index < 0) { + block_index = BLOCK_BUFFER_SIZE-1; + } + block[2]= block[1]; + block[1]= block[0]; + block[0] = &block_buffer[block_index]; + planner_reverse_pass_kernel(block[0], block[1], block[2]); + } + planner_reverse_pass_kernel(NULL, block[0], block[1]); +} + +// The kernel called by planner_recalculate() when scanning the plan from first to last entry. +void planner_forward_pass_kernel(block_t *previous, block_t *current, block_t *next) { + if(!current) { return; } + // If the previous block is an acceleration block, but it is not long enough to + // complete the full speed change within the block, we need to adjust out entry + // speed accordingly. Remember current->entry_factor equals the exit factor of + // the previous block. + if(previous->entry_factor < current->entry_factor) { + double max_entry_speed = max_allowable_speed(-settings.acceleration, + current->nominal_speed*previous->entry_factor, previous->millimeters); + double max_entry_factor = max_entry_speed/current->nominal_speed; + if (max_entry_factor < current->entry_factor) { + current->entry_factor = max_entry_factor; + } + } +} + +// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This +// implements the forward pass. +void planner_forward_pass() { + int8_t block_index = block_buffer_tail; + block_t *block[3] = {NULL, NULL, NULL}; + + while(block_index != block_buffer_head) { + block[0] = block[1]; + block[1] = block[2]; + block[2] = &block_buffer[block_index]; + planner_forward_pass_kernel(block[0],block[1],block[2]); + block_index = (block_index+1) % BLOCK_BUFFER_SIZE; + } + planner_forward_pass_kernel(block[1], block[2], NULL); +} + +// Recalculates the trapezoid speed profiles for all blocks in the plan according to the +// entry_factor for each junction. Must be called by planner_recalculate() after +// updating the blocks. +void planner_recalculate_trapezoids() { + int8_t block_index = block_buffer_tail; + block_t *current; + block_t *next = NULL; + + while(block_index != block_buffer_head) { + current = next; + next = &block_buffer[block_index]; + if (current) { + calculate_trapezoid_for_block(current, current->entry_factor, next->entry_factor); + } + block_index = (block_index+1) % BLOCK_BUFFER_SIZE; + } + calculate_trapezoid_for_block(next, next->entry_factor, factor_for_safe_speed(next)); +} + +// Recalculates the motion plan according to the following algorithm: +// +// 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_factor) +// so that: +// a. The junction jerk is within the set limit +// b. No speed reduction within one block requires faster deceleration than the one, true constant +// acceleration. +// 2. Go over every block in chronological order and dial down junction speed reduction values if +// a. The speed increase within one block would require faster accelleration than the one, true +// constant acceleration. +// +// When these stages are complete all blocks have an entry_factor that will allow all speed changes to +// be performed using only the one, true constant acceleration, and where no junction jerk is jerkier than +// the set limit. Finally it will: +// +// 3. Recalculate trapezoids for all blocks. + +void planner_recalculate() { + planner_reverse_pass(); + planner_forward_pass(); + planner_recalculate_trapezoids(); +} + +void plan_init() { + block_buffer_head = 0; + block_buffer_tail = 0; + plan_set_acceleration_manager_enabled(TRUE); + clear_vector(position); +} + +void plan_set_acceleration_manager_enabled(int enabled) { + if ((!!acceleration_manager_enabled) != (!!enabled)) { + st_synchronize(); + acceleration_manager_enabled = !!enabled; + } +} + +int plan_is_acceleration_manager_enabled() { + return(acceleration_manager_enabled); +} + +inline void plan_discard_current_block() { + if (block_buffer_head != block_buffer_tail) { + block_buffer_tail = (block_buffer_tail + 1) % BLOCK_BUFFER_SIZE; + } +} + +inline block_t *plan_get_current_block() { + if (block_buffer_head == block_buffer_tail) { return(NULL); } + return(&block_buffer[block_buffer_tail]); +} + +// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in +// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration +// calculation the caller must also provide the physical length of the line in millimeters. +void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate) { + // The target position of the tool in absolute steps + + // Calculate target position in absolute steps + int32_t target[3]; + target[X_AXIS] = lround(x*settings.steps_per_mm[X_AXIS]); + target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); + target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); + + // Calculate the buffer head after we push this byte + int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE; + // If the buffer is full: good! That means we are well ahead of the robot. + // Rest here until there is room in the buffer. + while(block_buffer_tail == next_buffer_head) { sleep_mode(); } + // Prepare to set up new block + block_t *block = &block_buffer[block_buffer_head]; + // Number of steps for each axis + block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); + block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); + block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); + block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); + // Bail if this is a zero-length block + if (block->step_event_count == 0) { return; }; + + double delta_x_mm = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS]; + double delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; + double delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; + block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm)); + + + uint32_t microseconds; + if (!invert_feed_rate) { + microseconds = lround((block->millimeters/feed_rate)*1000000); + } else { + microseconds = lround(ONE_MINUTE_OF_MICROSECONDS/feed_rate); + } + + // Calculate speed in mm/minute for each axis + double multiplier = 60.0*1000000.0/microseconds; + block->speed_x = delta_x_mm * multiplier; + block->speed_y = delta_y_mm * multiplier; + block->speed_z = delta_z_mm * multiplier; + block->nominal_speed = block->millimeters * multiplier; + block->nominal_rate = ceil(block->step_event_count * multiplier); + block->entry_factor = 0.0; + + // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line + // average travel per step event changes. For a line along one axis the travel per step event + // is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both + // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2). + // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed + // specifically for each line to compensate for this phenomenon: + double travel_per_step = block->millimeters/block->step_event_count; + block->rate_delta = ceil( + ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick + travel_per_step); // convert to: acceleration steps/min/acceleration_tick + if (acceleration_manager_enabled) { + // compute a preliminary conservative acceleration trapezoid + double safe_speed_factor = factor_for_safe_speed(block); + calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor); + } else { + block->initial_rate = block->nominal_rate; + block->final_rate = block->nominal_rate; + block->accelerate_until = 0; + block->decelerate_after = block->step_event_count; + block->rate_delta = 0; + } + + // Compute direction bits for this block + block->direction_bits = 0; + if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= (1<direction_bits |= (1<direction_bits |= (1<. +*/ + +// This module is to be considered a sub-module of stepper.c. Please don't include +// this file from any other module. + +#ifndef planner_h +#define planner_h + +#include + +// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in +// the source g-code and may never actually be reached if acceleration management is active. +typedef struct { + // Fields used by the bresenham algorithm for tracing the line + uint32_t steps_x, steps_y, steps_z; // Step count along each axis + uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) + int32_t step_event_count; // The number of step events required to complete this block + uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute + + // Fields used by the motion planner to manage acceleration + double speed_x, speed_y, speed_z; // Nominal mm/minute for each axis + double nominal_speed; // The nominal speed for this block in mm/min + double millimeters; // The total travel of this block in mm + double entry_factor; // The factor representing the change in speed at the start of this trapezoid. + // (The end of the curren speed trapezoid is defined by the entry_factor of the + // next block) + + // Settings for the trapezoid generator + uint32_t initial_rate; // The jerk-adjusted step rate at start of block + uint32_t final_rate; // The minimal rate at exit + int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive) + uint32_t accelerate_until; // The index of the step event on which to stop acceleration + uint32_t decelerate_after; // The index of the step event on which to start decelerating + +} block_t; + +// Initialize the motion plan subsystem +void plan_init(); + +// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in +// millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed +// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes. +void plan_buffer_line(double x, double y, double z, double feed_rate, int invert_feed_rate); + +// Called when the current block is no longer needed. Discards the block and makes the memory +// availible for new blocks. +inline void plan_discard_current_block(); + +// Gets the current block. Returns NULL if buffer empty +inline block_t *plan_get_current_block(); + +// Enables or disables acceleration-management for upcoming blocks +void plan_set_acceleration_manager_enabled(int enabled); + +// Is acceleration-management currently enabled? +int plan_is_acceleration_manager_enabled(); + +#endif \ No newline at end of file diff --git a/readme.textile b/readme.textile new file mode 100644 index 0000000..8dc8e46 --- /dev/null +++ b/readme.textile @@ -0,0 +1,15 @@ +h1. Grbl_for_CCHS - An embedded g-code interpreter and motion-controller for the ATmega644p microcontroller, adapted from Grbl. + +Grbl is a no-compromise, high performance, low cost alternative to parallel-port-based motion control for CNC milling. It will run on a vanilla Arduino (Duemillanove/Uno) as long as it sports an Atmega 328. + +The controller is written in highly optimized C utilizing every clever feature of the AVR-chips to achieve precise timing and asynchronous operation. It is able to maintain more than 30kHz of stable, jitter free control pulses. + +It accepts standards-compliant G-code and has been tested with the output of several CAM tools with no problems. Arcs, circles and helical motion are fully supported - but no support for tool offsets, functions or variables as these are apocryphal and fell into disuse after humans left G-code authoring to machines some time in the 80s. + +Grbl includes full acceleration management with look ahead. That means the controller will look up to 20 motions into the future and plan its velocities ahead to deliver smooth acceleration and jerk-free cornering. + +*Important note for Atmega 168 users:* Grbl used to be compatible with both the older Ardunios running atmega 168 and the newer with 328p. This had to go, as I was unable to fit the acceleration management into the 16k code space of the 168. If you want to run Grbl on an 168 I am still maintaining Grbl 0.51 "in the branch called 'v0_51'":https://github.com/simen/grbl/tree/v0_51. + +*Note for users upgrading from 0.51 to 0.6:* The new version has new and improved default pin-out. If nothing works when you upgrade, that is because the pulse trains are coming from the wrong pins. This is a simple matter of editing config.h – the whole legacy pin assignment is there for you to uncomment. + +_The project was initially inspired by the Arduino GCode Interpreter by Mike Ellery_ diff --git a/serial.c b/serial.c deleted file mode 100644 index 7f20197..0000000 --- a/serial.c +++ /dev/null @@ -1,219 +0,0 @@ -#include "serial.h" - -#include - -#include "config.h" // for XONXOFF -#include "arduino.h" - -#define BUFSIZE 64 -#define BAUD 115200 - -#define ASCII_XOFF 19 -#define ASCII_XON 17 - -volatile uint8_t rxhead = 0; -volatile uint8_t rxtail = 0; -volatile uint8_t rxbuf[BUFSIZE]; - -volatile uint8_t txhead = 0; -volatile uint8_t txtail = 0; -volatile uint8_t txbuf[BUFSIZE]; - -#define buf_canread(buffer) ((buffer ## head - buffer ## tail ) & (BUFSIZE - 1)) -#define buf_canwrite(buffer) ((buffer ## tail - buffer ## head - 1) & (BUFSIZE - 1)) - -#define buf_push(buffer, data) do { buffer ## buf[buffer ## head] = data; buffer ## head = (buffer ## head + 1) & (BUFSIZE - 1); } while (0) -#define buf_pop(buffer, data) do { data = buffer ## buf[buffer ## tail]; buffer ## tail = (buffer ## tail + 1) & (BUFSIZE - 1); } while (0) - -/* - ringbuffer logic: - head = written data pointer - tail = read data pointer - - when head == tail, buffer is empty - when head + 1 == tail, buffer is full - thus, number of available spaces in buffer is (tail - head) & bufsize - - can write: - (tail - head - 1) & (BUFSIZE - 1) - - write to buffer: - buf[head++] = data; head &= (BUFSIZE - 1); - - can read: - (head - tail) & (BUFSIZE - 1) - - read from buffer: - data = buf[tail++]; tail &= (BUFSIZE - 1); -*/ - -#ifdef XONXOFF -#define FLOWFLAG_STATE_XOFF 0 -#define FLOWFLAG_SEND_XON 1 -#define FLOWFLAG_SEND_XOFF 2 -#define FLOWFLAG_STATE_XON 4 -// initially, send an XON -volatile uint8_t flowflags = FLOWFLAG_SEND_XON; -#endif - -void serial_init() -{ -#if BAUD > 38401 - UCSR0A = MASK(U2X0); - UBRR0 = (((F_CPU / 8) / BAUD) - 0.5); -#else - UCSR0A = 0; - UBRR0 = (((F_CPU / 16) / BAUD) - 0.5); -#endif - - UCSR0B = MASK(RXEN0) | MASK(TXEN0); - UCSR0C = MASK(UCSZ01) | MASK(UCSZ00); - - UCSR0B |= MASK(RXCIE0) | MASK(UDRIE0); -} - -/* - Interrupts -*/ - -#ifdef USART_RX_vect -ISR(USART_RX_vect) -#else -ISR(USART0_RX_vect) -#endif -{ - if (buf_canwrite(rx)) - buf_push(rx, UDR0); - else { - uint8_t trash; - - // not reading the character makes the interrupt logic to swamp us with retries, so better read it and throw it away - trash = UDR0; - } - - #ifdef XONXOFF - if (flowflags & FLOWFLAG_STATE_XON && buf_canwrite(rx) <= 16) { - // the buffer has only 16 free characters left, so send an XOFF - // more characters might come in until the XOFF takes effect - flowflags = FLOWFLAG_SEND_XOFF | FLOWFLAG_STATE_XON; - // enable TX interrupt so we can send this character - UCSR0B |= MASK(UDRIE0); - } - #endif -} - -#ifdef USART_UDRE_vect -ISR(USART_UDRE_vect) -#else -ISR(USART0_UDRE_vect) -#endif -{ - #ifdef XONXOFF - if (flowflags & FLOWFLAG_SEND_XON) { - UDR0 = ASCII_XON; - flowflags = FLOWFLAG_STATE_XON; - } - else if (flowflags & FLOWFLAG_SEND_XOFF) { - UDR0 = ASCII_XOFF; - flowflags = FLOWFLAG_STATE_XOFF; - } - else - #endif - if (buf_canread(tx)) - buf_pop(tx, UDR0); - else - UCSR0B &= ~MASK(UDRIE0); -} - -/* - Read -*/ - -uint8_t serial_rxchars() -{ - return buf_canread(rx); -} - -uint8_t serial_popchar() -{ - uint8_t c = 0; - - // it's imperative that we check, because if the buffer is empty and we pop, we'll go through the whole buffer again - if (buf_canread(rx)) - buf_pop(rx, c); - - #ifdef XONXOFF - if ((flowflags & FLOWFLAG_STATE_XON) == 0 && buf_canread(rx) <= 16) { - // the buffer has (BUFSIZE - 16) free characters again, so send an XON - flowflags = FLOWFLAG_SEND_XON; - UCSR0B |= MASK(UDRIE0); - } - #endif - - return c; -} - -/* - Write -*/ - -void serial_writechar(uint8_t data) -{ - // check if interrupts are enabled - if (SREG & MASK(SREG_I)) { - // if they are, we should be ok to block since the tx buffer is emptied from an interrupt - for (;buf_canwrite(tx) == 0;); - buf_push(tx, data); - } - else { - // interrupts are disabled- maybe we're in one? - // anyway, instead of blocking, only write if we have room - if (buf_canwrite(tx)) - buf_push(tx, data); - } - // enable TX interrupt so we can send this character - UCSR0B |= MASK(UDRIE0); -} - -void serial_writeblock(void *data, int datalen) -{ - int i; - - for (i = 0; i < datalen; i++) - serial_writechar(((uint8_t *) data)[i]); -} - -void serial_writestr(uint8_t *data) -{ - uint8_t i = 0, r; - // yes, this is *supposed* to be assignment rather than comparison, so we break when r is assigned zero - while ((r = data[i++])) - serial_writechar(r); -} - -/* - Write from FLASH - - Extensions to output flash memory pointers. This prevents the data to - become part of the .data segment instead of the .code segment. That means - less memory is consumed for multi-character writes. - - For single character writes (i.e. '\n' instead of "\n"), using - serial_writechar() directly is the better choice. -*/ - -void serial_writeblock_P(PGM_P data, int datalen) -{ - int i; - - for (i = 0; i < datalen; i++) - serial_writechar(pgm_read_byte(&data[i])); -} - -void serial_writestr_P(PGM_P data) -{ - uint8_t r, i = 0; - // yes, this is *supposed* to be assignment rather than comparison, so we break when r is assigned zero - while ((r = pgm_read_byte(&data[i++]))) - serial_writechar(r); -} diff --git a/serial.h b/serial.h deleted file mode 100644 index cd6a54f..0000000 --- a/serial.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef _SERIAL_H -#define _SERIAL_H - -#include -#include -#include - -// initialise serial subsystem -void serial_init(void); - -// return number of characters in the receive buffer, and number of spaces in the send buffer -uint8_t serial_rxchars(void); -// uint8_t serial_txchars(void); - -// read one character -uint8_t serial_popchar(void); -// send one character -void serial_writechar(uint8_t data); - -// read/write many characters -// uint8_t serial_recvblock(uint8_t *block, int blocksize); -void serial_writeblock(void *data, int datalen); - -void serial_writestr(uint8_t *data); - -// write from flash -void serial_writeblock_P(PGM_P data, int datalen); -void serial_writestr_P(PGM_P data); - -#endif /* _SERIAL_H */ diff --git a/serial_protocol.c b/serial_protocol.c new file mode 100644 index 0000000..856d9e6 --- /dev/null +++ b/serial_protocol.c @@ -0,0 +1,77 @@ +/* + serial_protocol.c - the serial protocol master control unit + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + +#include +#include "serial_protocol.h" +#include "gcode.h" +#include "wiring_serial.h" +#include "settings.h" +#include "config.h" +#include +#include "nuts_bolts.h" +#include +#define LINE_BUFFER_SIZE 50 + +static char line[LINE_BUFFER_SIZE]; +static uint8_t char_counter; + +void status_message(int status_code) { + switch(status_code) { + case GCSTATUS_OK: + printPgmString(PSTR("ok\n\r")); break; + case GCSTATUS_BAD_NUMBER_FORMAT: + printPgmString(PSTR("error: Bad number format\n\r")); break; + case GCSTATUS_EXPECTED_COMMAND_LETTER: + printPgmString(PSTR("error: Expected command letter\n\r")); break; + case GCSTATUS_UNSUPPORTED_STATEMENT: + printPgmString(PSTR("error: Unsupported statement\n\r")); break; + case GCSTATUS_FLOATING_POINT_ERROR: + printPgmString(PSTR("error: Floating point error\n\r")); break; + default: + printPgmString(PSTR("error: ")); + printInteger(status_code); + printPgmString(PSTR("\n\r")); + } +} + +void sp_init() +{ + beginSerial(BAUD_RATE); + printPgmString(PSTR("\r\nGrbl " GRBL_VERSION)); + printPgmString(PSTR("\r\n")); +} + +void sp_process() +{ + char c; + while((c = serialRead()) != -1) + { + if((char_counter > 0) && ((c == '\n') || (c == '\r'))) { // Line is complete. Then execute! + line[char_counter] = 0; // terminate string + status_message(gc_execute_line(line)); + char_counter = 0; // reset line buffer index + } else if (c <= ' ') { // Throw away whitepace and control characters + } else if (c >= 'a' && c <= 'z') { // Upcase lowercase + line[char_counter++] = c-'a'+'A'; + } else { + line[char_counter++] = c; + } + } +} diff --git a/serial_protocol.h b/serial_protocol.h new file mode 100644 index 0000000..ee9ecf4 --- /dev/null +++ b/serial_protocol.h @@ -0,0 +1,30 @@ +/* + serial_protocol.h - the serial protocol master control unit + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ +#ifndef serial_h +#define serial_h + +// Initialize the serial protocol +void sp_init(); + +// Read command lines from the serial port and execute them as they +// come in. Blocks until the serial buffer is emptied. +void sp_process(); + +#endif diff --git a/sermsg.c b/sermsg.c deleted file mode 100644 index ae2245f..0000000 --- a/sermsg.c +++ /dev/null @@ -1,53 +0,0 @@ -#include "sermsg.h" - -#include "serial.h" - -void serwrite_hex4(uint8_t v) { - v &= 0xF; - if (v < 10) - serial_writechar('0' + v); - else - serial_writechar('A' - 10 + v); -} - -void serwrite_hex8(uint8_t v) { - serwrite_hex4(v >> 4); - serwrite_hex4(v & 0x0F); -} - -void serwrite_hex16(uint16_t v) { - serwrite_hex8(v >> 8); - serwrite_hex8(v & 0xFF); -} - -void serwrite_hex32(uint32_t v) { - serwrite_hex8(v >> 16); - serwrite_hex8(v & 0xFFFF); -} - -const uint32_t powers[] = {1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000}; - -void serwrite_uint32(uint32_t v) { - uint8_t e, t; - - for (e = 9; e > 0; e--) { - if (v >= powers[e]) - break; - } - - do - { - for (t = 0; v >= powers[e]; v -= powers[e], t++); - serial_writechar(t + '0'); - } - while (e--); -} - -void serwrite_int32(int32_t v) { - if (v < 0) { - serial_writechar('-'); - v = -v; - } - - serwrite_uint32(v); -} diff --git a/sermsg.h b/sermsg.h deleted file mode 100644 index 5566f66..0000000 --- a/sermsg.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef _SERMSG_H -#define _SERMSG_H - -#include - -// functions for sending hexadecimal -void serwrite_hex4(uint8_t v); -void serwrite_hex8(uint8_t v); -void serwrite_hex16(uint16_t v); -void serwrite_hex32(uint32_t v); - -// functions for sending decimal -#define serwrite_uint8(v) serwrite_uint32(v) -#define serwrite_int8(v) serwrite_int32(v) -#define serwrite_uint16(v) serwrite_uint32(v) -#define serwrite_int16(v) serwrite_int32(v) - -void serwrite_uint32(uint32_t v); -void serwrite_int32(int32_t v); - -#endif /* _SERMSG_H */ \ No newline at end of file diff --git a/sersendf.c b/sersendf.c deleted file mode 100644 index 4145fe7..0000000 --- a/sersendf.c +++ /dev/null @@ -1,133 +0,0 @@ -#include "sersendf.h" - -#include -#include - -#include "serial.h" -#include "sermsg.h" - -PGM_P str_ox = "0x"; - -void sersendf(char *format, ...) { - va_list args; - va_start(args, format); - - uint16_t i = 0; - uint8_t c, j = 0; - while ((c = format[i++])) { - if (j) { - switch(c) { - case 'l': - j = 4; - break; - case 'u': - if (j == 4) - serwrite_uint32(va_arg(args, uint32_t)); - else - serwrite_uint16(va_arg(args, uint16_t)); - j = 0; - break; - case 'd': - if (j == 4) - serwrite_int32(va_arg(args, int32_t)); - else - serwrite_int16(va_arg(args, int16_t)); - j = 0; - break; - case 'p': - case 'x': - serial_writestr_P(str_ox); - if (j == 4) - serwrite_hex32(va_arg(args, uint32_t)); - else - serwrite_hex16(va_arg(args, uint16_t)); - j = 0; - break; - case 'c': - serial_writechar(va_arg(args, uint16_t)); - j = 0; - break; - case 's': - serial_writestr(va_arg(args, uint8_t *)); - j = 0; - break; - default: - serial_writechar(c); - j = 0; - break; - } - } - else { - if (c == '%') { - j = 2; - } - else { - serial_writechar(c); - } - } - } - va_end(args); -} - -void sersendf_P(PGM_P format, ...) { - va_list args; - va_start(args, format); - - uint16_t i = 0; - uint8_t c = 1, j = 0; - while ((c = pgm_read_byte(&format[i++]))) { - if (j) { - switch(c) { - case 's': - j = 1; - break; - case 'l': - j = 4; - break; - case 'u': - if (j == 4) - serwrite_uint32(va_arg(args, uint32_t)); - else - serwrite_uint16(va_arg(args, uint16_t)); - j = 0; - break; - case 'd': - if (j == 4) - serwrite_int32(va_arg(args, int32_t)); - else - serwrite_int16(va_arg(args, int16_t)); - j = 0; - break; - case 'c': - serial_writechar(va_arg(args, uint16_t)); - j = 0; - break; -/* case 'x': - serial_writestr_P(str_ox); - if (j == 4) - serwrite_hex32(va_arg(args, uint32_t)); - else if (j == 1) - serwrite_hex8(va_arg(args, uint16_t)); - else - serwrite_hex16(va_arg(args, uint16_t)); - j = 0; - break; - case 'p': - serwrite_hex16(va_arg(args, uint16_t));*/ - default: - serial_writechar(c); - j = 0; - break; - } - } - else { - if (c == '%') { - j = 2; - } - else { - serial_writechar(c); - } - } - } - va_end(args); -} diff --git a/sersendf.h b/sersendf.h deleted file mode 100644 index 22aa592..0000000 --- a/sersendf.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef _SERSENDF_H -#define _SERSENDF_H - -#include - -void sersendf(char *format, ...) __attribute__ ((format (printf, 1, 2))); -void sersendf_P(PGM_P format, ...) __attribute__ ((format (printf, 1, 2))); - -#endif /* _SERSENDF_H */ diff --git a/settings.c b/settings.c new file mode 100644 index 0000000..970db68 --- /dev/null +++ b/settings.c @@ -0,0 +1,130 @@ +/* + settings.c - eeprom configuration handling + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + +#include +#include +#include "nuts_bolts.h" +#include "settings.h" +#include "eeprom.h" +#include "wiring_serial.h" +#include + +settings_t settings; + +// Version 1 outdated settings record +typedef struct { + double steps_per_mm[3]; + uint8_t microsteps; + uint8_t pulse_microseconds; + double default_feed_rate; + double default_seek_rate; + uint8_t invert_mask; + double mm_per_arc_segment; +} settings_v1_t; + +void settings_reset() { + settings.steps_per_mm[X_AXIS] = DEFAULT_X_STEPS_PER_MM; + settings.steps_per_mm[Y_AXIS] = DEFAULT_Y_STEPS_PER_MM; + settings.steps_per_mm[Z_AXIS] = DEFAULT_Z_STEPS_PER_MM; + settings.pulse_microseconds = DEFAULT_STEP_PULSE_MICROSECONDS; + settings.default_feed_rate = DEFAULT_FEEDRATE; + settings.default_seek_rate = DEFAULT_RAPID_FEEDRATE; + settings.acceleration = DEFAULT_ACCELERATION; + settings.mm_per_arc_segment = DEFAULT_MM_PER_ARC_SEGMENT; + settings.invert_mask = DEFAULT_STEPPING_INVERT_MASK; + settings.max_jerk = DEFAULT_MAX_JERK; + printPgmString(PSTR("\r\nsettings reset is done.")); +} + +void settings_dump() { + printPgmString(PSTR("$0 = ")); printFloat(settings.steps_per_mm[X_AXIS]); + printPgmString(PSTR(" (steps/mm x)\r\n$1 = ")); printFloat(settings.steps_per_mm[Y_AXIS]); + printPgmString(PSTR(" (steps/mm y)\r\n$2 = ")); printFloat(settings.steps_per_mm[Z_AXIS]); + printPgmString(PSTR(" (steps/mm z)\r\n$3 = ")); printInteger(settings.pulse_microseconds); + printPgmString(PSTR(" (microseconds step pulse)\r\n$4 = ")); printFloat(settings.default_feed_rate); + printPgmString(PSTR(" (mm/min default feed rate)\r\n$5 = ")); printFloat(settings.default_seek_rate); + printPgmString(PSTR(" (mm/min default seek rate)\r\n$6 = ")); printFloat(settings.mm_per_arc_segment); + printPgmString(PSTR(" (mm/arc segment)\r\n$7 = ")); printInteger(settings.invert_mask); + printPgmString(PSTR(" (step port invert mask. binary = ")); printIntegerInBase(settings.invert_mask, 2); + printPgmString(PSTR(")\r\n$8 = ")); printFloat(settings.acceleration); + printPgmString(PSTR(" (acceleration in mm/sec^2)\r\n$9 = ")); printFloat(settings.max_jerk); + printPgmString(PSTR(" (max instant cornering speed change in delta mm/min)")); + printPgmString(PSTR("\r\n'$x=value' to set parameter or just '$' to dump current settings\r\n")); + +} + +void write_settings() { + eeprom_put_char(0, SETTINGS_VERSION); + memcpy_to_eeprom_with_checksum(1, (char*)&settings, sizeof(settings_t)); +} + +int read_settings() { + // Check version-byte of eeprom + uint8_t version = eeprom_get_char(0); + + if (version == SETTINGS_VERSION) { + // Read settings-record and check checksum + if (!(memcpy_from_eeprom_with_checksum((char*)&settings, 1, sizeof(settings_t)))) { + return(FALSE); + } + } else if (version == 1) { + // Migrate from old settings version + if (!(memcpy_from_eeprom_with_checksum((char*)&settings, 1, sizeof(settings_v1_t)))) { + return(FALSE); + } + settings.acceleration = DEFAULT_ACCELERATION; + settings.max_jerk = DEFAULT_MAX_JERK; + } else { + return(FALSE); + } + return(TRUE); +} + +// A helper method to set settings from command line +void settings_store_setting(int parameter, double value) { + switch(parameter) { + case 0: case 1: case 2: + settings.steps_per_mm[parameter] = value; break; + case 3: settings.pulse_microseconds = round(value); break; + case 4: settings.default_feed_rate = value; break; + case 5: settings.default_seek_rate = value; break; + case 6: settings.mm_per_arc_segment = value; break; + case 7: settings.invert_mask = trunc(value); break; + case 8: settings.acceleration = value; break; + case 9: settings.max_jerk = fabs(value); break; + default: + printPgmString(PSTR("Unknown parameter\r\n")); + return; + } + write_settings(); + printPgmString(PSTR("Stored new setting\r\n")); +} + +// Initialize the config subsystem +void settings_init() { + //if(read_settings()) { + //printPgmString(PSTR("'$' to dump current settings\r\n")); + // } else { + printPgmString(PSTR("Using defaults.\r\n")); + settings_reset(); + //write_settings(); + //settings_dump(); + } + diff --git a/settings.h b/settings.h new file mode 100644 index 0000000..5831e0f --- /dev/null +++ b/settings.h @@ -0,0 +1,70 @@ +/* + settings.h - eeprom configuration handling + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + +#ifndef settings_h +#define settings_h + + +#include +#include + +#define GRBL_VERSION "0.6b" + +// Version of the EEPROM data. Will be used to migrate existing data from older versions of Grbl +// when firmware is upgraded. Always stored in byte 0 of eeprom +#define SETTINGS_VERSION 2 + +// Current global settings (persisted in EEPROM from byte 1 onwards) +typedef struct { + double steps_per_mm[3]; + uint8_t microsteps; + uint8_t pulse_microseconds; + double default_feed_rate; + double default_seek_rate; + uint8_t invert_mask; + double mm_per_arc_segment; + double acceleration; + double max_jerk; +} settings_t; +extern settings_t settings; + +// Initialize the configuration subsystem (load settings from EEPROM) +void settings_init(); + +// Print current settings +void settings_dump(); + +// A helper method to set new settings from command line +void settings_store_setting(int parameter, double value); + +// Default settings (used when resetting eeprom-settings) +#define MICROSTEPS 8 +#define DEFAULT_X_STEPS_PER_MM (94.488188976378*MICROSTEPS) +#define DEFAULT_Y_STEPS_PER_MM (94.488188976378*MICROSTEPS) +#define DEFAULT_Z_STEPS_PER_MM (94.488188976378*MICROSTEPS) +#define DEFAULT_STEP_PULSE_MICROSECONDS 30 +#define DEFAULT_MM_PER_ARC_SEGMENT 0.1 +#define DEFAULT_RAPID_FEEDRATE 480.0 // in millimeters per minute +#define DEFAULT_FEEDRATE 480.0 +#define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/100.0) +#define DEFAULT_MAX_JERK 50.0 +#define DEFAULT_STEPPING_INVERT_MASK 0 + +#endif diff --git a/spindle_control.c b/spindle_control.c new file mode 100644 index 0000000..0dc48c8 --- /dev/null +++ b/spindle_control.c @@ -0,0 +1,45 @@ +/* + spindle_control.c - spindle control methods + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + +#include "spindle_control.h" +#include "settings.h" +#include "config.h" + +#include + +void spindle_init() +{ + SPINDLE_ENABLE_DDR |= 1<= 0) { + SPINDLE_DIRECTION_PORT &= ~(1<. +*/ + +#ifndef spindle_control_h +#define spindle_control_h + +#include + +void spindle_init(); +void spindle_run(int direction, uint32_t rpm); +void spindle_stop(); + +#endif diff --git a/stepper.c b/stepper.c new file mode 100644 index 0000000..f432b7a --- /dev/null +++ b/stepper.c @@ -0,0 +1,297 @@ +/* + stepper.c - stepper motor driver: executes motion plans using stepper motors + Part of Grbl + + Copyright (c) 2009-2011 Simen Svale Skogsrud + + Grbl 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 3 of the License, or + (at your option) any later version. + + Grbl is distributed in the hope that 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 Grbl. If not, see . +*/ + +/* The timer calculations of this module informed by the 'RepRap cartesian firmware' by Zack Smith + and Philipp Tiefenbacher. */ + +#include "stepper.h" +#include "config.h" +#include "settings.h" +#include +#include +#include +#include "nuts_bolts.h" +#include +#include "planner.h" +#include "wiring_serial.h" +#include + +// Some useful constants +#define STEP_MASK ((1< +// +// The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta +// during the first block->accelerate_until step_events_completed, then keeps going at constant speed until +// step_events_completed reaches block->decelerate_after after which it decelerates until the trapezoid generator is reset. +// The slope of acceleration is always +/- block->rate_delta and is applied at a constant rate by trapezoid_generator_tick() +// that is called ACCELERATION_TICKS_PER_SECOND times per second. + +void set_step_events_per_minute(uint32_t steps_per_minute); + +void st_wake_up() { + ENABLE_STEPPER_DRIVER_INTERRUPT(); +} + +// Initializes the trapezoid generator from the current block. Called whenever a new +// block begins. +inline void trapezoid_generator_reset() { + trapezoid_adjusted_rate = current_block->initial_rate; + trapezoid_tick_cycle_counter = 0; // Always start a new trapezoid with a full acceleration tick + set_step_events_per_minute(trapezoid_adjusted_rate); +} + +// This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event +// interrupt. It can be assumed that the trapezoid-generator-parameters and the +// current_block stays untouched by outside handlers for the duration of this function call. +inline void trapezoid_generator_tick() { + if (current_block) { + if (step_events_completed < current_block->accelerate_until) { + trapezoid_adjusted_rate += current_block->rate_delta; + set_step_events_per_minute(trapezoid_adjusted_rate); + } else if (step_events_completed > current_block->decelerate_after) { + // NOTE: We will only reduce speed if the result will be > 0. This catches small + // rounding errors that might leave steps hanging after the last trapezoid tick. + if (trapezoid_adjusted_rate > current_block->rate_delta) { + trapezoid_adjusted_rate -= current_block->rate_delta; + } + if (trapezoid_adjusted_rate < current_block->final_rate) { + trapezoid_adjusted_rate = current_block->final_rate; + } + set_step_events_per_minute(trapezoid_adjusted_rate); + } else { + // Make sure we cruise at exactly nominal rate + if (trapezoid_adjusted_rate != current_block->nominal_rate) { + trapezoid_adjusted_rate = current_block->nominal_rate; + set_step_events_per_minute(trapezoid_adjusted_rate); + } + } + } +} + +// "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is executed at the rate set with +// config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately. +// It is supported by The Stepper Port Reset Interrupt which it uses to reset the stepper port after each pulse. +SIGNAL(TIMER1_COMPA_vect) +{ + // TODO: Check if the busy-flag can be eliminated by just disableing this interrupt while we are in it + + if(busy){ return; } // The busy-flag is used to avoid reentering this interrupt + // Set the direction pins a cuple of nanoseconds before we step the steppers + STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK); + // Then pulse the stepping pins + STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | out_bits; + // Reset step pulse reset timer so that The Stepper Port Reset Interrupt can reset the signal after + // exactly settings.pulse_microseconds microseconds. + TCNT2 = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND)/8); + + busy = TRUE; + sei(); // Re enable interrupts (normally disabled while inside an interrupt handler) + // ((We re-enable interrupts in order for SIG_OVERFLOW2 to be able to be triggered + // at exactly the right time even if we occasionally spend a lot of time inside this handler.)) + + // If there is no current block, attempt to pop one from the buffer + if (current_block == NULL) { + // Anything in the buffer? + current_block = plan_get_current_block(); + if (current_block != NULL) { + trapezoid_generator_reset(); + counter_x = -(current_block->step_event_count >> 1); + counter_y = counter_x; + counter_z = counter_x; + step_events_completed = 0; + } else { + DISABLE_STEPPER_DRIVER_INTERRUPT(); + } + } + + if (current_block != NULL) { + out_bits = current_block->direction_bits; + counter_x += current_block->steps_x; + if (counter_x > 0) { + out_bits |= (1<step_event_count; + } + counter_y += current_block->steps_y; + if (counter_y > 0) { + out_bits |= (1<step_event_count; + } + counter_z += current_block->steps_z; + if (counter_z > 0) { + out_bits |= (1<step_event_count; + } + // If current block is finished, reset pointer + step_events_completed += 1; + if (step_events_completed >= current_block->step_event_count) { + current_block = NULL; + plan_discard_current_block(); + } + } else { + out_bits = 0; + } + out_bits ^= settings.invert_mask; + + // In average this generates a trapezoid_generator_tick every CYCLES_PER_ACCELERATION_TICK by keeping track + // of the number of elapsed cycles. The code assumes that step_events occur significantly more often than + // trapezoid_generator_ticks as they well should. + trapezoid_tick_cycle_counter += cycles_per_step_event; + if(trapezoid_tick_cycle_counter > CYCLES_PER_ACCELERATION_TICK) { + trapezoid_tick_cycle_counter -= CYCLES_PER_ACCELERATION_TICK; + trapezoid_generator_tick(); + } + + busy=FALSE; +} + +// This interrupt is set up by SIG_OUTPUT_COMPARE1A when it sets the motor port bits. It resets +// the motor port after a short period (settings.pulse_microseconds) completing one step cycle. +SIGNAL(TIMER2_OVF_vect) +{ + // reset stepping pins (leave the direction pins) + STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); +} + +// Initialize and start the stepper motor subsystem +void st_init() +{ + // Configure directions of interface pins + STEPPING_DDR |= STEPPING_MASK; + STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask; + LIMIT_DDR &= ~(LIMIT_MASK); + STEPPERS_ENABLE_DDR |= 1<> 3; + prescaler = 1; // prescaler: 8 + actual_cycles = ceiling * 8L; + } else if (cycles <= 0x3fffffL) { + ceiling = cycles >> 6; + prescaler = 2; // prescaler: 64 + actual_cycles = ceiling * 64L; + } else if (cycles <= 0xffffffL) { + ceiling = (cycles >> 8); + prescaler = 3; // prescaler: 256 + actual_cycles = ceiling * 256L; + } else if (cycles <= 0x3ffffffL) { + ceiling = (cycles >> 10); + prescaler = 4; // prescaler: 1024 + actual_cycles = ceiling * 1024L; + } else { + // Okay, that was slower than we actually go. Just set the slowest speed + ceiling = 0xffff; + prescaler = 4; + actual_cycles = 0xffff * 1024; + } + // Set prescaler + TCCR1B = (TCCR1B & ~(0x07<. +*/ + +#ifndef stepper_h +#define stepper_h + +#include +#include + +// Initialize and start the stepper motor subsystem +void st_init(); + +// Block until all buffered steps are executed +void st_synchronize(); + +// Execute the homing cycle +void st_go_home(); + +// The stepper subsystem goes to sleep when it runs out of things to execute. Call this +// to notify the subsystem that it is time to go to work. +void st_wake_up(); + +#endif diff --git a/timer.c b/timer.c deleted file mode 100644 index 6ac461d..0000000 --- a/timer.c +++ /dev/null @@ -1,120 +0,0 @@ -#include "timer.h" - -#include - -#include "dda_queue.h" - -volatile uint32_t next_step_time; - -uint8_t clock_counter_10ms = 0; -uint8_t clock_counter_250ms = 0; -uint8_t clock_counter_1s = 0; -volatile uint8_t clock_flag = 0; - -// timer overflow, happens every TICK_TIME -ISR(TIMER1_CAPT_vect) { - /* - check if next step time will occur before next overflow - */ - if (next_step_time > TICK_TIME) - next_step_time -= TICK_TIME; - else if (next_step_time > 0) { - OCR1A = next_step_time & 0xFFFF; - TIMSK1 |= MASK(OCIE1A); - } - - /* - clock stuff - */ - clock_counter_10ms += TICK_TIME_MS; - if (clock_counter_10ms >= 10) { - clock_counter_10ms -= 10; - clock_flag |= CLOCK_FLAG_10MS; - - clock_counter_250ms += 10; - if (clock_counter_250ms >= 250) { - clock_counter_250ms -= 250; - clock_flag |= CLOCK_FLAG_250MS; - - clock_counter_1s += 1; - if (clock_counter_1s >= 4) { - clock_counter_1s -= 4; - clock_flag |= CLOCK_FLAG_1S; - } - } - } -} - -void timer1_compa_isr(void) __attribute__ ((hot)); -void timer1_compa_isr() { - // led on - WRITE(SCK, 1); - - // disable this interrupt. if we set a new timeout, it will be re-enabled when appropriate - TIMSK1 &= ~MASK(OCIE1A); - - // ensure we don't interrupt again unless timer is reset - next_step_time = 0; - - // stepper tick - queue_step(); - - // led off - WRITE(SCK, 0); -} - -ISR(TIMER1_COMPA_vect) { - timer1_compa_isr(); -} - -void timer_init() -{ - // no outputs - TCCR1A = 0; - // CTC mode- use ICR for top - TCCR1B = MASK(WGM13) | MASK(WGM12) | MASK(CS10); - // set timeout- first timeout is indeterminate, probably doesn't matter - ICR1 = TICK_TIME; - // overflow interrupt (uses input capture interrupt in CTC:ICR mode) - TIMSK1 = MASK(ICIE1); -} - -void setTimer(uint32_t delay) -{ - // save interrupt flag - uint8_t sreg = SREG; - // disable interrupts - cli(); - - // re-enable clock interrupt in case we're recovering from emergency stop - TIMSK1 |= MASK(ICIE1); - - if (delay > 0) { - // mangle timer variables - next_step_time = delay + TCNT1; - if (delay <= 16) { - // unfortunately, force registers don't trigger an interrupt, so we do the following - // don't step from timer interrupt - next_step_time = 0; - // "fire" ISR- maybe it sets a new timeout - timer1_compa_isr(); - } - else if (delay <= TICK_TIME) { - OCR1A = next_step_time & 0xFFFF; - TIMSK1 |= MASK(OCIE1A); - } - } - else { - next_step_time = 0; - } - - // restore interrupt flag - SREG = sreg; -} - -void timer_stop() { - // disable all interrupts - TIMSK1 = 0; - // reset timeout - next_step_time = 0; -} diff --git a/timer.h b/timer.h deleted file mode 100644 index dd6b6be..0000000 --- a/timer.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef _TIMER_H -#define _TIMER_H - -#include -#include - -// time-related constants -#define US * (F_CPU / 1000000) -#define MS * (F_CPU / 1000) - -/* -clock stuff -*/ -extern volatile uint8_t clock_flag; - -#define CLOCK_FLAG_10MS 1 -#define CLOCK_FLAG_250MS 2 -#define CLOCK_FLAG_1S 4 -#define ifclock(F) for (;clock_flag & (F);clock_flag &= ~(F)) - -/* -timer stuff -*/ -void timer_init(void) __attribute__ ((cold)); - -void setTimer(uint32_t delay); - -void timer_stop(void); - -#endif /* _TIMER_H */ diff --git a/watchdog.c b/watchdog.c deleted file mode 100644 index 621280b..0000000 --- a/watchdog.c +++ /dev/null @@ -1,46 +0,0 @@ -#include "watchdog.h" - -#ifdef USE_WATCHDOG - -#include -#include - -#include "arduino.h" -#include "serial.h" - -volatile uint8_t wd_flag = 0; - -// uint8_t mcusr_mirror __attribute__ ((section (".noinit"))); -// void get_mcusr(void) __attribute__((naked)) __attribute__((section(".init3"))); -// void get_mcusr(void) { -// mcusr_mirror = MCUSR; -// MCUSR = 0; -// wdt_disable(); -// } - -ISR(WDT_vect) { -// watchdog has tripped- no main loop activity for 0.5s, probably a bad thing -// if watchdog fires again, we will reset -// perhaps we should do something more intelligent in this interrupt? -wd_flag |= 1; -} - -void wd_init() { -// check if we were reset by the watchdog -// if (mcusr_mirror & MASK(WDRF)) -// serial_writestr_P(PSTR("Watchdog Reset!\n")); - -// 0.25s timeout, interrupt and system reset -wdt_enable(WDTO_500MS); -WDTCSR |= MASK(WDIE); -} - -void wd_reset() { -wdt_reset(); -if (wd_flag) { -WDTCSR |= MASK(WDIE); -wd_flag &= ~1; -} -} - -#endif /* USE_WATCHDOG */ \ No newline at end of file diff --git a/watchdog.h b/watchdog.h deleted file mode 100644 index 8881aa2..0000000 --- a/watchdog.h +++ /dev/null @@ -1,22 +0,0 @@ -#ifndef _WATCHDOG_H -#define _WATCHDOG_H - -#include "config.h" - -#ifdef USE_WATCHDOG - -// initialize -void wd_init(void) __attribute__ ((cold)); - -// reset timeout- must be called periodically or we reboot -void wd_reset(void); - -// notable lack of disable function - -#else /* USE_WATCHDOG */ - -#define wd_init() /* empty */ -#define wd_reset() /* empty */ - -#endif /* USE_WATCHDOG */ -#endif /* _WATCHDOG_H */ \ No newline at end of file diff --git a/wiring_serial.c b/wiring_serial.c new file mode 100644 index 0000000..69d68e9 --- /dev/null +++ b/wiring_serial.c @@ -0,0 +1,211 @@ +/* + wiring_serial.c - serial functions. + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2005-2006 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + $Id: wiring.c 248 2007-02-03 15:36:30Z mellis $ +*/ + +//#include "wiring_private.h" +#include +#include +#include + +// Define constants and variables for buffering incoming serial data. We're +// using a ring buffer (I think), in which rx_buffer_head is the index of the +// location to which to write the next incoming character and rx_buffer_tail +// is the index of the location from which to read. +#ifdef __AVR_ATmega328P__ +#define RX_BUFFER_SIZE 256 +#else +#define RX_BUFFER_SIZE 64 +#endif + +unsigned char rx_buffer[RX_BUFFER_SIZE]; + +int rx_buffer_head = 0; +int rx_buffer_tail = 0; + +void beginSerial(long baud) +{ + UBRR0H = ((F_CPU / 16 + baud / 2) / baud - 1) >> 8; + UBRR0L = ((F_CPU / 16 + baud / 2) / baud - 1); + + /* baud doubler off - Only needed on Uno XXX */ + UCSR0A &= ~(1 << U2X0); + + // enable rx and tx + UCSR0B |= 1< 0) { + buf[i++] = n % base; + n /= base; + } + + for (; i > 0; i--) + printByte(buf[i - 1] < 10 ? + '0' + buf[i - 1] : + 'A' + buf[i - 1] - 10); +} + +void printInteger(long n) +{ + if (n < 0) { + printByte('-'); + n = -n; + } + + printIntegerInBase(n, 10); +} + +void printFloat(double n) +{ + double integer_part, fractional_part; + fractional_part = modf(n, &integer_part); + printInteger(integer_part); + printByte('.'); + printInteger(round(fractional_part*1000)); +} + +// void printHex(unsigned long n) +// { +// printIntegerInBase(n, 16); +// } +// +// void printOctal(unsigned long n) +// { +// printIntegerInBase(n, 8); +// } +// +// void printBinary(unsigned long n) +// { +// printIntegerInBase(n, 2); +// } + +/* Including print() adds approximately 1500 bytes to the binary size, + * so we replace it with the smaller and less-confusing printString(), + * printInteger(), etc. +void print(const char *format, ...) +{ + char buf[256]; + va_list ap; + + va_start(ap, format); + vsnprintf(buf, 256, format, ap); + va_end(ap); + + printString(buf); +} +*/ diff --git a/wiring_serial.h b/wiring_serial.h new file mode 100644 index 0000000..516a0ea --- /dev/null +++ b/wiring_serial.h @@ -0,0 +1,45 @@ +/* + Based on wiring.h - Partial implementation of the Wiring API for the ATmega8. + Part of Arduino - http://www.arduino.cc/ + + Copyright (c) 2005-2006 David A. Mellis + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General + Public License along with this library; if not, write to the + Free Software Foundation, Inc., 59 Temple Place, Suite 330, + Boston, MA 02111-1307 USA + + $Id: wiring.h 387 2008-03-08 21:30:00Z mellis $ +*/ + +#ifndef wiring_h +#define wiring_h + +void beginSerial(long); +void serialWrite(unsigned char); +int serialAvailable(void); +int serialRead(void); +void serialFlush(void); +void printMode(int); +void printByte(unsigned char c); +void printNewline(void); +void printString(const char *s); +void printPgmString(const char *s); +void printInteger(long n); +void printHex(unsigned long n); +void printOctal(unsigned long n); +void printBinary(unsigned long n); +void printIntegerInBase(unsigned long n, unsigned long base); +void printFloat(double n); + +#endif