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