1  /******************************************************************************
  2   *
  3   * Copyright (C) 2014 Charles Steinkuehler (charles AT steinkuehler DOT net)
  4   *
  5   *
  6   * This module allows multiple drive motors (joints) to be connected to a
  7   * single motion axis.  This is useful for gantry style machines if you don't
  8   * want to use gantrykins
  9   *
 10   ******************************************************************************
 11   *
 12   * This program is free software; you can redistribute it and/or
 13   * modify it under the terms of the GNU General Public License
 14   * as published by the Free Software Foundation; either version 2
 15   * of the License, or (at your option) any later version.
 16   *
 17   * This program is distributed in the hope that it will be useful,
 18   * but WITHOUT ANY WARRANTY; without even the implied warranty of
 19   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 20   * GNU General Public License for more details.
 21   *
 22   * You should have received a copy of the GNU General Public License
 23   * along with this program; if not, write to the Free Software
 24   * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 25   * 02110-1301, USA.
 26   *
 27   * THE AUTHORS OF THIS PROGRAM ACCEPT ABSOLUTELY NO LIABILITY FOR
 28   * ANY HARM OR LOSS RESULTING FROM ITS USE.  IT IS _EXTREMELY_ UNWISE
 29   * TO RELY ON SOFTWARE ALONE FOR SAFETY.  Any machinery capable of
 30   * harming persons must have provisions for completely removing power
 31   * from all motors, etc, before persons enter any danger area.  All
 32   * machinery must be designed to comply with local and national safety
 33   * codes, and the authors of this software can not, and do not, take
 34   * any responsibility for such compliance.
 35   *
 36   * This code was written as part of the LinuxCNC project.  For more
 37   * information, go to www.linuxcnc.org.
 38   *
 39   ******************************************************************************/
 40  
 41  component gantry "LinuxCNC HAL component for driving multiple joints from a single axis";
 42  pin out float joint.##.pos-cmd [7 : personality] "Per-joint commanded position";
 43  pin in  float joint.##.pos-fb  [7 : personality] "Per-joint position feedback";
 44  pin in  bit   joint.##.home    [7 : personality] "Per-joint home switch";
 45  pin out float joint.##.offset  [7 : personality] "(debugging) Per-joint offset value, updated when homing";
 46  pin in  float position-cmd "Commanded position from motion";
 47  pin out float position-fb "Position feedback to motion";
 48  pin out bit   home "Combined home signal, true if all joint home inputs are true";
 49  pin out bit   limit "Combined limit signal, true if any joint home input is true";
 50  pin in  float search-vel "HOME_SEARCH_VEL from ini file";
 51  function read  fp "Update position-fb and home/limit outputs based on joint values";
 52  function write fp "Update joint pos-cmd outputs based on position-cmd in";
 53  description """
 54  Drives multiple physical motors (joints) from a single axis input
 55  .LP
 56  The `personality' value is the number of joints to control.  Two is typical, but
 57  up to seven is supported (a three joint setup has been tested with hardware).
 58  .LP
 59  All controlled joints track the commanded position (with a per-joint offset)
 60  unless in the process of homing.  Homing is when the commanded position is
 61  moving towards the homing switches (as determined by the sign of search-vel)
 62  and the joint home switches are not all in the same state.  When the system is
 63  homing and a joint home switch activates, the command value sent to that joint
 64  is "frozen" and the joint offset value is updated instead.  Once all home
 65  switches are active, there are no more adjustments made to the offset values
 66  and all joints run in lock-step once more.
 67  .LP
 68  For best results, set HOME_SEARCH_VEL and HOME_LATCH_VEL to the same direction
 69  and as slow as practical.  When a joint home switch trips, the commanded
 70  velocity will drop immediately from HOME_SEARCH_VEL to zero, with no limit on
 71  acceleration.
 72  """;
 73  license "GPL";
 74  variable float offset[7] = 0.0;
 75  variable float prev_cmd = 0.0;
 76  variable int   fb_joint = 0;
 77  variable int   latching = 0;
 78  ;;
 79  FUNCTION(read) {
 80      int i=1;
 81  
 82      // First (or only) joint
 83      home=joint_home(0);
 84      limit=joint_home(0);
 85  
 86      // All other joints, if configured
 87      while (i < personality) {
 88          // Check to see if machine is in latching state
 89          if(latching==0)
 90          {
 91              // Don't assert home until all joints hit their home switches
 92              home  &= joint_home(i);
 93          }
 94          else
 95          {
 96              // Don't release home until all joints have backed off their
 97              // home switches
 98              home |= joint_home(i);
 99          }
100  
101          // Remember the home state for next time
102          latching=home;
103  
104          // Limit is always asserted if any home switch is asserted
105          limit |= joint_home(i);
106          i++;
107      }
108  
109      // Joint used for feedback is 'sticky', but we have to switch to
110      // track active joints or motion gets upset with the sudden
111      // stop.  If all joints are not homed, but the current joint used
112      // for feedback is, find a joint that's still active
113      if ((joint_home(fb_joint) == 1) && (home == 0)) {
114          for (i=0; i < personality; i++) {
115              if (joint_home(i) == 0) {
116                  position_fb = joint_pos_fb(i) + offset[i];
117                  fb_joint = i;
118                  break;
119              }
120          }
121      } else {
122          position_fb = joint_pos_fb(fb_joint) + offset[fb_joint];
123      }
124  }
125  
126  FUNCTION(write) {
127      int i;
128      float delta;
129  
130      // Determine if we're moving in the same direction as home search
131  
132      // First calculate the direction we're moving now
133      delta = position_cmd - prev_cmd;
134  
135      // Stash current commanded position for next time
136      prev_cmd = position_cmd;
137  
138      // Then multiply our delta value by the search velocity
139      // If the signs match and neither is zero, the result will be positive
140      // indicate we are moving towards home.  Otherwise, the result will be
141      // zero or negative.
142      //
143      // If we're moving towards home and all home switches are not closed
144      if ( ((delta * search_vel) > 0) && (home==0) ) {
145          // Check each joint to see if it's home switch is active
146          for (i=0; i < personality; i++) {
147              // If home switch is active, update offset, not pos_cmd
148              // so the other joints can catch up
149              if (joint_home(i)==1) {
150                  offset[i] += delta;
151              }
152          }
153      }
154  
155      // Update each joint's commanded position
156      for (i=0; i < personality; i++) {
157          joint_pos_cmd(i) = position_cmd - offset[i];
158          joint_offset(i)  = offset[i];
159      }
160  }
161