carousel.comp
1 component carousel """Orient a toolchanger carousel using various encoding schemes 2 3 .B loadrt carousel pockets=\\fIN\\fR[,\\fIN\\fR] 4 .B encoding=\\fIssss\\fR[,\\fIsss\\fR]\\fB 5 .B num_sense=\\fIN\\fR[,\\fIN\\fR] 6 .B dir=\\fIN\\fR[,\\fIN] 7 8 .RS 4 9 .TP 10 \\fBpockets\\fR The number of pockets in each toolchanger. 11 Use up to 8 numbers separated by commas to create multiple carousel components. 12 .TP 13 \\fBencoding\\fR The position encoding. 14 gray, binary, bcd, index, edge or single. Default = 'gray' 15 .TP 16 \\fBnum_sense\\fR The number of position sense pins. 17 Default = 4. 18 .TP 19 \\fBdir\\fR Set to 1 for unidirectional or 2 for bidirectional operation. 20 Default = bidirectional 21 .TP 22 \\fBparity\\fR Set to 1 for odd parity, 0 for even parity checking. 23 Default = 0 (even) 24 .RE"""; 25 26 description """This component is intended to help operate various types of 27 carousel-type toolchangers. 28 The component can be configured to operate with binary, binary-coded decimal (BCD) 29 or gray-coded position feedback, with an individual sensor for each tool position 30 or with a sensor at each tool position and a separate index. 31 32 Both unidirectional and bidirectional systems are supported and those that reverse 33 against a stop when in position. 34 35 The number of carousel component instances created depends on the number of 36 entries in the 'pockets' modparam. For example 37 38 .B loadrt carousel pockets=10,10,8 39 40 Would create 3 carousel instances with 10, 10 and 8 pockets. The other 41 parameters are optional. If absent then defaults will be used. Any missing 42 entry will assume the previous value. 43 44 When the enable pin is set to true the component will immediately set the 45 "active" pin to true and then (for a bidirectional instance) calculate the 46 shortest path to the requested pocket number. The appropriate motor direction 47 output pins will then be set. Bit outputs for forward and reverse are provided 48 as well as a three-state velocity output for driving a DC motor PWM or a 49 velocity-mode stepgen. 50 51 The component will monitor the carousel position and, when the correct position 52 is reached, set the motor-control pins to 0, set "active" to 0 and set "ready" 53 to 1. 54 55 In index mode the behaviour is slightly different. The first time that the "enable" 56 pin is set; the carousel will rotate forwards until 57 both the index and pulse inputs are true. If there is no pulse line at the 58 index position then a HAL "or2" function can be used to allow the index sensor 59 to toggle both inputs. Setting "enable" low does not halt the homing move, so if 60 homing on first tool change is not needed then the enable pin can be toggled by 61 an axis homing pin or a script. \\fBedge\\fR is a special case of index mode for tool 62 changers with pockets on both the rising and falling edges of the position sensor. 63 (Seen on at least one Denford Orac.) 64 65 For tool changers which lock the carousel against a stop the \\fBrev-pulse\\fR pin can 66 be set to a non-zero value. The motor-rev pin will then be set for this many seconds 67 at the completion of the tool search and at the same time the reverse duty/cycle 68 velocity value will be sent to the motor-vel pin. 69 """; 70 71 pin in signed pocket-number """The pocket to move to when the .enable pin goes high. If the value 72 passed is gigher than the number of pockests specified in the "pockets" modparam then modulo arithmetic 73 is used. This is intended to allow the use of multiple tools in the same holder, as is sometimes 74 useful with lathes."""; 75 pin in bit enable "Set this pin high to start movement. Setting it low will stop movement"; 76 pin out bit active "indicates that the component is active"; 77 pin out bit ready "This pin goes high when the carousel is in-position"; 78 pin in bit strobe = 1 """Use this pin to indicate that the position feedback is valid. Often 79 provided by binary encoders"""; 80 pin in bit parity_ """Some encoders supply a parity bit, if this is connected then the 81 parity-error output bit will indicate parity errors"""; 82 pin in bit sense-# [32:personality] """Carousel position feedback pins. In 'index' mode there 83 will be only 2 pins. sense-0 is the index and sense-1 is the pocket sensor."""; 84 pin in float rev-pulse """The duration in seconds for which a ratchet changer (Boxford, Emco) 85 should pulse the reverse pin to lock the holder"""; 86 pin in float fwd-dc "Velocity or duty cycle when forwards rotation is desired"; 87 pin in float rev-dc " Velocity or duty cycle when reverse rotation is desired"; 88 pin in float hold-dc "Duty cycle when carousel is in-position (to hold against stop)"; 89 pin in bit jog-fwd "Jog the carousel forwards one tool position"; 90 pin in bit jog-rev """Jog the carousel in reverse (only if dir = 2). 91 It is very important that these pins should be debounced and should probably 92 also be interlocked to only operate when the machine is idle."""; 93 pin out bit motor-fwd "Indicates the motor should run forwards (bigger numbers)"; 94 pin out bit motor-rev "Indicates the motor should run reverse."; 95 pin out bit parity-error "Indicates a parity error"; 96 pin out signed current-position "This pin indicates the current position feedback"; 97 pin out float motor_vel "The duty-cycle or velocity to drive a DC motor or stepgen"; 98 99 param r signed state = 0 "Current component state"; 100 param r bit homing = 0 "Shows that homing is in progress. Only used for index mode"; 101 param r bit homed = 0 "Shows that homing is complete. Only used in index and edge modes"; 102 param r float timer "Shows the value of the internal timer"; 103 104 option count_function; 105 option extra_setup; 106 107 license "GPL"; 108 author "andy pugh"; 109 110 variable int inst_sense; 111 variable int inst_dir; 112 variable int inst_pockets; 113 variable int inst_code; 114 variable int inst_parity; 115 variable int old_index = 0; 116 variable int target; 117 function _ ; 118 119 ;; 120 121 int default_pockets = 8; 122 int default_code = 'G'; 123 int default_dir = 2; 124 int default_sense = 4; 125 int default_parity = 0; 126 127 #define MAX_CHAN 8 128 static int pockets[MAX_CHAN] = {-1}; 129 RTAPI_MP_ARRAY_INT(pockets, MAX_CHAN, "The number of pockets in each carousel") 130 static char *encoding[MAX_CHAN]; 131 RTAPI_MP_ARRAY_STRING(encoding, MAX_CHAN, "Position feedback type") 132 static int dir[MAX_CHAN] = {-1}; 133 RTAPI_MP_ARRAY_INT(dir, MAX_CHAN, 134 "set to 2 if the carousel is bidirectional") 135 static int num_sense[MAX_CHAN] = {-1}; 136 RTAPI_MP_ARRAY_INT(num_sense, MAX_CHAN, "The number of sense pins to create") 137 // We have a hal pin and a modparam with the same name. From the user 138 // point of view I think that makes sense. In the code parity[] is the modparam, 139 // parity_ is the hal pin. 140 static int parity[MAX_CHAN] = {-1}; 141 RTAPI_MP_ARRAY_INT(parity, MAX_CHAN, "0 for even parity, 1 for odd") 142 143 FUNCTION(_){ 144 int i, d, pow; 145 int pcalc = 0; 146 int mod_pocket = 0; 147 int p = 0; 148 unsigned int mask; 149 150 switch inst_code{ 151 case 'G': // Gray Code 152 for (i = 0; i < inst_sense ; i++) { 153 p += sense(i) << i; 154 pcalc ^= sense(i); 155 } 156 for(mask = p >> 1 ; mask != 0 ; mask = mask >> 1){ 157 p ^= mask; 158 } 159 break; 160 case 'B': // Straight Binary 161 for (i = 0; i < inst_sense ; i++) { 162 p += sense(i) << i; 163 pcalc ^= sense(i); 164 } 165 break; 166 case 'D': // BCD 167 i = 0; 168 pow = 1; 169 while (i < inst_sense){ 170 int lim; 171 d = 0; 172 for (lim = i + 4; i < lim && i < inst_sense; i++) { 173 d += sense(i) << (i % 4); 174 pcalc ^= sense(i); 175 } 176 p += d * pow; 177 pow *= 10; 178 } 179 break; 180 case 'S': // individual sensors 181 for (i = inst_sense - 1; sense(i) == 0 && i > 0 ; i--) {} 182 if (sense(i)) 183 p = i + 1; 184 break; 185 case 'I': // index + position. 186 p = current_position; 187 if (homed){ 188 if ( !old_index && sense(1) ){ 189 if (motor_fwd){ 190 p += 1; 191 if (p > inst_pockets) p -= inst_pockets; 192 } 193 if (motor_rev) { 194 p -= 1; 195 if (p < 1) p += inst_pockets; 196 } 197 } 198 old_index = sense(1); 199 } 200 break; 201 case 'E': // index + position, both edges. 202 p = current_position; 203 if (homed){ 204 if ( old_index != sense(1) ){ 205 if (motor_fwd){ 206 p += 1; 207 if (p > inst_pockets) p -= inst_pockets; 208 } 209 if (motor_rev) { 210 p -= 1; 211 if (p < 1) p += inst_pockets; 212 } 213 } 214 old_index = sense(1); 215 } 216 break; 217 218 } 219 220 if (strobe) { 221 current_position = p; 222 parity_error = (pcalc != (inst_parity ^ parity_)); 223 } 224 mod_pocket = ((pocket_number - 1) % inst_pockets) + 1; 225 // mod is odd with negatives, so just in case 226 if (mod_pocket < 1) mod_pocket = 1; 227 if (mod_pocket > inst_pockets) mod_pocket = inst_pockets; 228 switch (state){ 229 case 0: // waiting at start 230 if (jog_fwd || (jog_rev && inst_dir == 2)) { 231 if ((inst_code == 'I' || inst_code == 'E') && ! homed){ 232 state = 10; 233 break; 234 } 235 target = current_position + jog_fwd - jog_rev; 236 if (target > inst_pockets ) target = 1; 237 if (target < 1) target = inst_pockets; 238 if (jog_fwd){ 239 motor_fwd = 1; 240 motor_rev = 0; 241 motor_vel = fwd_dc; 242 } 243 if (jog_rev){ 244 motor_fwd = 0; 245 motor_rev = 1; 246 motor_vel = rev_dc; 247 } 248 active = 1; 249 state = 5; 250 break; 251 } 252 motor_vel = hold_dc; 253 if (! enable) return ; 254 active = 1; 255 if ((inst_code == 'I' || inst_code == 'E') && ! homed){ 256 state = 10; 257 break; 258 } 259 state = 1; 260 ready = 0; 261 case 1: // choose direction 262 if (pocket_number < 1 || pocket_number > inst_pockets) { 263 state = 0; 264 return; 265 } 266 if (inst_dir == 2){ 267 if (current_position < mod_pocket){ 268 if (mod_pocket - current_position > (inst_pockets / 2)) { 269 motor_fwd = 0; 270 motor_rev = 1; 271 motor_vel = rev_dc; 272 } else { 273 motor_fwd = 1; 274 motor_rev = 0; 275 motor_vel = fwd_dc; 276 } 277 } else { 278 if (current_position - mod_pocket > (inst_pockets / 2)) { 279 motor_fwd = 1; 280 motor_rev = 0; 281 motor_vel = fwd_dc; 282 } else { 283 motor_fwd = 0; 284 motor_rev = 1; 285 motor_vel = rev_dc; 286 } 287 } 288 } else { 289 motor_fwd = 1; 290 motor_rev = 0; 291 motor_vel = fwd_dc; 292 } 293 state = 2; 294 case 2: // moving 295 if ((current_position != mod_pocket) && enable) return; 296 if (rev_pulse > 0){ 297 motor_fwd = 0; 298 motor_rev = 1; 299 motor_vel = rev_dc; 300 timer = rev_pulse; 301 state = 3; 302 } else { 303 motor_fwd = 0; 304 motor_rev = 0; 305 motor_rev = hold_dc; 306 active = 0; 307 if (enable) ready = 1; 308 state = 4; 309 break; 310 } 311 case 3: 312 timer -= fperiod; 313 if (timer > 0) break; 314 state = 4; 315 motor_fwd = 0; 316 motor_rev = 0; 317 motor_vel = hold_dc; 318 active = 0; 319 if (enable) ready = 1; 320 case 4: //waiting for enable to go false 321 if (enable) return; 322 state = 0; 323 break; 324 case 5: //jogging fwd/rev 325 if (current_position != target) return; 326 motor_fwd = 0; 327 motor_rev = 0; 328 motor_vel = hold_dc; 329 active = 0; 330 if (jog_fwd || jog_rev) return; // require button release to jog again 331 state = 0; 332 break; 333 case 10: // start of homing 334 homed = 0; 335 homing = 1; 336 motor_fwd = 1; 337 motor_rev = 0; 338 motor_vel = fwd_dc; 339 state = 11; 340 case 11: // waiting for index & pulse 341 if ( (! old_index) && (sense(0) && sense(1)) ){ // index found 342 current_position = 1; 343 homed = 1; 344 homing = 0; 345 active = 0; 346 motor_fwd = 0; 347 motor_rev = 0; 348 motor_vel = 0; 349 state = 0; 350 } 351 old_index = (sense(0) && sense(1)); 352 break; // So that we don't see the tool1 pulse twice 353 } 354 } 355 356 EXTRA_SETUP(){ 357 if (pockets[extra_arg] > 0) default_pockets = pockets[extra_arg]; 358 if (encoding[extra_arg] == NULL) { 359 //it's already default_code 360 } else if (strncmp(encoding[extra_arg], "binary", 6) == 0) { 361 default_code = 'B'; 362 } else if (strncmp(encoding[extra_arg], "bcd", 3) == 0) { 363 default_code = 'D'; 364 } else if (strncmp(encoding[extra_arg], "single", 6) == 0) { 365 default_code = 'S'; 366 } else if (strncmp(encoding[extra_arg], "index", 5) == 0) { 367 default_code = 'I'; 368 } else if (strncmp(encoding[extra_arg], "edge", 4) == 0) { 369 default_code = 'E'; 370 } 371 372 if (dir[extra_arg] > 0) default_dir = (dir[extra_arg] > 1)? 2:1; 373 374 if (parity[extra_arg] != -1) default_parity = parity[extra_arg]; 375 376 if (default_code == 'I') { 377 default_sense = 2; 378 } else if (num_sense[extra_arg] > 0 ) { 379 default_sense = num_sense[extra_arg]; 380 } 381 382 inst_pockets = default_pockets; 383 inst_code = default_code; 384 inst_dir = default_dir; 385 inst_sense = default_sense; 386 inst_parity = default_parity; 387 388 if (inst_code == 'S' && inst_sense < inst_pockets) inst_sense = inst_pockets; 389 personality = inst_sense; 390 391 return 0; 392 } 393 394 int get_count(void){ 395 int i; 396 for (i = 0; pockets[i] != 0 && i < MAX_CHAN; i++){} 397 if (i == 0) return 1 ; 398 return i; 399 }