homing.c
1 /******************************************************************** 2 * Description: homing.c 3 * code to handle homing - originally in control.c, but moved out 4 * to improve modularity and keep control.c from bloating 5 * 6 * Author: jmkasunich 7 * License: GPL Version 2 8 * Created on: 9 * System: Linux 10 * 11 * Copyright (c) 2004 All rights reserved. 12 ********************************************************************/ 13 14 #include "rtapi.h" 15 #include "rtapi_math.h" 16 #include "motion.h" 17 #include "hal.h" 18 #include "mot_priv.h" 19 #include "homing.h" 20 21 #define ABS(x) (((x) < 0) ? -(x) : (x)) 22 23 // Mark strings for translation, but defer translation to userspace 24 #define _(s) (s) 25 26 /*********************************************************************** 27 * LOCAL CONSTANTS * 28 ************************************************************************/ 29 30 /* Length of delay between homing motions - this is intended to 31 ensure that all motion has ceased and switch bouncing has 32 ended. We might want to make this user adjustable, but for 33 now it's a constant. It is in seconds */ 34 #define HOME_DELAY 0.100 35 36 #define MAX_HOME_SEQUENCES EMCMOT_MAX_JOINTS 37 38 /*********************************************************************** 39 * LOCAL VARIABLE DECLARATIONS * 40 ************************************************************************/ 41 42 // home sequences (some states are required) 43 static home_sequence_state_t sequence_state; 44 static int home_sequence = -1; 45 static bool homing_active; 46 47 /* internal states for homing */ 48 typedef enum { 49 HOME_IDLE = 0, 50 HOME_START,// 1 51 HOME_UNLOCK,// 2 52 HOME_UNLOCK_WAIT,// 3 53 HOME_INITIAL_BACKOFF_START,// 4 54 HOME_INITIAL_BACKOFF_WAIT,// 5 55 HOME_INITIAL_SEARCH_START,// 6 56 HOME_INITIAL_SEARCH_WAIT,// 7 57 HOME_SET_COARSE_POSITION,// 8 58 HOME_FINAL_BACKOFF_START,// 9 59 HOME_FINAL_BACKOFF_WAIT,// 10 60 HOME_RISE_SEARCH_START,// 11 61 HOME_RISE_SEARCH_WAIT,// 12 62 HOME_FALL_SEARCH_START,// 13 63 HOME_FALL_SEARCH_WAIT,// 14 64 HOME_SET_SWITCH_POSITION,// 15 65 HOME_INDEX_ONLY_START,// 16 66 HOME_INDEX_SEARCH_START,// 17 67 HOME_INDEX_SEARCH_WAIT,// 18 68 HOME_SET_INDEX_POSITION,// 19 69 HOME_FINAL_MOVE_START,// 20 70 HOME_FINAL_MOVE_WAIT,// 21 71 HOME_LOCK,// 22 72 HOME_LOCK_WAIT,// 23 73 HOME_FINISHED,// 24 74 HOME_ABORT// 25 75 } home_state_t; 76 77 static int immediate_state; 78 79 // local per-joint data (includes hal pin data) 80 typedef struct { 81 home_state_t home_state; // OUT pin 82 bool homing; // OUT pin 83 bool homed; // OUT pin 84 bool home_sw; // IN pin 85 bool index_enable; // IO pin 86 bool at_home; 87 bool sync_final_move; // joints with neg sequence 88 bool joint_in_sequence; 89 int pause_timer; 90 double home_offset; // intfc, updateable 91 double home; // intfc, updateable 92 double home_final_vel; // intfc 93 double home_search_vel; // intfc 94 double home_latch_vel; // intfc 95 int home_flags; // intfc 96 int home_sequence; // intfc, updateable 97 bool volatile_home; // intfc 98 bool home_is_synchronized; 99 } home_local_data; 100 101 static home_local_data H[EMCMOT_MAX_JOINTS]; 102 103 // data for per-joint homing-specific hal pins: 104 typedef struct { 105 hal_bit_t *home_sw; // home switch input 106 hal_bit_t *homing; // joint is homing 107 hal_bit_t *homed; // joint was homed 108 hal_bit_t *index_enable; // motmod sets: request reset on index 109 // encoder clears: index arrived 110 hal_s32_t *home_state; // homing state machine state 111 } one_joint_home_data_t; 112 113 typedef struct { 114 one_joint_home_data_t jhd[EMCMOT_MAX_JOINTS]; 115 } all_joints_home_data_t; 116 117 static all_joints_home_data_t *joint_home_data = 0; 118 119 /*********************************************************************** 120 * LOCAL FUNCTIONS * 121 ************************************************************************/ 122 123 /* a couple of helper functions with code that would otherwise be 124 repeated in several different states of the homing state machine */ 125 126 /* 'home_start_move()' starts a move at the specified velocity. The 127 length of the move is equal to twice the overall range of the joint, 128 but the intent is that something (like a home switch or index pulse) 129 will stop it before that point. */ 130 static void home_start_move(emcmot_joint_t * joint, double vel) 131 { 132 double joint_range; 133 134 /* set up a long move */ 135 joint_range = joint->max_pos_limit - joint->min_pos_limit; 136 if (vel > 0.0) { 137 joint->free_tp.pos_cmd = joint->pos_cmd + 2.0 * joint_range; 138 } else { 139 joint->free_tp.pos_cmd = joint->pos_cmd - 2.0 * joint_range; 140 } 141 if (fabs(vel) < joint->vel_limit) { 142 joint->free_tp.max_vel = fabs(vel); 143 } else { 144 /* clamp on max vel for this joint */ 145 joint->free_tp.max_vel = joint->vel_limit; 146 } 147 /* start the move */ 148 joint->free_tp.enable = 1; 149 } // home_start_move() 150 151 /* 'home_do_moving_checks()' is called from states where the machine 152 is supposed to be moving. It checks to see if the machine has 153 hit a limit, or if the move has stopped. (Normally such moves 154 will be terminated by the home switch or an index pulse or some 155 other event, if the move goes to completion, something is wrong.) */ 156 static void home_do_moving_checks(emcmot_joint_t * joint,int jno) 157 { 158 /* check for limit switches */ 159 if (joint->on_pos_limit || joint->on_neg_limit) { 160 /* on limit, check to see if we should trip */ 161 if (!(H[jno].home_flags & HOME_IGNORE_LIMITS)) { 162 /* not ignoring limits, time to quit */ 163 reportError(_("hit limit in home state %d"), H[jno].home_state); 164 H[jno].home_state = HOME_ABORT; 165 immediate_state = 1; 166 return; 167 } 168 } 169 /* check for reached end of move */ 170 if (!joint->free_tp.active) { 171 /* reached end of move without hitting switch */ 172 joint->free_tp.enable = 0; 173 reportError(_("end of move in home state %d"), H[jno].home_state); 174 H[jno].home_state = HOME_ABORT; 175 immediate_state = 1; 176 return; 177 } 178 } // home_do_moving_checks() 179 180 static void update_home_is_synchronized(void) { 181 // invoke anytime H[*].home_sequence is altered 182 int jno,joint_num; 183 184 // first, clear all H[*].home_is_synchronized 185 for (jno = 0; jno < ALL_JOINTS; jno++) { 186 H[jno].home_is_synchronized = 0; 187 } 188 for (jno = 0; jno < ALL_JOINTS; jno++) { 189 if (H[jno].home_sequence < 0) { 190 H[jno].home_is_synchronized = 1; 191 continue; 192 } 193 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 194 if (joint_num == jno) continue; 195 if ( ( H[joint_num].home_sequence < 0) 196 && (ABS(H[joint_num].home_sequence) == H[jno].home_sequence) ) { 197 H[jno].home_is_synchronized = 1; 198 H[joint_num].home_is_synchronized = 1; 199 } 200 } 201 } 202 } 203 /*********************************************************************** 204 * PUBLIC FUNCTIONS * 205 ************************************************************************/ 206 207 void homing_init(void) 208 { 209 int i; 210 homing_active = 0; 211 for (i=0; i < EMCMOT_MAX_JOINTS; i++) { 212 H[i].home_state = HOME_IDLE; 213 H[i].home_search_vel = 0; 214 H[i].home_latch_vel = 0; 215 H[i].home_final_vel = 0; 216 H[i].home_offset = 0; 217 H[i].home = 0; 218 H[i].home_flags = 0; 219 H[i].home_sequence = -1; 220 H[i].volatile_home = 0; 221 } 222 } 223 224 int export_joint_home_pins(int njoints,int id) 225 { 226 int jno,retval; 227 one_joint_home_data_t *addr; 228 229 joint_home_data = hal_malloc(sizeof(all_joints_home_data_t)); 230 if (joint_home_data == 0) { 231 rtapi_print_msg(RTAPI_MSG_ERR, _("HOMING: all_joints_home_data_t malloc failed\n")); 232 return -1; 233 } 234 235 retval = 0; 236 for (jno = 0; jno < njoints; jno++) { 237 addr = &(joint_home_data->jhd[jno]); 238 239 if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->home_sw), id, 240 "joint.%d.home-sw-in", jno)) != 0) break; 241 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->homing), id, 242 "joint.%d.homing", jno)) != 0) break; 243 if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->homed), id, 244 "joint.%d.homed", jno)) != 0) break; 245 if ((retval = hal_pin_s32_newf(HAL_OUT, &(addr->home_state), id, 246 "joint.%d.home-state", jno)) != 0) break; 247 if ((retval = hal_pin_bit_newf(HAL_IO, &(addr->index_enable), id, 248 "joint.%d.index-enable", jno)) != 0) break; 249 } 250 return retval; 251 } // export_joint_home_pins() 252 253 void read_homing_in_pins(int njoints) 254 { 255 int jno; 256 one_joint_home_data_t *addr; 257 for (jno = 0; jno < njoints; jno++) { 258 addr = &(joint_home_data->jhd[jno]); 259 H[jno].home_sw = *(addr->home_sw); // IN 260 H[jno].index_enable = *(addr->index_enable); // IO 261 } 262 } 263 264 void write_homing_out_pins(int njoints) 265 { 266 int jno; 267 one_joint_home_data_t *addr; 268 for (jno = 0; jno < njoints; jno++) { 269 addr = &(joint_home_data->jhd[jno]); 270 *(addr->homing) = H[jno].homing; // OUT 271 *(addr->homed) = H[jno].homed; // OUT 272 *(addr->home_state) = H[jno].home_state; // OUT 273 *(addr->index_enable) = H[jno].index_enable; // IO 274 } 275 } 276 277 void set_home_sequence_state(home_sequence_state_t s_state) { 278 sequence_state = s_state; 279 } 280 281 void set_home_abort(int jno) { 282 H[jno].home_state = HOME_ABORT; 283 } 284 285 void set_home_start(int jno) { 286 H[jno].home_state = HOME_START; 287 } 288 289 void set_home_idle(int jno) { 290 H[jno].home_state = HOME_IDLE; 291 } 292 293 void set_joint_homing(int jno,bool value) { 294 H[jno].homing = value; 295 } 296 297 void set_joint_homed(int jno,bool value) { 298 H[jno].homed = value; 299 } 300 301 void set_joint_at_home(int jno, bool value) { 302 H[jno].at_home = value; 303 } 304 305 void set_joint_homing_params(int jno, 306 double offset, 307 double home, 308 double home_final_vel, 309 double home_search_vel, 310 double home_latch_vel, 311 int home_flags, 312 int home_sequence, 313 bool volatile_home 314 ) 315 { 316 H[jno].home_offset = offset; 317 H[jno].home = home; 318 H[jno].home_final_vel = home_final_vel; 319 H[jno].home_search_vel = home_search_vel; 320 H[jno].home_latch_vel = home_latch_vel; 321 H[jno].home_flags = home_flags; 322 H[jno].home_sequence = home_sequence; 323 H[jno].volatile_home = volatile_home; 324 update_home_is_synchronized(); 325 } 326 327 void update_joint_homing_params (int jno, 328 double offset, 329 double home, 330 int home_sequence 331 ) 332 { 333 H[jno].home_offset = offset; 334 H[jno].home = home; 335 H[jno].home_sequence = home_sequence; 336 update_home_is_synchronized(); 337 } 338 339 bool get_homing_is_active() { 340 return homing_active; 341 } 342 343 int get_home_sequence(int jno) { 344 return H[jno].home_sequence; 345 } 346 347 bool get_homing(int jno) { 348 return H[jno].homing; 349 } 350 351 bool get_homed(int jno) { 352 return H[jno].homed; 353 } 354 355 bool get_index_enable(int jno) { 356 return H[jno].index_enable; 357 } 358 359 bool get_home_is_volatile(int jno) { 360 return H[jno].volatile_home; 361 } 362 363 bool get_home_needs_unlock_first(int jno) { 364 return (H[jno].home_flags & HOME_UNLOCK_FIRST) ? 1 : 0; 365 } 366 367 bool get_home_is_idle(int jno) { 368 return H[jno].home_state == HOME_IDLE ? 1 : 0; 369 } 370 371 bool get_homing_at_index_search_wait(int jno) { 372 return H[jno].home_state == HOME_INDEX_SEARCH_WAIT ? 1 : 0; 373 } 374 375 home_sequence_state_t get_home_sequence_state(void) { 376 return sequence_state; 377 } 378 379 // SEQUENCE management 380 void do_homing_sequence(void) 381 { 382 int i,ii; 383 int special_case_sync_all; 384 int seen = 0; 385 emcmot_joint_t *joint; 386 int sequence_is_set = 0; 387 /* first pass init */ 388 if(home_sequence == -1) { 389 sequence_state = HOME_SEQUENCE_IDLE; 390 home_sequence = 0; 391 } 392 393 switch( sequence_state ) { 394 case HOME_SEQUENCE_IDLE: 395 /* nothing to do */ 396 break; 397 398 case HOME_SEQUENCE_DO_ONE_JOINT: 399 // Expect one joint with home_state==HOME_START 400 for (i=0; i < ALL_JOINTS; i++) { 401 joint = &joints[i]; 402 if (H[i].home_state == HOME_START) { 403 H[i].joint_in_sequence = 1; 404 home_sequence = ABS(H[i].home_sequence); 405 } else { 406 if (H[i].joint_in_sequence) { 407 // it may already be running, leave alone 408 } else { 409 H[i].joint_in_sequence = 0; 410 } 411 } 412 } 413 sequence_is_set = 1; 414 //drop through----drop through----drop through----drop through 415 416 case HOME_SEQUENCE_DO_ONE_SEQUENCE: 417 // Expect multiple joints with home_state==HOME_START 418 // specified by a negative sequence 419 // Determine home_sequence and set H[i].joint_in_sequence 420 // based on home_state[i] == HOME_START 421 if (!sequence_is_set) { 422 for (i=0; i < ALL_JOINTS; i++) { 423 joint = &joints[i]; 424 if (H[i].home_state == HOME_START) { 425 if ( sequence_is_set 426 && (ABS(H[i].home_sequence) != home_sequence)) { 427 reportError( 428 "homing.c Unexpected joint=%d jsequence=%d seq=%d\n" 429 ,i,H[i].home_sequence,home_sequence); 430 } 431 home_sequence = ABS(H[i].home_sequence); 432 sequence_is_set = 1; 433 } 434 H[i].joint_in_sequence = 1; //disprove 435 if ( (H[i].home_state != HOME_START) 436 || (home_sequence != ABS(H[i].home_sequence)) 437 ) { 438 H[i].joint_in_sequence = 0; 439 } 440 } 441 } 442 sequence_state = HOME_SEQUENCE_START; 443 444 //drop through----drop through----drop through----drop through 445 446 case HOME_SEQUENCE_START: 447 // Request to home all joints or a single sequence 448 // A negative H[i].home_sequence means sync final move 449 if (!sequence_is_set) { 450 // sequence_is_set not otherwise established: home-all 451 for (i=0; i < EMCMOT_MAX_JOINTS; i++) { 452 joint = &joints[i]; 453 H[i].joint_in_sequence = 1; 454 // unspecified joints have an unrealizable home_sequence: 455 if (H[i].home_sequence >100) { 456 // docs: 'If HOME_SEQUENCE is not specified then this joint 457 // will not be homed by the HOME ALL sequence' 458 H[i].joint_in_sequence = 0; // per docs 459 } 460 } 461 sequence_is_set = 1; 462 home_sequence = 0; 463 } 464 /* Initializations */ 465 for(i=0; i < MAX_HOME_SEQUENCES; i++) { 466 H[i].sync_final_move = 0; //reset to allow a rehome 467 } 468 for(i=0; i < ALL_JOINTS; i++) { 469 if (!H[i].joint_in_sequence) continue; 470 joint = &joints[i]; 471 if ( (H[i].home_flags & HOME_NO_REHOME) 472 && H[i].homed 473 ) { 474 continue; 475 } else { 476 H[i].homed = 0; 477 } 478 if (H[i].home_sequence < 0) { 479 // if a H[i].home_sequence is neg, find all joints that 480 // have the same ABS sequence value and make them the same 481 for(ii=0; ii < ALL_JOINTS; ii++) { 482 if (H[ii].home_sequence == ABS(H[i].home_sequence)) { 483 H[ii].home_sequence = H[i].home_sequence; 484 } 485 } 486 } 487 } 488 /* special_case_sync_all: if home_sequence == -1 for all joints 489 * synchronize all joints final move 490 */ 491 special_case_sync_all = 1; // disprove 492 for(i=0; i < ALL_JOINTS; i++) { 493 joint = &joints[i]; 494 if (H[i].home_sequence != -1) {special_case_sync_all = 0;} 495 } 496 if (special_case_sync_all) { 497 home_sequence = 1; 498 } 499 for(i=0; i < ALL_JOINTS; i++) { 500 if (!H[i].joint_in_sequence) continue; 501 joint = &joints[i]; 502 if ( H[i].home_state != HOME_IDLE && H[i].home_state != HOME_START) { 503 /* a home is already in progress, abort the home-all */ 504 sequence_state = HOME_SEQUENCE_IDLE; 505 return; 506 } 507 } 508 /* tell the world we're on the job */ 509 homing_active = 1; 510 //drop through----drop through----drop through----drop through 511 512 case HOME_SEQUENCE_START_JOINTS: 513 /* start all joints whose sequence number matches home_sequence */ 514 for(i=0; i < ALL_JOINTS; i++) { 515 joint = &joints[i]; 516 if(ABS(H[i].home_sequence) == home_sequence) { 517 if (!H[i].joint_in_sequence) continue; 518 /* start this joint */ 519 joint->free_tp.enable = 0; 520 H[i].home_state = HOME_START; 521 seen++; 522 } 523 } 524 if (seen || home_sequence == 0) { 525 sequence_state = HOME_SEQUENCE_WAIT_JOINTS; 526 } else { 527 /* no joints have this sequence number, we're done */ 528 sequence_state = HOME_SEQUENCE_IDLE; 529 /* tell the world */ 530 homing_active = 0; 531 } 532 break; 533 534 case HOME_SEQUENCE_WAIT_JOINTS: 535 for(i=0; i < ALL_JOINTS; i++) { 536 if (!H[i].joint_in_sequence) continue; 537 joint = &joints[i]; 538 // negative H[i].home_sequence means sync final move 539 if(ABS(H[i].home_sequence) != home_sequence) { 540 /* this joint is not at the current sequence number, ignore it */ 541 continue; 542 } 543 if(H[i].home_state != HOME_IDLE) { 544 /* still busy homing, keep waiting */ 545 seen = 1; 546 continue; 547 } 548 if (!H[i].at_home) { 549 /* joint should have been homed at this step, it is no longer 550 homing, but its not at home - must have failed. bail out */ 551 sequence_state = HOME_SEQUENCE_IDLE; 552 homing_active = 0; 553 return; 554 } 555 } 556 if(!seen) { 557 /* all joints at this step have finished, move on to next step */ 558 home_sequence ++; 559 sequence_state = HOME_SEQUENCE_START_JOINTS; 560 } 561 break; 562 563 default: 564 /* should never get here */ 565 reportError(_("unknown state '%d' during homing sequence"), 566 sequence_state); 567 sequence_state = HOME_SEQUENCE_IDLE; 568 homing_active = 0; 569 break; 570 } 571 } // do_homing_sequence() 572 573 // HOMING management 574 void do_homing(void) 575 { 576 int joint_num; 577 emcmot_joint_t *joint; 578 double offset, tmp; 579 int home_sw_active, homing_flag; 580 581 homing_flag = 0; 582 if (emcmotStatus->motion_state != EMCMOT_MOTION_FREE) { 583 /* can't home unless in free mode */ 584 return; 585 } 586 /* loop thru joints, treat each one individually */ 587 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 588 if (!H[joint_num].joint_in_sequence) continue; 589 /* point to joint struct */ 590 joint = &joints[joint_num]; 591 if (!GET_JOINT_ACTIVE_FLAG(joint)) { 592 /* if joint is not active, skip it */ 593 continue; 594 } 595 home_sw_active = H[joint_num].home_sw; 596 if (H[joint_num].home_state != HOME_IDLE) { 597 homing_flag = 1; /* at least one joint is homing */ 598 } 599 600 /* when an joint is homing, 'check_for_faults()' ignores its limit 601 switches, so that this code can do the right thing with them. Once 602 the homing process is finished, the 'check_for_faults()' resumes 603 checking */ 604 605 /* homing state machine */ 606 607 /* Some portions of the homing sequence can run thru two or more 608 states during a single servo period. This is done using 609 'immediate_state'. If a state transition sets it true (non-zero), 610 this 'do-while' will loop executing switch(home_state) immediately 611 to run the new state code. Otherwise, the loop will fall thru, and 612 switch(home_state) runs only once per servo period. Do _not_ set 613 'immediate_state' true unless you also change 'home_state', unless 614 you want an infinite loop! */ 615 do { 616 immediate_state = 0; 617 switch (H[joint_num].home_state) { 618 case HOME_IDLE: 619 /* nothing to do */ 620 break; 621 622 case HOME_START: 623 /* This state is responsible for getting the homing process 624 started. It doesn't actually do anything, it simply 625 determines what state is next */ 626 if (H[joint_num].home_flags & HOME_IS_SHARED && home_sw_active) { 627 reportError( 628 _("Cannot home while shared home switch is closed j=%d"), 629 joint_num); 630 H[joint_num].home_state = HOME_IDLE; 631 break; 632 } 633 /* set flags that communicate with the rest of EMC */ 634 if ( (H[joint_num].home_flags & HOME_NO_REHOME) 635 && H[joint_num].homed 636 ) { 637 H[joint_num].home_state = HOME_IDLE; 638 break; //no rehome allowed if absolute_enoder 639 } else { 640 H[joint_num].homing = 1; 641 H[joint_num].homed = 0; 642 } 643 H[joint_num].at_home = 0; 644 /* stop any existing motion */ 645 joint->free_tp.enable = 0; 646 /* reset delay counter */ 647 H[joint_num].pause_timer = 0; 648 /* figure out exactly what homing sequence is needed */ 649 if (H[joint_num].home_flags & HOME_ABSOLUTE_ENCODER) { 650 H[joint_num].home_flags &= ~HOME_IS_SHARED; // shared not applicable 651 H[joint_num].home_state = HOME_SET_SWITCH_POSITION; 652 immediate_state = 1; 653 H[joint_num].homed = 1; 654 break; 655 } 656 if (H[joint_num].home_flags & HOME_UNLOCK_FIRST) { 657 H[joint_num].home_state = HOME_UNLOCK; 658 } else { 659 H[joint_num].home_state = HOME_UNLOCK_WAIT; 660 immediate_state = 1; 661 } 662 break; 663 664 case HOME_UNLOCK: 665 // unlock now 666 emcmotSetRotaryUnlock(joint_num, 1); 667 H[joint_num].home_state = HOME_UNLOCK_WAIT; 668 break; 669 670 case HOME_UNLOCK_WAIT: 671 // if not yet unlocked, continue waiting 672 if ((H[joint_num].home_flags & HOME_UNLOCK_FIRST) && 673 !emcmotGetRotaryIsUnlocked(joint_num)) break; 674 675 // either we got here without an unlock needed, or the 676 // unlock is now complete. 677 if (H[joint_num].home_search_vel == 0.0) { 678 if (H[joint_num].home_latch_vel == 0.0) { 679 /* both vels == 0 means home at current position */ 680 H[joint_num].home_state = HOME_SET_SWITCH_POSITION; 681 immediate_state = 1; 682 } else if (H[joint_num].home_flags & HOME_USE_INDEX) { 683 /* home using index pulse only */ 684 H[joint_num].home_state = HOME_INDEX_ONLY_START; 685 immediate_state = 1; 686 } else { 687 reportError(_("invalid homing config: non-zero LATCH_VEL needs either SEARCH_VEL or USE_INDEX")); 688 H[joint_num].home_state = HOME_IDLE; 689 } 690 } else { 691 if (H[joint_num].home_latch_vel != 0.0) { 692 /* need to find home switch */ 693 H[joint_num].home_state = HOME_INITIAL_SEARCH_START; 694 immediate_state = 1; 695 } else { 696 reportError(_("invalid homing config: non-zero SEARCH_VEL needs LATCH_VEL")); 697 H[joint_num].home_state = HOME_IDLE; 698 } 699 } 700 break; 701 702 case HOME_INITIAL_BACKOFF_START: 703 /* This state is called if the homing sequence starts at a 704 location where the home switch is already tripped. It 705 starts a move away from the switch. */ 706 /* is the joint still moving? */ 707 if (joint->free_tp.active) { 708 /* yes, reset delay, wait until joint stops */ 709 H[joint_num].pause_timer = 0; 710 break; 711 } 712 /* has delay timed out? */ 713 if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { 714 /* no, update timer and wait some more */ 715 H[joint_num].pause_timer++; 716 break; 717 } 718 H[joint_num].pause_timer = 0; 719 /* set up a move at '-search_vel' to back off of switch */ 720 home_start_move(joint, - H[joint_num].home_search_vel); 721 /* next state */ 722 H[joint_num].home_state = HOME_INITIAL_BACKOFF_WAIT; 723 break; 724 725 case HOME_INITIAL_BACKOFF_WAIT: 726 /* This state is called while the machine is moving off of 727 the home switch. It terminates when the switch is cleared 728 successfully. If the move ends or hits a limit before it 729 clears the switch, the home is aborted. */ 730 /* are we off home switch yet? */ 731 if (! home_sw_active) { 732 /* yes, stop motion */ 733 joint->free_tp.enable = 0; 734 /* begin initial search */ 735 H[joint_num].home_state = HOME_INITIAL_SEARCH_START; 736 immediate_state = 1; 737 break; 738 } 739 home_do_moving_checks(joint,joint_num); 740 break; 741 742 case HOME_INITIAL_SEARCH_START: 743 /* This state is responsible for starting a move toward the 744 home switch. This move is at 'search_vel', which can be 745 fairly fast, because once the switch is found another 746 slower move will be used to set the exact home position. */ 747 /* is the joint already moving? */ 748 if (joint->free_tp.active) { 749 /* yes, reset delay, wait until joint stops */ 750 H[joint_num].pause_timer = 0; 751 break; 752 } 753 /* has delay timed out? */ 754 if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { 755 /* no, update timer and wait some more */ 756 H[joint_num].pause_timer++; 757 break; 758 } 759 H[joint_num].pause_timer = 0; 760 /* make sure we aren't already on home switch */ 761 if (home_sw_active) { 762 /* already on switch, need to back off it first */ 763 H[joint_num].home_state = HOME_INITIAL_BACKOFF_START; 764 immediate_state = 1; 765 break; 766 } 767 /* set up a move at 'search_vel' to find switch */ 768 home_start_move(joint, H[joint_num].home_search_vel); 769 /* next state */ 770 H[joint_num].home_state = HOME_INITIAL_SEARCH_WAIT; 771 break; 772 773 case HOME_INITIAL_SEARCH_WAIT: 774 /* This state is called while the machine is looking for the 775 home switch. It terminates when the switch is found. If 776 the move ends or hits a limit before it finds the switch, 777 the home is aborted. */ 778 /* have we hit home switch yet? */ 779 if (home_sw_active) { 780 /* yes, stop motion */ 781 joint->free_tp.enable = 0; 782 /* go to next step */ 783 H[joint_num].home_state = HOME_SET_COARSE_POSITION; 784 immediate_state = 1; 785 break; 786 } 787 home_do_moving_checks(joint,joint_num); 788 break; 789 790 case HOME_SET_COARSE_POSITION: 791 /* This state is called after the first time the switch is 792 found. At this point, we are approximately home. Although 793 we will do another slower pass to get the exact home 794 location, we reset the joint coordinates now so that screw 795 error comp will be appropriate for this portion of the 796 screw (previously we didn't know where we were at all). */ 797 /* set the current position to 'home_offset' */ 798 offset = H[joint_num].home_offset - joint->pos_fb; 799 /* this moves the internal position but does not affect the 800 motor position */ 801 joint->pos_cmd += offset; 802 joint->pos_fb += offset; 803 joint->free_tp.curr_pos += offset; 804 joint->motor_offset -= offset; 805 /* The next state depends on the signs of 'search_vel' and 806 'latch_vel'. If they are the same, that means we must 807 back up, then do the final homing moving the same 808 direction as the initial search, on a rising edge of the 809 switch. If they are opposite, it means that the final 810 homing will take place on a falling edge as the machine 811 moves off of the switch. */ 812 tmp = H[joint_num].home_search_vel * H[joint_num].home_latch_vel; 813 if (tmp > 0.0) { 814 /* search and latch vel are same direction */ 815 H[joint_num].home_state = HOME_FINAL_BACKOFF_START; 816 } else { 817 /* search and latch vel are opposite directions */ 818 H[joint_num].home_state = HOME_FALL_SEARCH_START; 819 } 820 immediate_state = 1; 821 break; 822 823 case HOME_FINAL_BACKOFF_START: 824 /* This state is called once the approximate location of the 825 switch has been found. It is responsible for starting a 826 move that will back off of the switch in preparation for a 827 final slow move that captures the exact switch location. */ 828 /* is the joint already moving? */ 829 if (joint->free_tp.active) { 830 /* yes, reset delay, wait until joint stops */ 831 H[joint_num].pause_timer = 0; 832 break; 833 } 834 /* has delay timed out? */ 835 if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { 836 /* no, update timer and wait some more */ 837 H[joint_num].pause_timer++; 838 break; 839 } 840 H[joint_num].pause_timer = 0; 841 /* we should still be on the switch */ 842 if (! home_sw_active) { 843 reportError( 844 _("Home switch inactive before start of backoff move j=%d"), 845 joint_num); 846 H[joint_num].home_state = HOME_IDLE; 847 break; 848 } 849 /* set up a move at '-search_vel' to back off of switch */ 850 home_start_move(joint, - H[joint_num].home_search_vel); 851 /* next state */ 852 H[joint_num].home_state = HOME_FINAL_BACKOFF_WAIT; 853 break; 854 855 case HOME_FINAL_BACKOFF_WAIT: 856 /* This state is called while the machine is moving off of 857 the home switch after finding its approximate location. 858 It terminates when the switch is cleared successfully. If 859 the move ends or hits a limit before it clears the switch, 860 the home is aborted. */ 861 /* are we off home switch yet? */ 862 if (! home_sw_active) { 863 /* yes, stop motion */ 864 joint->free_tp.enable = 0; 865 /* begin final search */ 866 H[joint_num].home_state = HOME_RISE_SEARCH_START; 867 immediate_state = 1; 868 break; 869 } 870 home_do_moving_checks(joint,joint_num); 871 break; 872 873 case HOME_RISE_SEARCH_START: 874 /* This state is called to start the final search for the 875 point where the home switch trips. It moves at 876 'latch_vel' and looks for a rising edge on the switch */ 877 /* is the joint already moving? */ 878 if (joint->free_tp.active) { 879 /* yes, reset delay, wait until joint stops */ 880 H[joint_num].pause_timer = 0; 881 break; 882 } 883 /* has delay timed out? */ 884 if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { 885 /* no, update timer and wait some more */ 886 H[joint_num].pause_timer++; 887 break; 888 } 889 H[joint_num].pause_timer = 0; 890 /* we should still be off of the switch */ 891 if (home_sw_active) { 892 reportError( 893 _("Home switch active before start of latch move j=%d"), 894 joint_num); 895 H[joint_num].home_state = HOME_IDLE; 896 break; 897 } 898 /* set up a move at 'latch_vel' to locate the switch */ 899 home_start_move(joint, H[joint_num].home_latch_vel); 900 /* next state */ 901 H[joint_num].home_state = HOME_RISE_SEARCH_WAIT; 902 break; 903 904 case HOME_RISE_SEARCH_WAIT: 905 /* This state is called while the machine is moving towards 906 the home switch on its final, low speed pass. It 907 terminates when the switch is detected. If the move ends 908 or hits a limit before it hits the switch, the home is 909 aborted. */ 910 /* have we hit the home switch yet? */ 911 if (home_sw_active) { 912 /* yes, where do we go next? */ 913 if (H[joint_num].home_flags & HOME_USE_INDEX) { 914 /* look for index pulse */ 915 H[joint_num].home_state = HOME_INDEX_SEARCH_START; 916 immediate_state = 1; 917 break; 918 } else { 919 /* no index pulse, stop motion */ 920 joint->free_tp.enable = 0; 921 /* go to next step */ 922 H[joint_num].home_state = HOME_SET_SWITCH_POSITION; 923 immediate_state = 1; 924 break; 925 } 926 } 927 home_do_moving_checks(joint,joint_num); 928 break; 929 930 case HOME_FALL_SEARCH_START: 931 /* This state is called to start the final search for the 932 point where the home switch releases. It moves at 933 'latch_vel' and looks for a falling edge on the switch */ 934 /* is the joint already moving? */ 935 if (joint->free_tp.active) { 936 /* yes, reset delay, wait until joint stops */ 937 H[joint_num].pause_timer = 0; 938 break; 939 } 940 /* has delay timed out? */ 941 if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { 942 /* no, update timer and wait some more */ 943 H[joint_num].pause_timer++; 944 break; 945 } 946 H[joint_num].pause_timer = 0; 947 /* we should still be on the switch */ 948 if (!home_sw_active) { 949 reportError( 950 _("Home switch inactive before start of latch move j=%d"), 951 joint_num); 952 H[joint_num].home_state = HOME_IDLE; 953 break; 954 } 955 /* set up a move at 'latch_vel' to locate the switch */ 956 home_start_move(joint, H[joint_num].home_latch_vel); 957 /* next state */ 958 H[joint_num].home_state = HOME_FALL_SEARCH_WAIT; 959 break; 960 961 case HOME_FALL_SEARCH_WAIT: 962 /* This state is called while the machine is moving away from 963 the home switch on its final, low speed pass. It 964 terminates when the switch is cleared. If the move ends or 965 hits a limit before it clears the switch, the home is 966 aborted. */ 967 /* have we cleared the home switch yet? */ 968 if (!home_sw_active) { 969 /* yes, where do we go next? */ 970 if (H[joint_num].home_flags & HOME_USE_INDEX) { 971 /* look for index pulse */ 972 H[joint_num].home_state = HOME_INDEX_SEARCH_START; 973 immediate_state = 1; 974 break; 975 } else { 976 /* no index pulse, stop motion */ 977 joint->free_tp.enable = 0; 978 /* go to next step */ 979 H[joint_num].home_state = HOME_SET_SWITCH_POSITION; 980 immediate_state = 1; 981 break; 982 } 983 } 984 home_do_moving_checks(joint,joint_num); 985 break; 986 987 case HOME_SET_SWITCH_POSITION: 988 /* This state is called when the machine has determined the 989 switch position as accurately as possible. It sets the 990 current joint position to 'home_offset', which is the 991 location of the home switch in joint coordinates. */ 992 /* set the current position to 'home_offset' */ 993 if (H[joint_num].home_flags & HOME_ABSOLUTE_ENCODER) { 994 offset = H[joint_num].home_offset; 995 } else { 996 offset = H[joint_num].home_offset - joint->pos_fb; 997 } 998 /* this moves the internal position but does not affect the 999 motor position */ 1000 joint->pos_cmd += offset; 1001 joint->pos_fb += offset; 1002 joint->free_tp.curr_pos += offset; 1003 joint->motor_offset -= offset; 1004 if (H[joint_num].home_flags & HOME_ABSOLUTE_ENCODER) { 1005 if (H[joint_num].home_flags & HOME_NO_FINAL_MOVE) { 1006 H[joint_num].home_state = HOME_FINISHED; 1007 immediate_state = 1; 1008 H[joint_num].homed = 1; 1009 break; 1010 } 1011 } 1012 /* next state */ 1013 H[joint_num].home_state = HOME_FINAL_MOVE_START; 1014 immediate_state = 1; 1015 break; 1016 1017 case HOME_INDEX_ONLY_START: 1018 /* This state is used if the machine has been pre-positioned 1019 near the home position, and simply needs to find the 1020 next index pulse. It starts a move at latch_vel, and 1021 sets index-enable, which tells the encoder driver to 1022 reset its counter to zero and clear the enable when the 1023 next index pulse arrives. */ 1024 /* is the joint already moving? */ 1025 if (joint->free_tp.active) { 1026 /* yes, reset delay, wait until joint stops */ 1027 H[joint_num].pause_timer = 0; 1028 break; 1029 } 1030 /* has delay timed out? */ 1031 if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { 1032 /* no, update timer and wait some more */ 1033 H[joint_num].pause_timer++; 1034 break; 1035 } 1036 H[joint_num].pause_timer = 0; 1037 /* Although we don't know the exact home position yet, we 1038 we reset the joint coordinates now so that screw error 1039 comp will be appropriate for this portion of the screw 1040 (previously we didn't know where we were at all). */ 1041 /* set the current position to 'home_offset' */ 1042 offset = H[joint_num].home_offset - joint->pos_fb; 1043 /* this moves the internal position but does not affect the 1044 motor position */ 1045 joint->pos_cmd += offset; 1046 joint->pos_fb += offset; 1047 joint->free_tp.curr_pos += offset; 1048 joint->motor_offset -= offset; 1049 /* set the index enable */ 1050 H[joint_num].index_enable = 1; 1051 /* set up a move at 'latch_vel' to find the index pulse */ 1052 home_start_move(joint, H[joint_num].home_latch_vel); 1053 /* next state */ 1054 H[joint_num].home_state = HOME_INDEX_SEARCH_WAIT; 1055 break; 1056 1057 case HOME_INDEX_SEARCH_START: 1058 /* This state is called after the machine has made a low 1059 speed pass to determine the limit switch location. It 1060 sets index-enable, which tells the encoder driver to 1061 reset its counter to zero and clear the enable when the 1062 next index pulse arrives. */ 1063 /* set the index enable */ 1064 H[joint_num].index_enable = 1; 1065 /* and move right into the waiting state */ 1066 H[joint_num].home_state = HOME_INDEX_SEARCH_WAIT; 1067 immediate_state = 1; 1068 home_do_moving_checks(joint,joint_num); 1069 break; 1070 1071 case HOME_INDEX_SEARCH_WAIT: 1072 /* This state is called after the machine has found the 1073 home switch and "armed" the encoder counter to reset on 1074 the next index pulse. It continues at low speed until 1075 an index pulse is detected, at which point it sets the 1076 final home position. If the move ends or hits a limit 1077 before an index pulse occurs, the home is aborted. */ 1078 /* has an index pulse arrived yet? encoder driver clears 1079 enable when it does */ 1080 if ( H[joint_num].index_enable == 0 ) { 1081 /* yes, stop motion */ 1082 joint->free_tp.enable = 0; 1083 /* go to next step */ 1084 H[joint_num].home_state = HOME_SET_INDEX_POSITION; 1085 immediate_state = 1; 1086 break; 1087 } 1088 home_do_moving_checks(joint,joint_num); 1089 break; 1090 1091 case HOME_SET_INDEX_POSITION: 1092 /* This state is called when the encoder has been reset at 1093 the index pulse position. It sets the current joint 1094 position to 'home_offset', which is the location of the 1095 index pulse in joint coordinates. */ 1096 /* set the current position to 'home_offset' */ 1097 joint->motor_offset = - H[joint_num].home_offset; 1098 joint->pos_fb = joint->motor_pos_fb - 1099 (joint->backlash_filt + joint->motor_offset); 1100 joint->pos_cmd = joint->pos_fb; 1101 joint->free_tp.curr_pos = joint->pos_fb; 1102 1103 if (H[joint_num].home_flags & HOME_INDEX_NO_ENCODER_RESET) { 1104 /* Special case: encoder does not reset on index pulse. 1105 This moves the internal position but does not affect 1106 the motor position */ 1107 offset = H[joint_num].home_offset - joint->pos_fb; 1108 joint->pos_cmd += offset; 1109 joint->pos_fb += offset; 1110 joint->free_tp.curr_pos += offset; 1111 joint->motor_offset -= offset; 1112 } 1113 1114 /* next state */ 1115 H[joint_num].home_state = HOME_FINAL_MOVE_START; 1116 immediate_state = 1; 1117 break; 1118 1119 case HOME_FINAL_MOVE_START: 1120 /* This state is called once the joint coordinate system is 1121 set properly. It moves to the actual 'home' position, 1122 which is not neccessarily the position of the home switch 1123 or index pulse. */ 1124 /* is the joint already moving? */ 1125 if (joint->free_tp.active) { 1126 /* yes, reset delay, wait until joint stops */ 1127 H[joint_num].pause_timer = 0; 1128 break; 1129 } 1130 /* has delay timed out? */ 1131 if (H[joint_num].pause_timer < (HOME_DELAY * servo_freq)) { 1132 /* no, update timer and wait some more */ 1133 H[joint_num].pause_timer++; 1134 if (H[joint_num].home_sequence < 0) { 1135 if (!H[home_sequence].sync_final_move) break; 1136 } else { 1137 break; 1138 } 1139 1140 } 1141 // negative H[joint_num].home_sequence means sync final move 1142 // defer final move until all joints in sequence are ready 1143 if ( (H[joint_num].home_sequence < 0) 1144 && ( ABS(H[joint_num].home_sequence) == home_sequence) 1145 ) { 1146 if (!H[home_sequence].sync_final_move) { 1147 int jno; 1148 emcmot_joint_t *jtmp; 1149 H[home_sequence].sync_final_move = 1; //disprove 1150 for (jno = 0; jno < ALL_JOINTS; jno++) { 1151 jtmp = &joints[jno]; 1152 if (!H[jno].joint_in_sequence) continue; 1153 if (ABS(H[jno].home_sequence) != home_sequence) {continue;} 1154 if (H[jno].home_flags & HOME_ABSOLUTE_ENCODER) {continue;} 1155 if ( (H[jno].home_state != HOME_FINAL_MOVE_START) 1156 || 1157 (jtmp->free_tp.active) 1158 ) { 1159 H[home_sequence].sync_final_move = 0; 1160 break; 1161 } 1162 } 1163 if (!H[home_sequence].sync_final_move) break; 1164 } 1165 } 1166 H[joint_num].pause_timer = 0; 1167 /* plan a move to home position */ 1168 joint->free_tp.pos_cmd = H[joint_num].home; 1169 /* if home_vel is set (>0) then we use that, otherwise we rapid there */ 1170 if (H[joint_num].home_final_vel > 0) { 1171 joint->free_tp.max_vel = fabs(H[joint_num].home_final_vel); 1172 /* clamp on max vel for this joint */ 1173 if (joint->free_tp.max_vel > joint->vel_limit) 1174 joint->free_tp.max_vel = joint->vel_limit; 1175 } else { 1176 joint->free_tp.max_vel = joint->vel_limit; 1177 } 1178 /* start the move */ 1179 joint->free_tp.enable = 1; 1180 H[joint_num].home_state = HOME_FINAL_MOVE_WAIT; 1181 break; 1182 1183 case HOME_FINAL_MOVE_WAIT: 1184 /* This state is called while the machine makes its final 1185 move to the home position. It terminates when the machine 1186 arrives at the final location. If the move hits a limit 1187 before it arrives, the home is aborted. */ 1188 /* have we arrived (and stopped) at home? */ 1189 if (!joint->free_tp.active) { 1190 /* yes, stop motion */ 1191 joint->free_tp.enable = 0; 1192 /* we're finally done */ 1193 H[joint_num].home_state = HOME_LOCK; 1194 immediate_state = 1; 1195 break; 1196 } 1197 if (joint->on_pos_limit || joint->on_neg_limit) { 1198 /* on limit, check to see if we should trip */ 1199 if (!(H[joint_num].home_flags & HOME_IGNORE_LIMITS)) { 1200 /* not ignoring limits, time to quit */ 1201 reportError(_("hit limit in home state j=%d"),joint_num); 1202 H[joint_num].home_state = HOME_ABORT; 1203 immediate_state = 1; 1204 break; 1205 } 1206 } 1207 break; 1208 1209 case HOME_LOCK: 1210 if (H[joint_num].home_flags & HOME_UNLOCK_FIRST) { 1211 emcmotSetRotaryUnlock(joint_num, 0); 1212 } else { 1213 immediate_state = 1; 1214 } 1215 H[joint_num].home_state = HOME_LOCK_WAIT; 1216 break; 1217 1218 case HOME_LOCK_WAIT: 1219 // if not yet locked, continue waiting 1220 if ((H[joint_num].home_flags & HOME_UNLOCK_FIRST) && 1221 emcmotGetRotaryIsUnlocked(joint_num)) break; 1222 1223 // either we got here without a lock needed, or the 1224 // lock is now complete. 1225 H[joint_num].home_state = HOME_FINISHED; 1226 immediate_state = 1; 1227 break; 1228 1229 case HOME_FINISHED: 1230 H[joint_num].homing = 0; 1231 H[joint_num].homed = 1; 1232 H[joint_num].at_home = 1; 1233 H[joint_num].home_state = HOME_IDLE; 1234 immediate_state = 1; 1235 H[joint_num].joint_in_sequence = 0; 1236 // This joint just finished homing. See if this is the 1237 // final one and all joints are now homed, and switch to 1238 // Teleop mode if so. 1239 if (checkAllHomed()) { // Note: not in homing api 1240 switch_to_teleop_mode(); 1241 homing_flag = 0; 1242 } 1243 break; 1244 1245 case HOME_ABORT: 1246 H[joint_num].homing = 0; 1247 H[joint_num].homed = 0; 1248 H[joint_num].at_home = 0; 1249 H[joint_num].joint_in_sequence = 0; 1250 joint->free_tp.enable = 0; 1251 H[joint_num].home_state = HOME_IDLE; 1252 H[joint_num].index_enable = 0; 1253 immediate_state = 1; 1254 break; 1255 1256 default: 1257 /* should never get here */ 1258 reportError(_("unknown state '%d' during homing j=%d"), 1259 H[joint_num].home_state,joint_num); 1260 H[joint_num].home_state = HOME_ABORT; 1261 immediate_state = 1; 1262 break; 1263 } /* end of switch(H[joint_num].home_state) */ 1264 } while (immediate_state); 1265 } /* end of loop through all joints */ 1266 1267 if ( homing_flag ) { 1268 /* at least one joint is homing, set global flag */ 1269 homing_active = 1; 1270 } else { 1271 /* is a homing sequence in progress? */ 1272 if (sequence_state == HOME_SEQUENCE_IDLE) { 1273 /* no, single joint only, we're done */ 1274 homing_active = 0; 1275 } 1276 } 1277 } // do_homing() 1278 1279 bool get_home_is_synchronized(int jno) { 1280 return H[jno].home_is_synchronized; 1281 }