├── LICENSE ├── README.md ├── cia402.comp └── example ├── cia402.hal └── ethercat-conf.xml /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | 294 | Copyright (C) 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | , 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # hal-cia402 2 | HAL Interface for CiA402 Devices, 3 | 4 | this component acts as a glue layer between hardware to Hal modules like Ethercat, CAN-Bus or others. 5 | 6 | It translates raw IO Data from the PDOs to the common linuxcnc Hal pin structure and has build in logic 7 | for the CiA402 State Control, feedback handling, external homing and build in scaling functions. 8 | 9 | It delivers two functions: read_all and write_all. 10 | 11 | 12 | The concept of integration in the correspondending task should be as following: 13 | 14 | 15 | 16 | HARDWARE INPUT-->--CiA402_read-->--Motion-->--CiA402_write-->--Hardware Output 17 | 18 | Hal Example: 19 | 20 | #Setup 21 | 22 | loadrt [KINS]KINEMATICS 23 | 24 | loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS 25 | 26 | (loadusr -W lcec_conf ethercat-conf.xml) 27 | 28 | loadrt lcec 29 | 30 | loadrt cia402 count=3 31 | 32 | loadrt pid names=x-pid,y-pid,z-pid 33 | 34 | 35 | 36 | #Functions servo-thread 37 | 38 | addf lcec.read-all servo-thread 39 | 40 | addf cia402.0.read-all servo-thread 41 | 42 | addf cia402.1.read-all servo-thread 43 | 44 | addf cia402.2.read-all servo-thread 45 | 46 | addf motion/ PIDs / PCL / etc . 47 | 48 | addf cia402.0.write-all servo-thread 49 | 50 | addf cia402.1.write-all servo-thread 51 | 52 | addf cia402.2.write-all servo-thread 53 | 54 | addf lcec.write-all servo-thread 55 | 56 | 57 | #nets ..... 58 | 59 | 60 | 61 | Modes of Operation: 62 | 63 | By default the component is set to CSP Mode, CSV Mode could be selected with an: 64 | 65 | setp cia402.0.csp-mode 0 in hal. 66 | 67 | Mode changing in runtime is currently not supported, to avoid 68 | unwanted behaviour. 69 | 70 | Homing: 71 | 72 | For using the servo drives internal homing procedure configure your 73 | joint homing to Home on Index Pulse only and connect the components 74 | home input to the motion index-enable Pin: 75 | 76 | HOME_SEARCH_VEL = 0.0 77 | HOME_LATCH_VEL = 0.2 (Any value but zero, the homing speed is predetermined by the drives configured speed 78 | HOME_USE_INDEX = TRUE 79 | 80 | If you are using PIDs, don't forget to connect the PIDs index-enable pin. 81 | 82 | 83 | Flexibility: 84 | 85 | Even though this component exports many pins, you can choose which functions you want to use: 86 | 87 | If you would like to use the CiA State Machine, connect Statusword and Controlword. 88 | 89 | For single use of the scaling function connect only the fb and cmd pins from position or velocity. 90 | 91 | If no Drives homing is needed, let the Pins unconnected. 92 | 93 | -------------------------------------------------------------------------------- /cia402.comp: -------------------------------------------------------------------------------- 1 | component cia402 "HAL CiA402 Drive interface layer"; 2 | // 3 | // Copyright (C) 2019 Dominik Braun 4 | // 5 | // This program is free software; you can redistribute it and/or modify 6 | // it under the terms of the GNU General Public License as published by 7 | // the Free Software Foundation; either version 2 of the License, or 8 | // (at your option) any later version. 9 | // 10 | // This program is distributed in the hope that it will be useful, 11 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | // GNU General Public License for more details. 14 | // 15 | // You should have received a copy of the GNU General Public License 16 | // along with this program; if not, write to the Free Software 17 | // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | // 19 | author "Dominik Braun"; 20 | license "GPL"; 21 | 22 | description """ 23 | HAL Interface for CiA402 Devices, 24 | 25 | this component acts as a glue layer between hardware to Hal modules like Ethercat, CAN-Bus or others.\n 26 | It translates raw IO Data from the PDOs to the common linuxcnc Hal pin structure and has build in logic\n 27 | 28 | for the CiA402 State Control, feedback handling, external homing and build in scaling functions.\n 29 | 30 | It delivers two functions: read_all and write_all.\n 31 | The concept of integration in the correspondending task should be as following: \n 32 | 33 | 34 | 35 | ################### ########### ######### ############ #####################\n 36 | #HARDWARE INPUT # # CiA402 # #Motion # # CiA402 # # Hardware Output #\n 37 | # like #-->>--#read_all #-->>>--#Pids #-->--#write_all #-->>--# like #\n 38 | #Ethercat read_all# # etc. # # # # # # Ethercat write_all#\n 39 | ################### ########### ######### ############ #####################\n 40 | \n 41 | Hal Example:\n 42 | 43 | #########\n 44 | # Setup\n 45 | #########\n 46 | 47 | 48 | loadrt [KINS]KINEMATICS\n 49 | loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS\n 50 | (loadusr -W lcec_conf ethercat-conf.xml)\n 51 | loadrt lcec\n 52 | loadrt cia402 count=3\n 53 | loadrt pid names=x-pid,y-pid,z-pid\n 54 | 55 | ##########################\n 56 | # Functions servo-thread\n 57 | ##########################\n 58 | addf lcec.read-all servo-thread\n 59 | addf cia402.0.read-all servo-thread\n 60 | addf cia402.1.read-all servo-thread\n 61 | addf cia402.2.read-all servo-thread\n 62 | 63 | addf motion/ PIDs / PCL / etc .\n 64 | 65 | addf cia402.0.write-all servo-thread\n 66 | addf cia402.1.write-all servo-thread\n 67 | addf cia402.2.write-all servo-thread\n 68 | addf lcec.write-all servo-thread\n 69 | #########################################\n 70 | #nets .....\n 71 | 72 | By default the component is set to CSP Mode,\n 73 | CSV Mode could be selected with an:\n 74 | setp cia402.0.csp-mode 0 in hal.\n 75 | Mode changing in runtime is currently not supported, to avoid\n 76 | unwanted behaviour.\n 77 | 78 | For using the servo drives internal homing procedure configure your\n 79 | joint homing to Home on Index Pulse only and connect the components\n 80 | home Input to the index-enable Pin:\n 81 | 82 | HOME_SEARCH_VEL = 0.0\n 83 | HOME_LATCH_VEL = 0.2 (Any value but zero, the homing speed is predetermined by the drives configured speed\n 84 | HOME_USE_INDEX = TRUE\n 85 | 86 | If you are using PIDs, don't forget to connect the PIDs index-enable pin.\n 87 | 88 | 89 | Even though this component exports many pins, you can choose which functions you want to use:\n 90 | 91 | If you would like to use the CiA State Machine, connect Statusword and Controlword.\n 92 | For single use of the scaling function connect only the fb and cmd pins from position or velocity.\n 93 | If no Drives homing is needed, let the Pins unconnected."""; 94 | 95 | //CiA IOs 96 | //inputs from drive 97 | pin in unsigned statusword "Drives CiA402 Statusword, index 0x6041"; 98 | pin in signed opmode_display "Drives Modes of Operation feedback register, index 0x6061"; 99 | pin in signed drv_actual_position "Drives actual Position, index 0x6064"; 100 | pin in signed drv_actual_velocity "Drives actual Velocity, index 0x606C"; 101 | //outputs to drive 102 | pin out unsigned controlword "Drives CIA402 Controlword, index 0x6040"; 103 | pin out signed opmode "Drives Modes of Operation Input, index 0x6060"; 104 | pin out signed drv_target_position "Position command to the drive, index 0x607A"; 105 | pin out signed drv_target_velocity "Velocity command to the drive, index 0x60FF"; 106 | 107 | //Control IOs 108 | pin in bit enable "true enables the Drive"; 109 | pin in float pos_cmd "target Position, from Motion or PID"; 110 | pin in float velocity_cmd "target Velocity, from Motion or PID"; 111 | 112 | pin out float pos_fb "Position feedback, scaled"; 113 | pin out float velocity_fb "Velocity feedback scaled"; 114 | pin out bit drv_fault "true indicates an Drive Error"; 115 | 116 | //Homing IOs 117 | pin io bit home "true starts the Drives internal home procedure, connect this to joint.x.index-enable pin"; 118 | pin out bit stat_homed "true indicates that the Drive is internaly homed"; 119 | pin out bit stat_homing "true indicates that the Drives homing procedure is running"; 120 | //Auxilary IOs 121 | pin out bit stat_switchon_ready "Drive in CiA State: ready to switch on"; 122 | pin out bit stat_switched_on "Drive in CiA State: switched on"; 123 | pin out bit stat_op_enabled "Drive in CiA State: Operation enabled"; 124 | pin out bit stat_voltage_enabled "Drive in CiA State: switched on, Voltage enabled"; 125 | pin out bit stat_fault "Drive hast the fault bit set"; 126 | pin out bit stat_quick_stop "Drive in State: Quick Stop"; 127 | pin out bit stat_switchon_disabled "Drive in CiA State: Switch on disabled"; 128 | pin out bit stat_warning "Drive has the warning bit set"; 129 | pin out bit stat_remote "Drive in State remote / bus operation"; 130 | pin out bit stat_target_reached "Drive has reached target position / velocity"; 131 | pin out bit opmode_no_mode "Drive has no operation Mode set"; 132 | pin out bit opmode_homing "Drive in Mode Homing"; 133 | pin out bit opmode_cyclic_position "Drive in Mode Cyclic synchronus Position"; 134 | pin out bit opmode_cyclic_velocity "Drive in Mode Cyclic synchronus Velocity"; 135 | 136 | pin in bit fault_reset "true, sends fault reset command to the Drive"; 137 | 138 | //parameters 139 | param rw float pos_scale "increments per machine unit"; 140 | param rw float velo_scale "velocity in machine units per 1 Motor revolution"; 141 | param rw bit auto_fault_reset "true resets an actual Drive Fault automatically at the next enable Signal"; 142 | param rw bit csp_mode "true= CS Position Mode, false= CS Velocity Mode, is only recognized at Linuxcnc Startup, default true"; 143 | 144 | //internals 145 | variable float pos_scale_old; 146 | variable float velo_scale_old; 147 | variable bool enable_old; 148 | variable bool stat_homed_old; 149 | variable bool stat_fault_old; 150 | variable double pos_scale_rcpt; 151 | variable double velo_scale_rcpt; 152 | variable bool pos_mode; 153 | variable bool init_pos_mode = 0; 154 | variable long auto_fault_reset_delay; 155 | 156 | //declare functions 157 | function read_all; 158 | function write_all; 159 | option extra_setup yes; 160 | 161 | ;; 162 | 163 | //constants 164 | #define FAULT_AUTORESET_DELAY_NS 100000000LL 165 | #define OPMODE_CYCLIC_POSITION 8 166 | #define OPMODE_CYCLIC_VELOCITY 9 167 | #define OPMODE_HOMING 6 168 | #define OPMODE_NONE 0 169 | 170 | EXTRA_SETUP () { 171 | // initialize variables 172 | pos_scale = 1.0; 173 | pos_scale_old = pos_scale + 1.0; 174 | pos_scale_rcpt = 1.0; 175 | velo_scale = 1.0; 176 | velo_scale_old = velo_scale + 1.0; 177 | velo_scale_rcpt = 1.0; 178 | auto_fault_reset = 1; 179 | csp_mode = 1; 180 | return 0; 181 | } 182 | 183 | void check_scales(hal_float_t *scale,float *scale_old, double *scale_rcpt) { 184 | // check for change in scale value 185 | if (*scale != *scale_old) { 186 | // scale value has changed, test and update it 187 | if ((*scale < 1e-20) && (*scale > -1e-20)) { 188 | // value too small, divide by zero is a bad thing 189 | *scale = 1.0; 190 | } 191 | // save new scale to detect future changes 192 | *scale_old = *scale; 193 | // we actually want the reciprocal 194 | *scale_rcpt = 1.0 / *scale; 195 | } 196 | } 197 | 198 | FUNCTION(read_all) { 199 | 200 | // check for change in scale value 201 | check_scales(&pos_scale, &pos_scale_old, &pos_scale_rcpt); 202 | check_scales(&velo_scale, &velo_scale_old, &velo_scale_rcpt); 203 | 204 | // read position feedback 205 | pos_fb = ((double)drv_actual_position) * pos_scale_rcpt; 206 | 207 | // read velocity feedback 208 | velocity_fb = ((double)drv_actual_velocity) * velo_scale_rcpt; 209 | 210 | // read Modes of Operation 211 | opmode_no_mode = (opmode_display == OPMODE_NONE); 212 | opmode_homing = (opmode_display == OPMODE_HOMING); 213 | opmode_cyclic_velocity = (opmode_display == OPMODE_CYCLIC_VELOCITY); 214 | opmode_cyclic_position = (opmode_display == OPMODE_CYCLIC_POSITION); 215 | 216 | // read status 217 | stat_switchon_ready = (statusword >> 0) & 1; 218 | stat_switched_on = (statusword >> 1) & 1; 219 | stat_op_enabled = (statusword >> 2) & 1; 220 | stat_fault = (statusword >> 3) & 1; 221 | stat_voltage_enabled = (statusword >> 4) & 1; 222 | stat_quick_stop = (statusword >> 5) & 1; 223 | stat_switchon_disabled = (statusword >> 6) & 1; 224 | stat_warning = (statusword >> 7) & 1; 225 | stat_remote = (statusword >> 9) & 1; 226 | 227 | if (opmode_cyclic_position || opmode_cyclic_velocity) { 228 | stat_target_reached = (statusword >> 10) & 1; 229 | } else { 230 | stat_target_reached = 0; 231 | } 232 | 233 | //home states 234 | if (opmode_homing) { 235 | stat_homed = ((statusword >> 10) & 1) && ((statusword >> 12) & 1); 236 | stat_homing = !stat_homed && !((statusword >> 10) & 1); 237 | } 238 | 239 | // update fault output 240 | if (auto_fault_reset_delay > 0) { 241 | auto_fault_reset_delay -= period; 242 | drv_fault = 0; 243 | } else { 244 | drv_fault = stat_fault && enable; 245 | } 246 | } 247 | 248 | FUNCTION(write_all) { 249 | int enable_edge; 250 | 251 | //init opmode 252 | if (!init_pos_mode) { 253 | pos_mode = csp_mode; 254 | init_pos_mode = 1; 255 | } 256 | 257 | // detect enable edge 258 | enable_edge = enable && !enable_old; 259 | enable_old = enable; 260 | 261 | // write control register 262 | controlword = (1 << 2); // quick stop is not supported 263 | if (stat_fault) { 264 | home = 0; 265 | if (fault_reset) { 266 | controlword |= (1 << 7); // fault reset 267 | } 268 | if (auto_fault_reset && enable_edge) { 269 | auto_fault_reset_delay = FAULT_AUTORESET_DELAY_NS; 270 | controlword |= (1 << 7); // fault reset 271 | } 272 | } else { 273 | if (enable) { 274 | controlword |= (1 << 1); // enable voltage 275 | if (stat_switchon_ready) { 276 | controlword |= (1 << 0); // switch on 277 | if (stat_switched_on) { 278 | controlword |= (1 << 3); // enable op 279 | } 280 | } 281 | } 282 | } 283 | 284 | // write position command 285 | drv_target_position = (int32_t) (pos_cmd * pos_scale); 286 | // write velocity command 287 | drv_target_velocity = (int32_t) (velocity_cmd * velo_scale); 288 | 289 | // reset home command 290 | if (home && (stat_homed && !stat_homed_old) && opmode_homing) { 291 | home = 0; 292 | } 293 | stat_homed_old = stat_homed; 294 | 295 | // OP Mode 296 | // set to position mode 297 | if (stat_voltage_enabled && !home ) { 298 | opmode = OPMODE_CYCLIC_POSITION; 299 | } 300 | // set velo mode 301 | if (stat_voltage_enabled && !pos_mode && !home) { 302 | opmode = OPMODE_CYCLIC_VELOCITY; 303 | } 304 | // mode Home and start homing 305 | if (home) { 306 | opmode = OPMODE_HOMING; 307 | controlword |= (home << 4); 308 | } 309 | } 310 | 311 | -------------------------------------------------------------------------------- /example/cia402.hal: -------------------------------------------------------------------------------- 1 | ########################################################### 2 | # 3 | # CIA 402 example snippet Hal 4 | # 5 | ########################################################### 6 | 7 | ########################################################### 8 | # Setup 9 | ########################################################### 10 | 11 | loadrt [KINS]KINEMATICS 12 | loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS 13 | 14 | loadusr -W lcec_conf ethercat-conf.xml 15 | loadrt lcec 16 | loadrt cia402 count=3 17 | loadrt pid names=x-pid,y-pid,z-pid 18 | 19 | 20 | ########################################################### 21 | # Functions servo-thread 22 | ########################################################### 23 | 24 | addf lcec.read-all servo-thread 25 | addf cia402.0.read-all servo-thread 26 | addf cia402.1.read-all servo-thread 27 | addf cia402.2.read-all servo-thread 28 | 29 | addf motion-command-handler servo-thread 30 | addf motion-controller servo-thread 31 | 32 | addf x-pid.do-pid-calcs servo-thread 33 | addf y-pid.do-pid-calcs servo-thread 34 | addf z-pid.do-pid-calcs servo-thread 35 | 36 | addf cia402.0.write-all servo-thread 37 | addf cia402.1.write-all servo-thread 38 | addf cia402.2.write-all servo-thread 39 | addf lcec.write-all servo-thread 40 | 41 | ######################################### 42 | #nets 43 | ######################################### 44 | net emc-enable => iocontrol.0.emc-enable-in 45 | sets emc-enable 1 46 | 47 | #config 48 | setp cia402.0.csp-mode 1 49 | setp cia402.0.pos-scale 3600 50 | 51 | #from servo(ethercat) to cia402 52 | net x-statusword lcec.0.0.cia-statusword => cia402.0.statusword 53 | net x-opmode-display lcec.0.0.opmode-display => cia402.0.opmode-display 54 | net x-drv-act-pos lcec.0.0.actual-position => cia402.0.drv-actual-position 55 | net x-drv-act-velo lcec.0.0.actual-velocity => cia402.0.drv-actual-velocity 56 | 57 | #from cia402 to servo(ethercat) 58 | net x-controlword cia402.0.controlword => lcec.0.0.cia-controlword 59 | net x-modes-of-operation cia402.0.opmode => lcec.0.0.opmode 60 | net x-drv-target-pos cia402.0.drv-target-position => lcec.0.0.target-position 61 | net x-drv-target-velo cia402.0.drv-target-velocity => lcec.0.0.target-velocity 62 | 63 | #from motion to cia 64 | net x-enable <= joint.0.amp-enable-out => cia402.0.enable 65 | net x-amp-fault => joint.0.amp-fault-in <= cia402.0.drv-fault 66 | net x-pos-cmd <= joint.0.motor-pos-cmd => cia402.0.pos-cmd 67 | net x-pos-fb => joint.0.motor-pos-fb <= cia402.0.pos-fb 68 | 69 | #homing 70 | net x-home-index <= joint.0.index-enable => cia402.0.home 71 | 72 | 73 | -------------------------------------------------------------------------------- /example/ethercat-conf.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | --------------------------------------------------------------------------------