control.c
1 /******************************************************************** 2 * Description: control.c 3 * emcmotController() is the main loop running at the servo cycle 4 * rate. All state logic and trajectory calcs are called from here. 5 * 6 * Derived from a work by Fred Proctor & Will Shackleford 7 * 8 * Author: 9 * License: GPL Version 2 10 * Created on: 11 * System: Linux 12 * 13 * Copyright (c) 2004 All rights reserved. 14 ********************************************************************/ 15 16 #include "posemath.h" 17 #include "rtapi.h" 18 #include "hal.h" 19 #include "motion.h" 20 #include "mot_priv.h" 21 #include "rtapi_math.h" 22 #include "tp.h" 23 #include "tc.h" 24 #include "simple_tp.h" 25 #include "motion_debug.h" 26 #include "config.h" 27 #include "motion_types.h" 28 #include "homing.h" 29 30 // Mark strings for translation, but defer translation to userspace 31 #define _(s) (s) 32 static int ext_offset_teleop_limit = 0; 33 static int ext_offset_coord_limit = 0; 34 static double ext_offset_epsilon; 35 /* kinematics flags */ 36 KINEMATICS_FORWARD_FLAGS fflags = 0; 37 KINEMATICS_INVERSE_FLAGS iflags = 0; 38 39 /* 1/servo cycle time */ 40 double servo_freq; 41 42 /*! \todo FIXME - debugging - uncomment the following line to log changes in 43 JOINT_FLAG and MOTION_FLAG */ 44 // #define WATCH_FLAGS 1 45 46 47 /*********************************************************************** 48 * LOCAL VARIABLE DECLARATIONS * 49 ************************************************************************/ 50 51 /* the (nominal) period the last time the motion handler was invoked */ 52 static unsigned long last_period = 0; 53 54 /* servo cycle time */ 55 static double servo_period; 56 57 extern struct emcmot_status_t *emcmotStatus; 58 59 // *pcmd_p[0] is shorthand for emcmotStatus->carte_pos_cmd.tran.x 60 // *pcmd_p[1] is shorthand for emcmotStatus->carte_pos_cmd.tran.y 61 // etc. 62 static double *pcmd_p[EMCMOT_MAX_AXIS]; 63 64 /*********************************************************************** 65 * LOCAL FUNCTION PROTOTYPES * 66 ************************************************************************/ 67 68 /* the following functions are called (in this order) by the main 69 controller function. They are an attempt to break the huge 70 function (originally 1600 lines) into something a little easier 71 to understand. 72 */ 73 74 /* 'process_inputs()' is responsible for reading hardware input 75 signals (from the HAL) and doing basic processing on them. In 76 the case of position feedback, that means removing backlash or 77 screw error comp and calculating the following error. For 78 switches, it means debouncing them and setting flags in the 79 emcmotStatus structure. 80 */ 81 static void process_inputs(void); 82 83 /* 'do forward kins()' takes the position feedback in joint coords 84 and applies the forward kinematics to it to generate feedback 85 in Cartesean coordinates. It has code to handle machines that 86 don't have forward kins, and other special cases, such as when 87 the joints have not been homed. 88 */ 89 static void do_forward_kins(void); 90 91 /* probe inputs need to be handled after forward kins are run, since 92 cartesian feedback position is latched when the probe fires, and it 93 should be based on the feedback read in on this servo cycle. 94 */ 95 static void process_probe_inputs(void); 96 97 /* 'check_for_faults()' is responsible for detecting fault conditions 98 such as limit switches, amp faults, following error, etc. It only 99 checks active axes. It is also responsible for generating an error 100 message. (Later, once I understand the cmd/status/error interface 101 better, it will probably generate error codes that can be passed 102 up the architecture toward the GUI - printing error messages 103 directly seems a little messy) 104 */ 105 static void check_for_faults(void); 106 107 /* 'set_operating_mode()' handles transitions between the operating 108 modes, which are free, coordinated, and teleop. This stuff needs 109 to be better documented. It is basically a state machine, with 110 a current state, a desired state, and rules determining when the 111 state can change. It should be rewritten as such, but for now 112 it consists of code copied exactly from emc1. 113 */ 114 static void set_operating_mode(void); 115 116 /* 'handle_jjogwheels()' reads jogwheels, decides if they should be 117 enabled, and if so, changes the free mode planner's target position 118 when the jogwheel(s) turn. 119 */ 120 static void handle_jjogwheels(void); 121 static void handle_ajogwheels(void); 122 123 /* 'do_homing_sequence()' decides what, if anything, needs to be done 124 related to multi-joint homing. 125 126 no prototype here, implemented in homing.c, proto in mot_priv.h 127 */ 128 129 /* 'do_homing()' looks at the home_state field of each joint struct 130 to decide what, if anything, needs to be done related to homing 131 the joint. Homing is implemented as a state machine, the exact 132 sequence of states depends on the machine configuration. It 133 can be as simple as immediately setting the current position to 134 zero, or a it can be a multi-step process (find switch, set 135 approximate zero, back off switch, find index, set final zero, 136 rapid to home position), or anywhere in between. 137 138 no prototype here, implemented in homing.c, proto in mot_priv.h 139 */ 140 141 /* 'get_pos_cmds()' generates the position setpoints. This includes 142 calling the trajectory planner and interpolating its outputs. 143 */ 144 static void get_pos_cmds(long period); 145 146 /* 'compute_screw_comp()' is responsible for calculating backlash and 147 lead screw error compensation. (Leadscrew error compensation is 148 a more sophisticated version that includes backlash comp.) It uses 149 the velocity in emcmotStatus->joint_vel_cmd to determine which way 150 each joint is moving, and the position in emcmotStatus->joint_pos_cmd 151 to determine where the joint is at. That information is used to 152 create the compensation value that is added to the joint_pos_cmd 153 to create motor_pos_cmd, and is subtracted from motor_pos_fb to 154 get joint_pos_fb. (This function does not add or subtract the 155 compensation value, it only computes it.) The basic compensation 156 value is in backlash_corr, however has makes step changes when 157 the direction reverses. backlash_filt is a ramped version, and 158 that is the one that is later added/subtracted from the position. 159 */ 160 static void compute_screw_comp(void); 161 162 /* 'output_to_hal()' writes the handles the final stages of the 163 control function. It applies screw comp and writes the 164 final motor position to the HAL (which routes it to the PID 165 loop). It also drives other HAL outputs, and it writes a 166 number of internal variables to HAL parameters so they can 167 be observed with halscope and halmeter. 168 */ 169 static void output_to_hal(void); 170 171 /* 'update_status()' copies assorted status information to shared 172 memory (the emcmotStatus structure) so that it is available to 173 higher level code. 174 */ 175 static void update_status(void); 176 177 static void initialize_external_offsets(void); 178 static void plan_external_offsets(void); 179 static void sync_teleop_tp_to_carte_pos(int); 180 static void sync_carte_pos_to_teleop_tp(int); 181 static void apply_ext_offsets_to_carte_pos(int); 182 static int update_coord_with_bound(void); 183 static int update_teleop_with_check(int,simple_tp_t*); 184 185 /*********************************************************************** 186 * PUBLIC FUNCTION CODE * 187 ************************************************************************/ 188 189 /* 190 emcmotController() runs the trajectory and interpolation calculations 191 each control cycle 192 193 This function gets called at regular intervals - therefore it does NOT 194 have a loop within it! 195 196 Inactive axes are still calculated, but the PIDs are inhibited and 197 the amp enable/disable are inhibited 198 */ 199 void emcmotController(void *arg, long period) 200 { 201 static int do_once = 1; 202 if (do_once) { 203 pcmd_p[0] = &(emcmotStatus->carte_pos_cmd.tran.x); 204 pcmd_p[1] = &(emcmotStatus->carte_pos_cmd.tran.y); 205 pcmd_p[2] = &(emcmotStatus->carte_pos_cmd.tran.z); 206 pcmd_p[3] = &(emcmotStatus->carte_pos_cmd.a); 207 pcmd_p[4] = &(emcmotStatus->carte_pos_cmd.b); 208 pcmd_p[5] = &(emcmotStatus->carte_pos_cmd.c); 209 pcmd_p[6] = &(emcmotStatus->carte_pos_cmd.u); 210 pcmd_p[7] = &(emcmotStatus->carte_pos_cmd.v); 211 pcmd_p[8] = &(emcmotStatus->carte_pos_cmd.w); 212 do_once = 0; 213 } 214 215 static long long int last = 0; 216 217 long long int now = rtapi_get_clocks(); 218 long int this_run = (long int)(now - last); 219 *(emcmot_hal_data->last_period) = this_run; 220 #ifdef HAVE_CPU_KHZ 221 *(emcmot_hal_data->last_period_ns) = this_run * 1e6 / cpu_khz; 222 #endif 223 224 // we need this for next time 225 last = now; 226 227 228 /* calculate servo period as a double - period is in integer nsec */ 229 servo_period = period * 0.000000001; 230 231 if(period != last_period) { 232 emcmotSetCycleTime(period); 233 last_period = period; 234 } 235 236 /* calculate servo frequency for calcs like vel = Dpos / period */ 237 /* it's faster to do vel = Dpos * freq */ 238 servo_freq = 1.0 / servo_period; 239 /* increment head count to indicate work in progress */ 240 emcmotStatus->head++; 241 /* here begins the core of the controller */ 242 243 read_homing_in_pins(ALL_JOINTS); 244 process_inputs(); 245 do_forward_kins(); 246 process_probe_inputs(); 247 check_for_faults(); 248 set_operating_mode(); 249 handle_jjogwheels(); 250 handle_ajogwheels(); 251 do_homing_sequence(); 252 do_homing(); 253 get_pos_cmds(period); 254 compute_screw_comp(); 255 plan_external_offsets(); 256 output_to_hal(); 257 write_homing_out_pins(ALL_JOINTS); 258 update_status(); 259 /* here ends the core of the controller */ 260 emcmotStatus->heartbeat++; 261 /* set tail to head, to indicate work complete */ 262 emcmotStatus->tail = emcmotStatus->head; 263 /* end of controller function */ 264 } 265 266 /*********************************************************************** 267 * LOCAL FUNCTION CODE * 268 ************************************************************************/ 269 /* The protoypes and documentation for these functions are located 270 at the top of the file in the section called "local function 271 prototypes" 272 */ 273 274 static void process_inputs(void) 275 { 276 int joint_num, spindle_num; 277 double abs_ferror, scale; 278 joint_hal_t *joint_data; 279 emcmot_joint_t *joint; 280 unsigned char enables; 281 /* read spindle angle (for threading, etc) */ 282 for (spindle_num = 0; spindle_num < emcmotConfig->numSpindles; spindle_num++){ 283 emcmotStatus->spindle_status[spindle_num].spindleRevs = 284 *emcmot_hal_data->spindle[spindle_num].spindle_revs; 285 emcmotStatus->spindle_status[spindle_num].spindleSpeedIn = 286 *emcmot_hal_data->spindle[spindle_num].spindle_speed_in; 287 emcmotStatus->spindle_status[spindle_num].at_speed = 288 *emcmot_hal_data->spindle[spindle_num].spindle_is_atspeed; 289 } 290 /* compute net feed and spindle scale factors */ 291 if ( emcmotStatus->motion_state == EMCMOT_MOTION_COORD ) { 292 /* use the enables that were queued with the current move */ 293 enables = emcmotStatus->enables_queued; 294 } else { 295 /* use the enables that are in effect right now */ 296 enables = emcmotStatus->enables_new; 297 } 298 /* feed scaling first: feed_scale, adaptive_feed, and feed_hold */ 299 scale = 1.0; 300 if ( (emcmotStatus->motion_state != EMCMOT_MOTION_FREE) 301 && (enables & FS_ENABLED) ) { 302 if (emcmotStatus->motionType == EMC_MOTION_TYPE_TRAVERSE) { 303 scale *= emcmotStatus->rapid_scale; 304 } else { 305 scale *= emcmotStatus->feed_scale; 306 } 307 } 308 if ( enables & AF_ENABLED ) { 309 /* read and clamp adaptive feed HAL pin */ 310 double adaptive_feed_in = *emcmot_hal_data->adaptive_feed; 311 // Clip range to +/- 1.0 312 if ( adaptive_feed_in > 1.0 ) { 313 adaptive_feed_in = 1.0; 314 } else if (adaptive_feed_in < -1.0) { 315 adaptive_feed_in = -1.0; 316 } 317 // Handle case of negative adaptive feed 318 // Actual scale factor is always positive by default 319 double adaptive_feed_out = fabs(adaptive_feed_in); 320 // Case 1: positive to negative direction change 321 if ( adaptive_feed_in < 0.0 && emcmotDebug->coord_tp.reverse_run == TC_DIR_FORWARD) { 322 // User commands feed in reverse direction, but we're not running in reverse yet 323 if (tpSetRunDir(&emcmotDebug->coord_tp, TC_DIR_REVERSE) != TP_ERR_OK) { 324 // Need to decelerate to a stop first 325 adaptive_feed_out = 0.0; 326 } 327 } else if (adaptive_feed_in > 0.0 && emcmotDebug->coord_tp.reverse_run == TC_DIR_REVERSE ) { 328 // User commands feed in forward direction, but we're running in reverse 329 if (tpSetRunDir(&emcmotDebug->coord_tp, TC_DIR_FORWARD) != TP_ERR_OK) { 330 // Need to decelerate to a stop first 331 adaptive_feed_out = 0.0; 332 } 333 } 334 //Otherwise, if direction and sign match, we're ok 335 scale *= adaptive_feed_out; 336 } 337 if ( enables & FH_ENABLED ) { 338 /* read feed hold HAL pin */ 339 if ( *emcmot_hal_data->feed_hold ) { 340 scale = 0; 341 } 342 } 343 /*non maskable (except during spinndle synch move) feed hold inhibit pin */ 344 if ( enables & *emcmot_hal_data->feed_inhibit ) { 345 scale = 0; 346 } 347 /* save the resulting combined scale factor */ 348 emcmotStatus->net_feed_scale = scale; 349 350 /* now do spindle scaling */ 351 for (spindle_num=0; spindle_num < emcmotConfig->numSpindles; spindle_num++){ 352 scale = 1.0; 353 if ( enables & SS_ENABLED ) { 354 scale *= emcmotStatus->spindle_status[spindle_num].scale; 355 } 356 /*non maskable (except during spindle synch move) spindle inhibit pin */ 357 if ( enables & *emcmot_hal_data->spindle[spindle_num].spindle_inhibit ) { 358 scale = 0; 359 } 360 /* save the resulting combined scale factor */ 361 emcmotStatus->spindle_status[spindle_num].net_scale = scale; 362 } 363 364 /* read and process per-joint inputs */ 365 for (joint_num = 0; joint_num < ALL_JOINTS ; joint_num++) { 366 /* point to joint HAL data */ 367 joint_data = &(emcmot_hal_data->joint[joint_num]); 368 /* point to joint data */ 369 joint = &joints[joint_num]; 370 if (!GET_JOINT_ACTIVE_FLAG(joint)) { 371 /* if joint is not active, skip it */ 372 continue; 373 } 374 /* copy data from HAL to joint structure */ 375 joint->motor_pos_fb = *(joint_data->motor_pos_fb); 376 /* calculate pos_fb */ 377 if (( get_homing_at_index_search_wait(joint_num) ) && 378 ( get_index_enable(joint_num) == 0 )) { 379 /* special case - we're homing the joint, and it just 380 hit the index. The encoder count might have made a 381 step change. The homing code will correct for it 382 later, so we ignore motor_pos_fb and set pos_fb 383 to match the commanded value instead. */ 384 joint->pos_fb = joint->pos_cmd; 385 } else { 386 /* normal case: subtract backlash comp and motor offset */ 387 joint->pos_fb = joint->motor_pos_fb - 388 (joint->backlash_filt + joint->motor_offset); 389 } 390 /* calculate following error */ 391 if ( IS_EXTRA_JOINT(joint_num) && get_homed(joint_num) ) { 392 joint->ferror = 0; // not relevant for homed extrajoints 393 } else { 394 joint->ferror = joint->pos_cmd - joint->pos_fb; 395 } 396 abs_ferror = fabs(joint->ferror); 397 /* update maximum ferror if needed */ 398 if (abs_ferror > joint->ferror_high_mark) { 399 joint->ferror_high_mark = abs_ferror; 400 } 401 402 /* calculate following error limit */ 403 if (joint->vel_limit > 0.0) { 404 joint->ferror_limit = 405 joint->max_ferror * fabs(joint->vel_cmd) / joint->vel_limit; 406 } else { 407 joint->ferror_limit = 0; 408 } 409 if (joint->ferror_limit < joint->min_ferror) { 410 joint->ferror_limit = joint->min_ferror; 411 } 412 /* update following error flag */ 413 if (abs_ferror > joint->ferror_limit) { 414 SET_JOINT_FERROR_FLAG(joint, 1); 415 } else { 416 SET_JOINT_FERROR_FLAG(joint, 0); 417 } 418 419 /* read limit switches */ 420 if (*(joint_data->pos_lim_sw)) { 421 SET_JOINT_PHL_FLAG(joint, 1); 422 } else { 423 SET_JOINT_PHL_FLAG(joint, 0); 424 } 425 if (*(joint_data->neg_lim_sw)) { 426 SET_JOINT_NHL_FLAG(joint, 1); 427 } else { 428 SET_JOINT_NHL_FLAG(joint, 0); 429 } 430 joint->on_pos_limit = GET_JOINT_PHL_FLAG(joint); 431 joint->on_neg_limit = GET_JOINT_NHL_FLAG(joint); 432 /* read amp fault input */ 433 if (*(joint_data->amp_fault)) { 434 SET_JOINT_FAULT_FLAG(joint, 1); 435 } else { 436 SET_JOINT_FAULT_FLAG(joint, 0); 437 } 438 } 439 440 // a fault was signalled during a spindle-orient in progress 441 // signal error, and cancel the orient 442 for (spindle_num = 0; spindle_num < emcmotConfig->numSpindles; spindle_num++){ 443 if(*(emcmot_hal_data->spindle[spindle_num].spindle_amp_fault)){ 444 emcmotStatus->spindle_status[spindle_num].fault = 1; 445 }else{ 446 emcmotStatus->spindle_status[spindle_num].fault = 0; 447 } 448 if (*(emcmot_hal_data->spindle[spindle_num].spindle_orient)) { 449 if (*(emcmot_hal_data->spindle[spindle_num].spindle_orient_fault)) { 450 emcmotStatus->spindle_status[spindle_num].orient_state = EMCMOT_ORIENT_FAULTED; 451 *(emcmot_hal_data->spindle[spindle_num].spindle_orient) = 0; 452 emcmotStatus->spindle_status[spindle_num].orient_fault = 453 *(emcmot_hal_data->spindle[spindle_num].spindle_orient_fault); 454 reportError(_("fault %d during orient in progress"), 455 emcmotStatus->spindle_status[spindle_num].orient_fault); 456 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; 457 tpAbort(&emcmotDebug->coord_tp); 458 SET_MOTION_ERROR_FLAG(1); 459 } else if (*(emcmot_hal_data->spindle[spindle_num].spindle_is_oriented)) { 460 *(emcmot_hal_data->spindle[spindle_num].spindle_orient) = 0; 461 *(emcmot_hal_data->spindle[spindle_num].spindle_locked) = 1; 462 emcmotStatus->spindle_status[spindle_num].locked = 1; 463 emcmotStatus->spindle_status[spindle_num].brake = 1; 464 emcmotStatus->spindle_status[spindle_num].orient_state = EMCMOT_ORIENT_COMPLETE; 465 rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_ORIENT complete, spindle locked"); 466 } 467 } 468 } 469 } 470 471 static void do_forward_kins(void) 472 { 473 /* there are four possibilities for kinType: 474 475 IDENTITY: Both forward and inverse kins are available, and they 476 can used without an initial guess, even if one or more joints 477 are not homed. In this case, we apply the forward kins to the 478 joint->pos_fb to produce carte_pos_fb, and if all axes are homed 479 we set carte_pos_fb_ok to 1 to indicate that the feedback data 480 is good. 481 482 BOTH: Both forward and inverse kins are available, but the forward 483 kins need an initial guess, and/or the kins require all joints to 484 be homed before they work properly. Here we must tread carefully. 485 IF all the joints have been homed, we apply the forward kins to 486 the joint->pos_fb to produce carte_pos_fb, and set carte_pos_fb_ok 487 to indicate that the feedback is good. We use the previous value 488 of carte_pos_fb as the initial guess. If all joints have not been 489 homed, we don't call the kinematics, instead we set carte_pos_fb to 490 the cartesean coordinates of home, as stored in the global worldHome, 491 and we set carte_fb_ok to 0 to indicate that the feedback is invalid. 492 \todo FIXME - maybe setting to home isn't the right thing to do. We need 493 it to be set to home eventually, (right before the first attemt to 494 run the kins), but that doesn't mean we should say we're at home 495 when we're not. 496 497 INVERSE_ONLY: Only inverse kinematics are available, forward 498 kinematics cannot be used. So we have to fake it, the question is 499 simply "what way of faking it is best". In free mode, or if all 500 axes have not been homed, the feedback position is unknown. If 501 we are in teleop or coord mode, or if we are in free mode and all 502 axes are homed, and haven't been moved since they were homed, then 503 we set carte_pos_fb to carte_pos_cmd, and set carte_pos_fb_ok to 1. 504 If we are in free mode, and any joint is not homed, or any joint has 505 moved since it was homed, we leave cart_pos_fb alone, and set 506 carte_pos_fb_ok to 0. 507 508 FORWARD_ONLY: Only forward kinematics are available, inverse kins 509 cannot be used. This exists for completeness only, since EMC won't 510 work without inverse kinematics. 511 512 */ 513 514 /*! \todo FIXME FIXME FIXME - need to put a rate divider in here, run it 515 at the traj rate */ 516 517 double joint_pos[EMCMOT_MAX_JOINTS] = {0,}; 518 int joint_num, result; 519 emcmot_joint_t *joint; 520 521 /* copy joint position feedback to local array */ 522 for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { 523 /* point to joint struct */ 524 joint = &joints[joint_num]; 525 /* copy feedback */ 526 joint_pos[joint_num] = joint->pos_fb; 527 } 528 switch (emcmotConfig->kinType) { 529 530 case KINEMATICS_IDENTITY: 531 kinematicsForward(joint_pos, &emcmotStatus->carte_pos_fb, &fflags, 532 &iflags); 533 if (checkAllHomed()) { 534 emcmotStatus->carte_pos_fb_ok = 1; 535 } else { 536 emcmotStatus->carte_pos_fb_ok = 0; 537 } 538 break; 539 540 case KINEMATICS_BOTH: 541 if (checkAllHomed()) { 542 /* is previous value suitable for use as initial guess? */ 543 if (!emcmotStatus->carte_pos_fb_ok) { 544 /* no, use home position as initial guess */ 545 emcmotStatus->carte_pos_fb = emcmotStatus->world_home; 546 } 547 /* calculate Cartesean position feedback from joint pos fb */ 548 result = 549 kinematicsForward(joint_pos, &emcmotStatus->carte_pos_fb, 550 &fflags, &iflags); 551 /* check to make sure kinematics converged */ 552 if (result < 0) { 553 /* error during kinematics calculations */ 554 emcmotStatus->carte_pos_fb_ok = 0; 555 } else { 556 /* it worked! */ 557 emcmotStatus->carte_pos_fb_ok = 1; 558 } 559 } else { 560 emcmotStatus->carte_pos_fb_ok = 0; 561 } 562 break; 563 564 case KINEMATICS_INVERSE_ONLY: 565 566 if ((GET_MOTION_COORD_FLAG()) || (GET_MOTION_TELEOP_FLAG())) { 567 /* use Cartesean position command as feedback value */ 568 emcmotStatus->carte_pos_fb = emcmotStatus->carte_pos_cmd; 569 emcmotStatus->carte_pos_fb_ok = 1; 570 } else { 571 emcmotStatus->carte_pos_fb_ok = 0; 572 } 573 break; 574 575 default: 576 emcmotStatus->carte_pos_fb_ok = 0; 577 break; 578 } 579 } 580 581 static void process_probe_inputs(void) 582 { 583 static int old_probeVal = 0; 584 unsigned char probe_type = emcmotStatus->probe_type; 585 586 // don't error 587 char probe_suppress = probe_type & 1; 588 int axis_num; 589 590 // trigger when the probe clears, instead of the usual case of triggering when it trips 591 char probe_whenclears = !!(probe_type & 2); 592 593 /* read probe input */ 594 emcmotStatus->probeVal = !!*(emcmot_hal_data->probe_input); 595 if (emcmotStatus->probing) { 596 /* check if the probe has been tripped */ 597 if (emcmotStatus->probeVal ^ probe_whenclears) { 598 /* remember the current position */ 599 emcmotStatus->probedPos = emcmotStatus->carte_pos_fb; 600 /* stop! */ 601 emcmotStatus->probing = 0; 602 emcmotStatus->probeTripped = 1; 603 tpAbort(&emcmotDebug->coord_tp); 604 /* check if the probe hasn't tripped, but the move finished */ 605 } else if (GET_MOTION_INPOS_FLAG() && tpQueueDepth(&emcmotDebug->coord_tp) == 0) { 606 /* we are already stopped, but we need to remember the current 607 position here, because it will still be queried */ 608 emcmotStatus->probedPos = emcmotStatus->carte_pos_fb; 609 emcmotStatus->probing = 0; 610 if (probe_suppress) { 611 emcmotStatus->probeTripped = 0; 612 } else if(probe_whenclears) { 613 reportError(_("G38.4 move finished without breaking contact.")); 614 SET_MOTION_ERROR_FLAG(1); 615 } else { 616 reportError(_("G38.2 move finished without making contact.")); 617 SET_MOTION_ERROR_FLAG(1); 618 } 619 } 620 } else if (!old_probeVal && emcmotStatus->probeVal) { 621 // not probing, but we have a rising edge on the probe. 622 // this could be expensive if we don't stop. 623 int i; 624 int aborted = 0; 625 626 if(!GET_MOTION_INPOS_FLAG() && tpQueueDepth(&emcmotDebug->coord_tp) && 627 tpGetExecId(&emcmotDebug->coord_tp) <= 0) { 628 // running an MDI command 629 tpAbort(&emcmotDebug->coord_tp); 630 reportError(_("Probe tripped during non-probe MDI command.")); 631 SET_MOTION_ERROR_FLAG(1); 632 } 633 634 for(i=0; i<NO_OF_KINS_JOINTS; i++) { 635 emcmot_joint_t *joint = &joints[i]; 636 637 if (!GET_JOINT_ACTIVE_FLAG(joint)) { 638 /* if joint is not active, skip it */ 639 continue; 640 } 641 642 // abort any homing 643 if(get_homing(i)) { 644 set_home_abort(i); 645 aborted=1; 646 } 647 648 // abort any joint jogs 649 if(joint->free_tp.enable == 1) { 650 joint->free_tp.enable = 0; 651 // since homing uses free_tp, this protection of aborted 652 // is needed so the user gets the correct error. 653 if(!aborted) aborted=2; 654 } 655 } 656 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 657 emcmot_axis_t *axis; 658 axis = &axes[axis_num]; 659 // abort any coordinate jogs 660 if (axis->teleop_tp.enable) { 661 axis->teleop_tp.enable = 0; 662 axis->teleop_tp.curr_vel = 0.0; 663 aborted = 3; 664 } 665 } 666 667 if(aborted == 1) { 668 reportError(_("Probe tripped during homing motion.")); 669 } 670 671 if(aborted == 2) { 672 reportError(_("Probe tripped during a joint jog.")); 673 } 674 if(aborted == 3) { 675 reportError(_("Probe tripped during a coordinate jog.")); 676 } 677 } 678 old_probeVal = emcmotStatus->probeVal; 679 } 680 681 static void check_for_faults(void) 682 { 683 int joint_num, spindle_num; 684 emcmot_joint_t *joint; 685 int neg_limit_override, pos_limit_override; 686 687 /* check for various global fault conditions */ 688 /* only check enable input if running */ 689 if ( GET_MOTION_ENABLE_FLAG() != 0 ) { 690 if ( *(emcmot_hal_data->enable) == 0 ) { 691 reportError(_("motion stopped by enable input")); 692 emcmotDebug->enabling = 0; 693 } 694 } 695 /* check for spindle ampfifier errors */ 696 for (spindle_num = 0; spindle_num < emcmotConfig->numSpindles; spindle_num++){ 697 if(emcmotStatus->spindle_status[spindle_num].fault && GET_MOTION_ENABLE_FLAG()){ 698 reportError(_("spindle %d amplifier fault"), spindle_num); 699 emcmotDebug->enabling = 0; 700 } 701 } 702 /* check for various joint fault conditions */ 703 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 704 /* point to joint data */ 705 joint = &joints[joint_num]; 706 /* only check active, enabled axes */ 707 if ( GET_JOINT_ACTIVE_FLAG(joint) && GET_JOINT_ENABLE_FLAG(joint) ) { 708 /* are any limits for this joint overridden? */ 709 neg_limit_override = emcmotStatus->overrideLimitMask & ( 1 << (joint_num*2)); 710 pos_limit_override = emcmotStatus->overrideLimitMask & ( 2 << (joint_num*2)); 711 /* check for hard limits */ 712 if ((GET_JOINT_PHL_FLAG(joint) && ! pos_limit_override ) || 713 (GET_JOINT_NHL_FLAG(joint) && ! neg_limit_override )) { 714 /* joint is on limit switch, should we trip? */ 715 if (get_homing(joint_num)) { 716 /* no, ignore limits */ 717 } else { 718 /* trip on limits */ 719 if (!GET_JOINT_ERROR_FLAG(joint)) { 720 /* report the error just this once */ 721 reportError(_("joint %d on limit switch error"), 722 joint_num); 723 } 724 SET_JOINT_ERROR_FLAG(joint, 1); 725 emcmotDebug->enabling = 0; 726 } 727 } 728 /* check for amp fault */ 729 if (GET_JOINT_FAULT_FLAG(joint)) { 730 /* joint is faulted, trip */ 731 if (!GET_JOINT_ERROR_FLAG(joint)) { 732 /* report the error just this once */ 733 reportError(_("joint %d amplifier fault"), joint_num); 734 } 735 SET_JOINT_ERROR_FLAG(joint, 1); 736 emcmotDebug->enabling = 0; 737 } 738 /* check for excessive following error */ 739 if (GET_JOINT_FERROR_FLAG(joint)) { 740 if (!GET_JOINT_ERROR_FLAG(joint)) { 741 /* report the error just this once */ 742 reportError(_("joint %d following error"), joint_num); 743 } 744 SET_JOINT_ERROR_FLAG(joint, 1); 745 emcmotDebug->enabling = 0; 746 } 747 /* end of if JOINT_ACTIVE_FLAG(joint) */ 748 } 749 /* end of check for joint faults loop */ 750 } 751 } 752 753 static void set_operating_mode(void) 754 { 755 int joint_num, axis_num; 756 emcmot_joint_t *joint; 757 emcmot_axis_t *axis; 758 double positions[EMCMOT_MAX_JOINTS]; 759 760 /* check for disabling */ 761 if (!emcmotDebug->enabling && GET_MOTION_ENABLE_FLAG()) { 762 /* clear out the motion emcmotDebug->coord_tp and interpolators */ 763 tpClear(&emcmotDebug->coord_tp); 764 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 765 /* point to joint data */ 766 joint = &joints[joint_num]; 767 /* disable free mode planner */ 768 joint->free_tp.enable = 0; 769 joint->free_tp.curr_vel = 0.0; 770 /* drain coord mode interpolators */ 771 cubicDrain(&(joint->cubic)); 772 if (GET_JOINT_ACTIVE_FLAG(joint)) { 773 SET_JOINT_INPOS_FLAG(joint, 1); 774 SET_JOINT_ENABLE_FLAG(joint, 0); 775 set_joint_homing(joint_num,0); 776 set_home_idle(joint_num); 777 } 778 /* don't clear the joint error flag, since that may signify why 779 we just went into disabled state */ 780 } 781 782 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 783 /* point to axis data */ 784 axis = &axes[axis_num]; 785 /* disable teleop mode planner */ 786 axis->teleop_tp.enable = 0; 787 axis->teleop_tp.curr_vel = 0.0; 788 } 789 790 SET_MOTION_ENABLE_FLAG(0); 791 /* don't clear the motion error flag, since that may signify why we 792 just went into disabled state */ 793 } 794 795 /* check for emcmotDebug->enabling */ 796 if (emcmotDebug->enabling && !GET_MOTION_ENABLE_FLAG()) { 797 if (*(emcmot_hal_data->eoffset_limited)) { 798 reportError("Starting beyond Soft Limits"); 799 *(emcmot_hal_data->eoffset_limited) = 0; 800 } 801 initialize_external_offsets(); 802 tpSetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd); 803 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 804 /* point to joint data */ 805 joint = &joints[joint_num]; 806 joint->free_tp.curr_pos = joint->pos_cmd; 807 if (GET_JOINT_ACTIVE_FLAG(joint)) { 808 SET_JOINT_ENABLE_FLAG(joint, 1); 809 set_joint_homing(joint_num,0); 810 set_home_idle(joint_num); 811 } 812 /* clear any outstanding joint errors when going into enabled 813 state */ 814 SET_JOINT_ERROR_FLAG(joint, 0); 815 } 816 if ( !GET_MOTION_ENABLE_FLAG() ) { 817 if (GET_MOTION_TELEOP_FLAG()) { 818 sync_teleop_tp_to_carte_pos(0); 819 } 820 } 821 SET_MOTION_ENABLE_FLAG(1); 822 /* clear any outstanding motion errors when going into enabled state */ 823 SET_MOTION_ERROR_FLAG(0); 824 } 825 826 /* check for entering teleop mode */ 827 if (emcmotDebug->teleoperating && !GET_MOTION_TELEOP_FLAG()) { 828 if (GET_MOTION_INPOS_FLAG()) { 829 830 /* update coordinated emcmotDebug->coord_tp position */ 831 tpSetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd); 832 /* drain the cubics so they'll synch up */ 833 for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) { 834 if (joint_num < NO_OF_KINS_JOINTS) { 835 /* point to joint data */ 836 joint = &joints[joint_num]; 837 cubicDrain(&(joint->cubic)); 838 positions[joint_num] = joint->coarse_pos; 839 } else { 840 positions[joint_num] = 0; 841 } 842 } 843 /* Initialize things to do when starting teleop mode. */ 844 SET_MOTION_TELEOP_FLAG(1); 845 SET_MOTION_ERROR_FLAG(0); 846 847 kinematicsForward(positions, &emcmotStatus->carte_pos_cmd, &fflags, &iflags); 848 // entering teleop (INPOS), remove ext offsets 849 sync_teleop_tp_to_carte_pos(-1); 850 } else { 851 /* not in position-- don't honor mode change */ 852 emcmotDebug->teleoperating = 0; 853 } 854 } else { 855 if (GET_MOTION_INPOS_FLAG()) { 856 if (!emcmotDebug->teleoperating && GET_MOTION_TELEOP_FLAG()) { 857 SET_MOTION_TELEOP_FLAG(0); 858 if (!emcmotDebug->coordinating) { 859 for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { 860 /* point to joint data */ 861 joint = &joints[joint_num]; 862 /* update free planner positions */ 863 joint->free_tp.curr_pos = joint->pos_cmd; 864 } 865 } 866 } 867 } 868 869 /* check for entering coordinated mode */ 870 if (emcmotDebug->coordinating && !GET_MOTION_COORD_FLAG()) { 871 if (GET_MOTION_INPOS_FLAG()) { 872 /* preset traj planner to current position */ 873 874 apply_ext_offsets_to_carte_pos(-1); // subtract at coord mode start 875 876 tpSetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd); 877 /* drain the cubics so they'll synch up */ 878 for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { 879 /* point to joint data */ 880 joint = &joints[joint_num]; 881 cubicDrain(&(joint->cubic)); 882 } 883 /* clear the override limits flags */ 884 emcmotDebug->overriding = 0; 885 emcmotStatus->overrideLimitMask = 0; 886 SET_MOTION_COORD_FLAG(1); 887 SET_MOTION_TELEOP_FLAG(0); 888 SET_MOTION_ERROR_FLAG(0); 889 } else { 890 /* not in position-- don't honor mode change */ 891 emcmotDebug->coordinating = 0; 892 } 893 } 894 895 /* check entering free space mode */ 896 if (!emcmotDebug->coordinating && GET_MOTION_COORD_FLAG()) { 897 if (GET_MOTION_INPOS_FLAG()) { 898 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 899 /* point to joint data */ 900 joint = &joints[joint_num]; 901 /* set joint planner curr_pos to current location */ 902 joint->free_tp.curr_pos = joint->pos_cmd; 903 /* but it can stay disabled until a move is required */ 904 joint->free_tp.enable = 0; 905 } 906 SET_MOTION_COORD_FLAG(0); 907 SET_MOTION_TELEOP_FLAG(0); 908 SET_MOTION_ERROR_FLAG(0); 909 } else { 910 /* not in position-- don't honor mode change */ 911 emcmotDebug->coordinating = 1; 912 } 913 } 914 } 915 /*! \todo FIXME - this code is temporary - eventually this function will be 916 cleaned up and simplified, and 'motion_state' will become the master 917 for this info, instead of having to gather it from several flags */ 918 if (!GET_MOTION_ENABLE_FLAG()) { 919 emcmotStatus->motion_state = EMCMOT_MOTION_DISABLED; 920 } else if (GET_MOTION_TELEOP_FLAG()) { 921 emcmotStatus->motion_state = EMCMOT_MOTION_TELEOP; 922 } else if (GET_MOTION_COORD_FLAG()) { 923 emcmotStatus->motion_state = EMCMOT_MOTION_COORD; 924 } else { 925 emcmotStatus->motion_state = EMCMOT_MOTION_FREE; 926 } 927 } //set_operating_mode 928 929 static void handle_jjogwheels(void) 930 { 931 int joint_num; 932 emcmot_joint_t *joint; 933 joint_hal_t *joint_data; 934 int new_jjog_counts, delta; 935 double distance, pos, stop_dist; 936 static int first_pass = 1; /* used to set initial conditions */ 937 938 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 939 double jaccel_limit; 940 /* point to joint data */ 941 joint_data = &(emcmot_hal_data->joint[joint_num]); 942 joint = &joints[joint_num]; 943 if (!GET_JOINT_ACTIVE_FLAG(joint)) { 944 /* if joint is not active, skip it */ 945 continue; 946 } 947 948 // disallow accel bogus fractions 949 if ( (*(joint_data->jjog_accel_fraction) > 1) 950 || (*(joint_data->jjog_accel_fraction) < 0) ) { 951 jaccel_limit = joint->acc_limit; 952 } else { 953 jaccel_limit = (*(joint_data->jjog_accel_fraction)) * joint->acc_limit; 954 } 955 /* get counts from jogwheel */ 956 new_jjog_counts = *(joint_data->jjog_counts); 957 delta = new_jjog_counts - joint->old_jjog_counts; 958 /* save value for next time */ 959 joint->old_jjog_counts = new_jjog_counts; 960 /* initialization complete */ 961 if ( first_pass ) { 962 continue; 963 } 964 /* did the wheel move? */ 965 if ( delta == 0 ) { 966 /* no, nothing to do */ 967 continue; 968 } 969 if (GET_MOTION_TELEOP_FLAG()) { 970 joint->free_tp.enable = 0; 971 return; 972 } 973 /* must be in free mode and enabled */ 974 if (GET_MOTION_COORD_FLAG()) { 975 continue; 976 } 977 if (!GET_MOTION_ENABLE_FLAG()) { 978 continue; 979 } 980 /* the jogwheel input for this joint must be enabled */ 981 if ( *(joint_data->jjog_enable) == 0 ) { 982 continue; 983 } 984 /* must not be homing */ 985 if (get_homing_is_active() ) { 986 continue; 987 } 988 /* must not be doing a keyboard jog */ 989 if (joint->kb_jjog_active) { 990 continue; 991 } 992 if (emcmotStatus->net_feed_scale < 0.0001 ) { 993 /* don't jog if feedhold is on or if feed override is zero */ 994 break; 995 } 996 if (get_home_needs_unlock_first(joint_num) ) { 997 reportError("Can't wheel jog locking joint_num=%d",joint_num); 998 continue; 999 } 1000 if (get_home_is_synchronized(joint_num)) { 1001 if (emcmotConfig->kinType == KINEMATICS_IDENTITY) { 1002 rtapi_print_msg(RTAPI_MSG_ERR, 1003 "Homing is REQUIRED to wheel jog requested coordinate\n" 1004 "because joint (%d) home_sequence is synchronized (%d)\n" 1005 ,joint_num,get_home_sequence(joint_num) ); 1006 } else { 1007 rtapi_print_msg(RTAPI_MSG_ERR, 1008 "Cannot wheel jog joint %d because home_sequence synchronized (%d)\n" 1009 ,joint_num,get_home_sequence(joint_num) ); 1010 } 1011 continue; 1012 } 1013 /* calculate distance to jog */ 1014 distance = delta * *(joint_data->jjog_scale); 1015 /* check for joint already on hard limit */ 1016 if (distance > 0.0 && GET_JOINT_PHL_FLAG(joint)) { 1017 continue; 1018 } 1019 if (distance < 0.0 && GET_JOINT_NHL_FLAG(joint)) { 1020 continue; 1021 } 1022 /* calc target position for jog */ 1023 pos = joint->free_tp.pos_cmd + distance; 1024 /* don't jog past limits */ 1025 refresh_jog_limits(joint,joint_num); 1026 if (pos > joint->max_jog_limit) { 1027 continue; 1028 } 1029 if (pos < joint->min_jog_limit) { 1030 continue; 1031 } 1032 /* The default is to move exactly as far as the wheel commands, 1033 even if that move takes much longer than the wheel movement 1034 that commanded it. Some folks prefer that the move stop as 1035 soon as the wheel does, even if that means not moving the 1036 commanded distance. Velocity mode is for those folks. If 1037 the command is faster than the machine can track, excess 1038 command is simply dropped. */ 1039 if ( *(joint_data->jjog_vel_mode) ) { 1040 double v = joint->vel_limit * emcmotStatus->net_feed_scale; 1041 /* compute stopping distance at max speed */ 1042 stop_dist = v * v / ( 2 * jaccel_limit); 1043 /* if commanded position leads the actual position by more 1044 than stopping distance, discard excess command */ 1045 if ( pos > joint->pos_cmd + stop_dist ) { 1046 pos = joint->pos_cmd + stop_dist; 1047 } else if ( pos < joint->pos_cmd - stop_dist ) { 1048 pos = joint->pos_cmd - stop_dist; 1049 } 1050 } 1051 /* set target position and use full velocity and accel */ 1052 joint->free_tp.pos_cmd = pos; 1053 joint->free_tp.max_vel = joint->vel_limit; 1054 joint->free_tp.max_acc = jaccel_limit; 1055 /* lock out other jog sources */ 1056 joint->wheel_jjog_active = 1; 1057 /* and let it go */ 1058 joint->free_tp.enable = 1; 1059 SET_JOINT_ERROR_FLAG(joint, 0); 1060 /* clear joint homed flag(s) if we don't have forward kins. 1061 Otherwise, a transition into coordinated mode will incorrectly 1062 assume the homed position. Do all if they've all been moved 1063 since homing, otherwise just do this one */ 1064 clearHomes(joint_num); 1065 } 1066 1067 // done with initialization, do the whole thing from now on 1068 first_pass = 0; 1069 } 1070 1071 static void handle_ajogwheels(void) 1072 { 1073 int axis_num; 1074 emcmot_axis_t *axis; 1075 axis_hal_t *axis_data; 1076 int new_ajog_counts, delta; 1077 double distance, pos, stop_dist; 1078 static int first_pass = 1; /* used to set initial conditions */ 1079 1080 // change from teleop to move off joint soft limit 1081 if ( emcmotStatus->on_soft_limit ) { return; } 1082 1083 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 1084 double aaccel_limit; 1085 axis = &axes[axis_num]; 1086 axis_data = &(emcmot_hal_data->axis[axis_num]); 1087 1088 // disallow accel bogus fractions 1089 if ( (*(axis_data->ajog_accel_fraction) > 1) 1090 || (*(axis_data->ajog_accel_fraction) < 0) ) { 1091 aaccel_limit = axis->acc_limit; 1092 } else { 1093 aaccel_limit = *(axis_data->ajog_accel_fraction) * axis->acc_limit; 1094 } 1095 1096 new_ajog_counts = *(axis_data->ajog_counts); 1097 delta = new_ajog_counts - axis->old_ajog_counts; 1098 axis->old_ajog_counts = new_ajog_counts; 1099 if ( first_pass ) { continue; } 1100 if ( delta == 0 ) { 1101 //just update counts 1102 continue; 1103 } 1104 if (!GET_MOTION_TELEOP_FLAG()) { 1105 axis->teleop_tp.enable = 0; 1106 return; 1107 } 1108 if (!GET_MOTION_TELEOP_FLAG()) { continue; } 1109 if (!GET_MOTION_ENABLE_FLAG()) { continue; } 1110 if ( *(axis_data->ajog_enable) == 0 ) { continue; } 1111 if (get_homing_is_active() ) { continue; } 1112 if (axis->kb_ajog_active) { continue; } 1113 1114 if (axis->locking_joint >= 0) { 1115 rtapi_print_msg(RTAPI_MSG_ERR, 1116 "Cannot wheel jog a locking indexer AXIS_%c\n", 1117 "XYZABCUVW"[axis_num]); 1118 continue; 1119 } 1120 1121 distance = delta * *(axis_data->ajog_scale); 1122 pos = axis->teleop_tp.pos_cmd + distance; 1123 if ( *(axis_data->ajog_vel_mode) ) { 1124 double v = axis->vel_limit; 1125 /* compute stopping distance at max speed */ 1126 stop_dist = v * v / ( 2 * aaccel_limit); 1127 /* if commanded position leads the actual position by more 1128 than stopping distance, discard excess command */ 1129 if ( pos > axis->pos_cmd + stop_dist ) { 1130 pos = axis->pos_cmd + stop_dist; 1131 } else if ( pos < axis->pos_cmd - stop_dist ) { 1132 pos = axis->pos_cmd - stop_dist; 1133 } 1134 } 1135 if (pos > axis->max_pos_limit) { break; } 1136 if (pos < axis->min_pos_limit) { break; } 1137 axis->teleop_tp.pos_cmd = pos; 1138 axis->teleop_tp.max_vel = axis->vel_limit; 1139 axis->teleop_tp.max_acc = aaccel_limit; 1140 axis->wheel_ajog_active = 1; 1141 axis->teleop_tp.enable = 1; 1142 } 1143 first_pass = 0; 1144 } 1145 1146 static void get_pos_cmds(long period) 1147 { 1148 int joint_num, axis_num, result; 1149 emcmot_joint_t *joint; 1150 emcmot_axis_t *axis; 1151 double positions[EMCMOT_MAX_JOINTS]; 1152 double vel_lim; 1153 1154 /* used in teleop mode to compute the max accell requested */ 1155 int onlimit = 0; 1156 int joint_limit[EMCMOT_MAX_JOINTS][2]; 1157 int violated_teleop_limit = 0; 1158 1159 /* copy joint position feedback to local array */ 1160 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 1161 /* point to joint struct */ 1162 joint = &joints[joint_num]; 1163 /* copy coarse command */ 1164 positions[joint_num] = joint->coarse_pos; 1165 } 1166 /* if less than a full complement of joints, zero out the rest */ 1167 while ( joint_num < EMCMOT_MAX_JOINTS ) { 1168 positions[joint_num++] = 0.0; 1169 } 1170 1171 /* RUN MOTION CALCULATIONS: */ 1172 1173 /* run traj planner code depending on the state */ 1174 switch ( emcmotStatus->motion_state) { 1175 case EMCMOT_MOTION_FREE: 1176 /* in free mode, each joint is planned independently */ 1177 /* initial value for flag, if needed it will be cleared below */ 1178 SET_MOTION_INPOS_FLAG(1); 1179 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 1180 /* point to joint struct */ 1181 joint = &joints[joint_num]; 1182 if (!GET_JOINT_ACTIVE_FLAG(joint)) { 1183 /* if joint is not active, skip it */ 1184 continue; 1185 } 1186 // extra joint is not managed herein after homing: 1187 if (IS_EXTRA_JOINT(joint_num) && get_homed(joint_num)) continue; 1188 1189 if(joint->acc_limit > emcmotStatus->acc) 1190 joint->acc_limit = emcmotStatus->acc; 1191 /* compute joint velocity limit */ 1192 if ( get_home_is_idle(joint_num) ) { 1193 /* velocity limit = joint limit * global scale factor */ 1194 /* the global factor is used for feedrate override */ 1195 vel_lim = joint->vel_limit * emcmotStatus->net_feed_scale; 1196 /* must not be greater than the joint physical limit */ 1197 if (vel_lim > joint->vel_limit) { 1198 vel_lim = joint->vel_limit; 1199 } 1200 /* set vel limit in free TP */ 1201 if (vel_lim < joint->free_tp.max_vel) 1202 joint->free_tp.max_vel = vel_lim; 1203 } else { 1204 /* except if homing, when we set free_tp max vel in do_homing */ 1205 } 1206 /* set acc limit in free TP */ 1207 /* execute free TP */ 1208 if (joint->wheel_jjog_active) { 1209 double jaccel_limit; 1210 joint_hal_t *joint_data; 1211 joint_data = &(emcmot_hal_data->joint[joint_num]); 1212 if ( (*(joint_data->jjog_accel_fraction) > 1) 1213 || (*(joint_data->jjog_accel_fraction) < 0) ) { 1214 jaccel_limit = joint->acc_limit; 1215 } else { 1216 jaccel_limit = (*(joint_data->jjog_accel_fraction)) * joint->acc_limit; 1217 } 1218 joint->free_tp.max_acc = jaccel_limit; 1219 } else { 1220 joint->free_tp.max_acc = joint->acc_limit; 1221 } 1222 simple_tp_update(&(joint->free_tp), servo_period ); 1223 /* copy free TP output to pos_cmd and coarse_pos */ 1224 joint->pos_cmd = joint->free_tp.curr_pos; 1225 joint->vel_cmd = joint->free_tp.curr_vel; 1226 //no acceleration output form simple_tp, but the pin will 1227 //still show the acceleration from the interpolation. 1228 //its delayed, but thats ok during jogging or homing. 1229 joint->acc_cmd = 0.0; 1230 joint->coarse_pos = joint->free_tp.curr_pos; 1231 /* update joint status flag and overall status flag */ 1232 if ( joint->free_tp.active ) { 1233 /* active TP means we're moving, so not in position */ 1234 SET_JOINT_INPOS_FLAG(joint, 0); 1235 SET_MOTION_INPOS_FLAG(0); 1236 /* if we move at all, clear at_home flag */ 1237 set_joint_at_home(joint_num,0); 1238 /* is any limit disabled for this move? */ 1239 if ( emcmotStatus->overrideLimitMask ) { 1240 emcmotDebug->overriding = 1; 1241 } 1242 } else { 1243 SET_JOINT_INPOS_FLAG(joint, 1); 1244 /* joint has stopped, so any outstanding jogs are done */ 1245 joint->kb_jjog_active = 0; 1246 joint->wheel_jjog_active = 0; 1247 } 1248 }//for loop for joints 1249 /* if overriding is true and we're in position, the jog 1250 is complete, and the limits should be re-enabled */ 1251 if ( (emcmotDebug->overriding ) && ( GET_MOTION_INPOS_FLAG() ) ) { 1252 emcmotStatus->overrideLimitMask = 0; 1253 emcmotDebug->overriding = 0; 1254 } 1255 /*! \todo FIXME - this should run at the traj rate */ 1256 switch (emcmotConfig->kinType) { 1257 1258 case KINEMATICS_IDENTITY: 1259 kinematicsForward(positions, &emcmotStatus->carte_pos_cmd, &fflags, &iflags); 1260 if (checkAllHomed()) { 1261 emcmotStatus->carte_pos_cmd_ok = 1; 1262 } else { 1263 emcmotStatus->carte_pos_cmd_ok = 0; 1264 } 1265 break; 1266 1267 case KINEMATICS_BOTH: 1268 if (checkAllHomed()) { 1269 /* is previous value suitable for use as initial guess? */ 1270 if (!emcmotStatus->carte_pos_cmd_ok) { 1271 /* no, use home position as initial guess */ 1272 emcmotStatus->carte_pos_cmd = emcmotStatus->world_home; 1273 } 1274 /* calculate Cartesean position command from joint coarse pos cmd */ 1275 result = 1276 kinematicsForward(positions, &emcmotStatus->carte_pos_cmd, &fflags, &iflags); 1277 /* check to make sure kinematics converged */ 1278 if (result < 0) { 1279 /* error during kinematics calculations */ 1280 emcmotStatus->carte_pos_cmd_ok = 0; 1281 } else { 1282 /* it worked! */ 1283 emcmotStatus->carte_pos_cmd_ok = 1; 1284 } 1285 } else { 1286 emcmotStatus->carte_pos_cmd_ok = 0; 1287 } 1288 break; 1289 1290 case KINEMATICS_INVERSE_ONLY: 1291 emcmotStatus->carte_pos_cmd_ok = 0; 1292 break; 1293 1294 default: 1295 emcmotStatus->carte_pos_cmd_ok = 0; 1296 break; 1297 } 1298 /* end of FREE mode */ 1299 break; 1300 1301 case EMCMOT_MOTION_COORD: 1302 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 1303 axis = &axes[axis_num]; 1304 axis->teleop_tp.enable = 0; 1305 axis->teleop_tp.curr_vel = 0.0; 1306 } // for(axis_num) 1307 1308 /* check joint 0 to see if the interpolators are empty */ 1309 while (cubicNeedNextPoint(&(joints[0].cubic))) { 1310 /* they're empty, pull next point(s) off Cartesian planner */ 1311 /* run coordinated trajectory planning cycle */ 1312 1313 tpRunCycle(&emcmotDebug->coord_tp, period); 1314 /* get new commanded traj pos */ 1315 tpGetPos(&emcmotDebug->coord_tp, &emcmotStatus->carte_pos_cmd); 1316 1317 if ( update_coord_with_bound() ) { 1318 ext_offset_coord_limit = 1; 1319 } else { 1320 ext_offset_coord_limit = 0; 1321 } 1322 1323 /* OUTPUT KINEMATICS - convert to joints in local array */ 1324 result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, 1325 &iflags, &fflags); 1326 if(result == 0) 1327 { 1328 /* copy to joint structures and spline them up */ 1329 for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { 1330 if(!isfinite(positions[joint_num])) 1331 { 1332 reportError(_("kinematicsInverse gave non-finite joint location on joint %d"), 1333 joint_num); 1334 SET_MOTION_ERROR_FLAG(1); 1335 SET_MOTION_ENABLE_FLAG(0); 1336 emcmotDebug->enabling = 0; 1337 break; 1338 } 1339 /* point to joint struct */ 1340 joint = &joints[joint_num]; 1341 joint->coarse_pos = positions[joint_num]; 1342 /* spline joints up-- note that we may be adding points 1343 that fail soft limits, but we'll abort at the end of 1344 this cycle so it doesn't really matter */ 1345 cubicAddPoint(&(joint->cubic), joint->coarse_pos); 1346 } 1347 } 1348 else 1349 { 1350 reportError(_("kinematicsInverse failed")); 1351 SET_MOTION_ERROR_FLAG(1); 1352 SET_MOTION_ENABLE_FLAG(0); 1353 emcmotDebug->enabling = 0; 1354 break; 1355 } 1356 1357 /* END OF OUTPUT KINS */ 1358 } // while 1359 /* there is data in the interpolators */ 1360 /* run interpolation */ 1361 for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { 1362 /* point to joint struct */ 1363 joint = &joints[joint_num]; 1364 /* interpolate to get new position and velocity */ 1365 joint->pos_cmd = cubicInterpolate(&(joint->cubic), 0, &(joint->vel_cmd), &(joint->acc_cmd), 0); 1366 } 1367 /* report motion status */ 1368 SET_MOTION_INPOS_FLAG(0); 1369 if (tpIsDone(&emcmotDebug->coord_tp)) { 1370 SET_MOTION_INPOS_FLAG(1); 1371 } 1372 break; 1373 1374 case EMCMOT_MOTION_TELEOP: 1375 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 1376 axis = &axes[axis_num]; 1377 // teleop_tp.max_vel is always positive 1378 if(axis->teleop_tp.max_vel > axis->vel_limit) { 1379 axis->teleop_tp.max_vel = axis->vel_limit; 1380 } 1381 if (update_teleop_with_check(axis_num,&(axis->teleop_tp) )) { 1382 violated_teleop_limit = 1; 1383 ext_offset_teleop_limit = 1; 1384 } else { 1385 axis->teleop_vel_cmd = axis->teleop_tp.curr_vel; 1386 axis->pos_cmd = axis->teleop_tp.curr_pos; 1387 } 1388 1389 if(!axis->teleop_tp.active) { 1390 axis->kb_ajog_active = 0; 1391 axis->wheel_ajog_active = 0; 1392 } 1393 1394 if (axis->ext_offset_tp.enable) { 1395 if (update_teleop_with_check(axis_num,&(axis->ext_offset_tp)) ) { 1396 violated_teleop_limit = 1; 1397 ext_offset_teleop_limit = 1; 1398 } 1399 } 1400 } 1401 if (!violated_teleop_limit) { 1402 ext_offset_teleop_limit = 0; 1403 ext_offset_coord_limit = 0; //in case was set in prior coord motion 1404 } 1405 1406 sync_carte_pos_to_teleop_tp(+1); // teleop 1407 1408 /* the next position then gets run through the inverse kins, 1409 to compute the next positions of the joints */ 1410 1411 /* OUTPUT KINEMATICS - convert to joints in local array */ 1412 result = kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions, &iflags, &fflags); 1413 1414 /* copy to joint structures and spline them up */ 1415 if(result == 0) 1416 { 1417 for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { 1418 if(!isfinite(positions[joint_num])) 1419 { 1420 reportError(_("kinematicsInverse gave non-finite joint location on joint %d"), 1421 joint_num); 1422 SET_MOTION_ERROR_FLAG(1); 1423 SET_MOTION_ENABLE_FLAG(0); 1424 emcmotDebug->enabling = 0; 1425 break; 1426 } 1427 /* point to joint struct */ 1428 joint = &joints[joint_num]; 1429 joint->coarse_pos = positions[joint_num]; 1430 /* spline joints up-- note that we may be adding points 1431 that fail soft limits, but we'll abort at the end of 1432 this cycle so it doesn't really matter */ 1433 cubicAddPoint(&(joint->cubic), joint->coarse_pos); 1434 /* interpolate to get new position and velocity */ 1435 joint->pos_cmd = cubicInterpolate(&(joint->cubic), 0, &(joint->vel_cmd), &(joint->acc_cmd), 0); 1436 } 1437 } 1438 else 1439 { 1440 reportError(_("kinematicsInverse failed")); 1441 SET_MOTION_ERROR_FLAG(1); 1442 SET_MOTION_ENABLE_FLAG(0); 1443 emcmotDebug->enabling = 0; 1444 break; 1445 } 1446 1447 1448 /* END OF OUTPUT KINS */ 1449 1450 /* end of teleop mode */ 1451 break; 1452 1453 case EMCMOT_MOTION_DISABLED: 1454 /* set position commands to match feedbacks, this avoids 1455 disturbances and/or following errors when enabling */ 1456 emcmotStatus->carte_pos_cmd = emcmotStatus->carte_pos_fb; 1457 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 1458 /* point to joint struct */ 1459 joint = &joints[joint_num]; 1460 /* save old command */ 1461 joint->pos_cmd = joint->pos_fb; 1462 /* set joint velocity and acceleration to zero */ 1463 joint->vel_cmd = 0.0; 1464 joint->acc_cmd = 0.0; 1465 } 1466 1467 break; 1468 default: 1469 break; 1470 } 1471 /* check command against soft limits */ 1472 /* This is a backup check, it should be impossible to command 1473 a move outside the soft limits. However there is at least 1474 two cases that isn't caught upstream: 1475 1) if an arc has both endpoints inside the limits, but the curve extends outside, 1476 2) if homing params are wrong then after homing joint pos_cmd are outside, 1477 the upstream checks will pass it. 1478 */ 1479 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 1480 /* point to joint data */ 1481 joint = &joints[joint_num]; 1482 /* skip inactive or unhomed axes */ 1483 if ((!GET_JOINT_ACTIVE_FLAG(joint)) || (!get_homed(joint_num))) { 1484 continue; 1485 } 1486 1487 /* check for soft limits */ 1488 if (joint->pos_cmd > joint->max_pos_limit) { 1489 joint_limit[joint_num][1] = 1; 1490 onlimit = 1; 1491 } 1492 else if (joint->pos_cmd < joint->min_pos_limit) { 1493 joint_limit[joint_num][0] = 1; 1494 onlimit = 1; 1495 } 1496 } 1497 if ( onlimit ) { 1498 if ( ! emcmotStatus->on_soft_limit ) { 1499 /* just hit the limit */ 1500 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 1501 if (joint_limit[joint_num][0] == 1) { 1502 joint = &joints[joint_num]; 1503 reportError(_("Exceeded NEGATIVE soft limit (%.5f) on joint %d\n"), 1504 joint->min_pos_limit, joint_num); 1505 if (emcmotConfig->kinType == KINEMATICS_IDENTITY) { 1506 reportError(_("Stop, fix joints axis LIMITS, then Restart")); 1507 } else { 1508 reportError(_("Hint: switch to joint mode to jog off soft limit")); 1509 } 1510 } else if (joint_limit[joint_num][1] == 1) { 1511 joint = &joints[joint_num]; 1512 reportError(_("Exceeded POSITIVE soft limit (%.5f) on joint %d\n"), 1513 joint->max_pos_limit,joint_num); 1514 if (emcmotConfig->kinType == KINEMATICS_IDENTITY) { 1515 reportError(_("Stop, fix joints and axis LIMITS, then Restart")); 1516 } else { 1517 reportError(_("Hint: switch to joint mode to jog off soft limit")); 1518 } 1519 } 1520 } 1521 SET_MOTION_ERROR_FLAG(1); 1522 emcmotStatus->on_soft_limit = 1; 1523 } 1524 } else { 1525 emcmotStatus->on_soft_limit = 0; 1526 } 1527 if ( emcmotDebug->teleoperating 1528 && GET_MOTION_TELEOP_FLAG() 1529 && emcmotStatus->on_soft_limit ) { 1530 SET_MOTION_ERROR_FLAG(1); 1531 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 1532 axis = &axes[axis_num]; 1533 axis->teleop_tp.enable = 0; 1534 axis->teleop_tp.curr_vel = 0.0; 1535 } 1536 } 1537 if (ext_offset_teleop_limit || ext_offset_coord_limit) { 1538 *(emcmot_hal_data->eoffset_limited) = 1; 1539 } else { 1540 *(emcmot_hal_data->eoffset_limited) = 0; 1541 } 1542 } // get_pos_cmds() 1543 1544 /* NOTES: These notes are just my understanding of how things work. 1545 1546 There are seven sets of position information. 1547 1548 1) emcmotStatus->carte_pos_cmd 1549 2) emcmotStatus->joints[n].coarse_pos 1550 3) emcmotStatus->joints[n].pos_cmd 1551 4) emcmotStatus->joints[n].motor_pos_cmd 1552 5) emcmotStatus->joints[n].motor_pos_fb 1553 6) emcmotStatus->joints[n].pos_fb 1554 7) emcmotStatus->carte_pos_fb 1555 1556 Their exact contents and meaning are as follows: 1557 1558 1) This is the desired position, in Cartesean coordinates. It is 1559 updated at the traj rate, not the servo rate. 1560 In coord mode, it is determined by the traj planner 1561 In teleop mode, it is determined by the traj planner? 1562 In free mode, it is not used, since free mode motion takes 1563 place in joint space, not cartesean space. It may be 1564 displayed by the GUI however, so it is updated by 1565 applying forward kins to (2), unless forward kins are 1566 not available, in which case it is copied from (7). 1567 1568 2) This is the desired position, in joint coordinates, but 1569 before interpolation. It is updated at the traj rate, not 1570 the servo rate.. 1571 In coord mode, it is generated by applying inverse kins to (1) 1572 In teleop mode, it is generated by applying inverse kins to (1) 1573 In free mode, it is not used, since the free mode planner generates 1574 a new (3) position every servo period without interpolation. 1575 However, it is used indirectly by GUIs, so it is copied from (3). 1576 1577 3) This is the desired position, in joint coords, after interpolation. 1578 A new set of these coords is generated every servo period. 1579 In coord mode, it is generated from (2) by the interpolator. 1580 In teleop mode, it is generated from (2) by the interpolator. 1581 In free mode, it is generated by the simple free mode traj planner. 1582 1583 4) This is the desired position, in motor coords. Motor coords are 1584 generated by adding backlash compensation, lead screw error 1585 compensation, and offset (for homing) to (3). 1586 It is generated the same way regardless of the mode, and is the 1587 output to the PID loop or other position loop. 1588 1589 5) This is the actual position, in motor coords. It is the input from 1590 encoders or other feedback device (or from virtual encoders on open 1591 loop machines). It is "generated" by reading the feedback device. 1592 1593 6) This is the actual position, in joint coordinates. It is generated 1594 by subtracting offset, lead screw error compensation, and backlash 1595 compensation from (5). It is generated the same way regardless of 1596 the operating mode. 1597 1598 7) This is the actual position, in Cartesean coordinates. It is updated 1599 at the traj rate, not the servo rate. 1600 OLD VERSION: 1601 In the old version, there are four sets of code to generate actualPos. 1602 One for each operating mode, and one for when motion is disabled. 1603 The code for coord and teleop modes is identical. The code for free 1604 mode is somewhat different, in particular to deal with the case where 1605 one or more axes are not homed. The disabled code is quite similar, 1606 but not identical, to the coord mode code. In general, the code 1607 calculates actualPos by applying the forward kins to (6). However, 1608 where forward kins are not available, actualPos is either copied 1609 from (1) (assumes no following error), or simply left alone. 1610 These special cases are handled differently for each operating mode. 1611 NEW VERSION: 1612 I would like to both simplify and relocate this. As far as I can 1613 tell, actualPos should _always_ be the best estimate of the actual 1614 machine position in Cartesean coordinates. So it should always be 1615 calculated the same way. 1616 In addition to always using the same code to calculate actualPos, 1617 I want to move that code. It is really a feedback calculation, and 1618 as such it belongs with the rest of the feedback calculations early 1619 in control.c, not as part of the output generation code as it is now. 1620 Ideally, actualPos would always be calculated by applying forward 1621 kinematics to (6). However, forward kinematics may not be available, 1622 or they may be unusable because one or more axes aren't homed. In 1623 that case, the options are: A) fake it by copying (1), or B) admit 1624 that we don't really know the Cartesean coordinates, and simply 1625 don't update actualPos. Whatever approach is used, I can see no 1626 reason not to do it the same way regardless of the operating mode. 1627 I would propose the following: If there are forward kins, use them, 1628 unless they don't work because of unhomed axes or other problems, 1629 in which case do (B). If no forward kins, do (A), since otherwise 1630 actualPos would _never_ get updated. 1631 1632 */ 1633 1634 static void compute_screw_comp(void) 1635 { 1636 int joint_num; 1637 emcmot_joint_t *joint; 1638 emcmot_comp_t *comp; 1639 double dpos; 1640 double a_max, v_max, v, s_to_go, ds_stop, ds_vel, ds_acc, dv_acc; 1641 1642 1643 /* compute the correction */ 1644 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 1645 /* point to joint struct */ 1646 joint = &joints[joint_num]; 1647 if (!GET_JOINT_ACTIVE_FLAG(joint)) { 1648 /* if joint is not active, skip it */ 1649 continue; 1650 } 1651 /* point to compensation data */ 1652 comp = &(joint->comp); 1653 if ( comp->entries > 0 ) { 1654 /* there is data in the comp table, use it */ 1655 /* first make sure we're in the right spot in the table */ 1656 while ( joint->pos_cmd < comp->entry->nominal ) { 1657 comp->entry--; 1658 } 1659 while ( joint->pos_cmd >= (comp->entry+1)->nominal ) { 1660 comp->entry++; 1661 } 1662 /* now interpolate */ 1663 dpos = joint->pos_cmd - comp->entry->nominal; 1664 if (joint->vel_cmd > 0.0) { 1665 /* moving "up". apply forward screw comp */ 1666 joint->backlash_corr = comp->entry->fwd_trim + 1667 comp->entry->fwd_slope * dpos; 1668 } else if (joint->vel_cmd < 0.0) { 1669 /* moving "down". apply reverse screw comp */ 1670 joint->backlash_corr = comp->entry->rev_trim + 1671 comp->entry->rev_slope * dpos; 1672 } else { 1673 /* not moving, use whatever was there before */ 1674 } 1675 } else { 1676 /* no compensation data, just use +/- 1/2 of backlash */ 1677 /** FIXME: this can actually be removed - if the user space code 1678 sends a single compensation entry with any nominal value, 1679 and with fwd_trim = +0.5 times the backlash value, and 1680 rev_trim = -0.5 times backlash, the above screw comp code 1681 will give exactly the same result as this code. */ 1682 /* determine which way the compensation should be applied */ 1683 if (joint->vel_cmd > 0.0) { 1684 /* moving "up". apply positive backlash comp */ 1685 joint->backlash_corr = 0.5 * joint->backlash; 1686 } else if (joint->vel_cmd < 0.0) { 1687 /* moving "down". apply negative backlash comp */ 1688 joint->backlash_corr = -0.5 * joint->backlash; 1689 } else { 1690 /* not moving, use whatever was there before */ 1691 } 1692 } 1693 /* at this point, the correction has been computed, but 1694 the value may make abrupt jumps on direction reversal */ 1695 /* 1696 * 07/09/2005 - S-curve implementation by Bas Laarhoven 1697 * 1698 * Implementation: 1699 * Generate a ramped velocity profile for backlash or screw error comp. 1700 * The velocity is ramped up to the maximum speed setting (if possible), 1701 * using the maximum acceleration setting. 1702 * At the end, the speed is ramped dowm using the same acceleration. 1703 * The algorithm keeps looking ahead. Depending on the distance to go, 1704 * the speed is increased, kept constant or decreased. 1705 * 1706 * Limitations: 1707 * Since the compensation adds up to the normal movement, total 1708 * accelleration and total velocity may exceed maximum settings! 1709 * Currently this is limited to 150% by implementation. 1710 * To fix this, the calculations in get_pos_cmd should include 1711 * information from the backlash corection. This makes things 1712 * rather complicated and it might be better to implement the 1713 * backlash compensation at another place to prevent this kind 1714 * of interaction. 1715 * More testing under different circumstances will show if this 1716 * needs a more complicate solution. 1717 * For now this implementation seems to generate smoother 1718 * movements and less following errors than the original code. 1719 */ 1720 1721 /* Limit maximum accelleration and velocity 'overshoot' 1722 * to 150% of the maximum settings. 1723 * The TP and backlash shouldn't use more than 100% 1724 * (together) but this requires some interaction that 1725 * isn't implemented yet. 1726 */ 1727 v_max = 0.5 * joint->vel_limit * emcmotStatus->net_feed_scale; 1728 a_max = 0.5 * joint->acc_limit; 1729 v = joint->backlash_vel; 1730 if (joint->backlash_corr >= joint->backlash_filt) { 1731 s_to_go = joint->backlash_corr - joint->backlash_filt; /* abs val */ 1732 if (s_to_go > 0) { 1733 // off target, need to move 1734 ds_vel = v * servo_period; /* abs val */ 1735 dv_acc = a_max * servo_period; /* abs val */ 1736 ds_stop = 0.5 * (v + dv_acc) * 1737 (v + dv_acc) / a_max; /* abs val */ 1738 if (s_to_go <= ds_stop + ds_vel) { 1739 // ramp down 1740 if (v > dv_acc) { 1741 // decellerate one period 1742 ds_acc = 0.5 * dv_acc * servo_period; /* abs val */ 1743 joint->backlash_vel -= dv_acc; 1744 joint->backlash_filt += ds_vel - ds_acc; 1745 } else { 1746 // last step to target 1747 joint->backlash_vel = 0.0; 1748 joint->backlash_filt = joint->backlash_corr; 1749 } 1750 } else { 1751 if (v + dv_acc > v_max) { 1752 dv_acc = v_max - v; /* abs val */ 1753 } 1754 ds_acc = 0.5 * dv_acc * servo_period; /* abs val */ 1755 ds_stop = 0.5 * (v + dv_acc) * 1756 (v + dv_acc) / a_max; /* abs val */ 1757 if (s_to_go > ds_stop + ds_vel + ds_acc) { 1758 // ramp up 1759 joint->backlash_vel += dv_acc; 1760 joint->backlash_filt += ds_vel + ds_acc; 1761 } else { 1762 // constant velocity 1763 joint->backlash_filt += ds_vel; 1764 } 1765 } 1766 } else if (s_to_go < 0) { 1767 // safely handle overshoot (should not occur) 1768 joint->backlash_vel = 0.0; 1769 joint->backlash_filt = joint->backlash_corr; 1770 } 1771 } else { /* joint->backlash_corr < 0.0 */ 1772 s_to_go = joint->backlash_filt - joint->backlash_corr; /* abs val */ 1773 if (s_to_go > 0) { 1774 // off target, need to move 1775 ds_vel = -v * servo_period; /* abs val */ 1776 dv_acc = a_max * servo_period; /* abs val */ 1777 ds_stop = 0.5 * (v - dv_acc) * 1778 (v - dv_acc) / a_max; /* abs val */ 1779 if (s_to_go <= ds_stop + ds_vel) { 1780 // ramp down 1781 if (-v > dv_acc) { 1782 // decellerate one period 1783 ds_acc = 0.5 * dv_acc * servo_period; /* abs val */ 1784 joint->backlash_vel += dv_acc; /* decrease */ 1785 joint->backlash_filt -= ds_vel - ds_acc; 1786 } else { 1787 // last step to target 1788 joint->backlash_vel = 0.0; 1789 joint->backlash_filt = joint->backlash_corr; 1790 } 1791 } else { 1792 if (-v + dv_acc > v_max) { 1793 dv_acc = v_max + v; /* abs val */ 1794 } 1795 ds_acc = 0.5 * dv_acc * servo_period; /* abs val */ 1796 ds_stop = 0.5 * (v - dv_acc) * 1797 (v - dv_acc) / a_max; /* abs val */ 1798 if (s_to_go > ds_stop + ds_vel + ds_acc) { 1799 // ramp up 1800 joint->backlash_vel -= dv_acc; /* increase */ 1801 joint->backlash_filt -= ds_vel + ds_acc; 1802 } else { 1803 // constant velocity 1804 joint->backlash_filt -= ds_vel; 1805 } 1806 } 1807 } else if (s_to_go < 0) { 1808 // safely handle overshoot (should not occur) 1809 joint->backlash_vel = 0.0; 1810 joint->backlash_filt = joint->backlash_corr; 1811 } 1812 } 1813 /* backlash (and motor offset) will be applied to output later */ 1814 /* end of joint loop */ 1815 } 1816 } 1817 1818 /*! \todo FIXME - once the HAL refactor is done so that metadata isn't stored 1819 in shared memory, I want to seriously consider moving some of the 1820 structures into the HAL memory block. This will eliminate most of 1821 this useless copying, and make nearly everything accessible to 1822 halscope and halmeter for debugging. 1823 */ 1824 1825 static void output_to_hal(void) 1826 { 1827 int joint_num, axis_num, spindle_num; 1828 emcmot_joint_t *joint; 1829 emcmot_axis_t *axis; 1830 joint_hal_t *joint_data; 1831 axis_hal_t *axis_data; 1832 static int old_motion_index[EMCMOT_MAX_SPINDLES] = {0}; 1833 static int old_hal_index[EMCMOT_MAX_SPINDLES] = {0}; 1834 1835 /* output machine info to HAL for scoping, etc */ 1836 *(emcmot_hal_data->motion_enabled) = GET_MOTION_ENABLE_FLAG(); 1837 *(emcmot_hal_data->in_position) = GET_MOTION_INPOS_FLAG(); 1838 *(emcmot_hal_data->coord_mode) = GET_MOTION_COORD_FLAG(); 1839 *(emcmot_hal_data->teleop_mode) = GET_MOTION_TELEOP_FLAG(); 1840 *(emcmot_hal_data->coord_error) = GET_MOTION_ERROR_FLAG(); 1841 *(emcmot_hal_data->on_soft_limit) = emcmotStatus->on_soft_limit; 1842 1843 for (spindle_num = 0; spindle_num < emcmotConfig->numSpindles; spindle_num++){ 1844 if(emcmotStatus->spindle_status[spindle_num].css_factor) { 1845 double denom = fabs(emcmotStatus->spindle_status[spindle_num].xoffset 1846 - emcmotStatus->carte_pos_cmd.tran.x); 1847 double speed; 1848 double maxpositive; 1849 if(denom > 0) speed = emcmotStatus->spindle_status[spindle_num].css_factor / denom; 1850 else speed = emcmotStatus->spindle_status[spindle_num].speed; 1851 1852 speed = speed * emcmotStatus->spindle_status[spindle_num].net_scale; 1853 maxpositive = fabs(emcmotStatus->spindle_status[spindle_num].speed); 1854 // cap speed to G96 D... 1855 if(speed < -maxpositive) 1856 speed = -maxpositive; 1857 if(speed > maxpositive) 1858 speed = maxpositive; 1859 1860 *(emcmot_hal_data->spindle[spindle_num].spindle_speed_out) = speed; 1861 *(emcmot_hal_data->spindle[spindle_num].spindle_speed_out_rps) = speed/60.; 1862 } else { 1863 *(emcmot_hal_data->spindle[spindle_num].spindle_speed_out) = 1864 emcmotStatus->spindle_status[spindle_num].speed * 1865 emcmotStatus->spindle_status[spindle_num].net_scale; 1866 *(emcmot_hal_data->spindle[spindle_num].spindle_speed_out_rps) = 1867 emcmotStatus->spindle_status[spindle_num].speed * 1868 emcmotStatus->spindle_status[spindle_num].net_scale / 60.; 1869 } 1870 *(emcmot_hal_data->spindle[spindle_num].spindle_speed_out_abs) = 1871 fabs(*(emcmot_hal_data->spindle[spindle_num].spindle_speed_out)); 1872 *(emcmot_hal_data->spindle[spindle_num].spindle_speed_out_rps_abs) = 1873 fabs(*(emcmot_hal_data->spindle[spindle_num].spindle_speed_out_rps)); 1874 *(emcmot_hal_data->spindle[spindle_num].spindle_speed_cmd_rps) = 1875 emcmotStatus->spindle_status[spindle_num].speed / 60.; 1876 *(emcmot_hal_data->spindle[spindle_num].spindle_on) = 1877 ((emcmotStatus->spindle_status[spindle_num].speed * 1878 emcmotStatus->spindle_status[spindle_num].net_scale) != 0) ? 1 : 0; 1879 *(emcmot_hal_data->spindle[spindle_num].spindle_forward) = 1880 (*emcmot_hal_data->spindle[spindle_num].spindle_speed_out > 0) ? 1 : 0; 1881 *(emcmot_hal_data->spindle[spindle_num].spindle_reverse) = 1882 (*emcmot_hal_data->spindle[spindle_num].spindle_speed_out < 0) ? 1 : 0; 1883 *(emcmot_hal_data->spindle[spindle_num].spindle_brake) = 1884 (emcmotStatus->spindle_status[spindle_num].brake != 0) ? 1 : 0; 1885 } 1886 1887 *(emcmot_hal_data->program_line) = emcmotStatus->id; 1888 *(emcmot_hal_data->tp_reverse) = emcmotStatus->reverse_run; 1889 *(emcmot_hal_data->motion_type) = emcmotStatus->motionType; 1890 *(emcmot_hal_data->distance_to_go) = emcmotStatus->distance_to_go; 1891 if(GET_MOTION_COORD_FLAG()) { 1892 *(emcmot_hal_data->current_vel) = emcmotStatus->current_vel; 1893 *(emcmot_hal_data->requested_vel) = emcmotStatus->requested_vel; 1894 } else if (GET_MOTION_TELEOP_FLAG()) { 1895 int i; 1896 double v2 = 0.0; 1897 for(i=0; i < EMCMOT_MAX_AXIS; i++) 1898 if(axes[i].teleop_tp.active) 1899 v2 += axes[i].teleop_vel_cmd * axes[i].teleop_vel_cmd; 1900 if(v2 > 0.0) 1901 emcmotStatus->current_vel = (*emcmot_hal_data->current_vel) = sqrt(v2); 1902 else 1903 emcmotStatus->current_vel = (*emcmot_hal_data->current_vel) = 0.0; 1904 *(emcmot_hal_data->requested_vel) = 0.0; 1905 } else { 1906 int i; 1907 double v2 = 0.0; 1908 for(i=0; i < ALL_JOINTS; i++) 1909 if(GET_JOINT_ACTIVE_FLAG(&(joints[i])) && joints[i].free_tp.active) 1910 v2 += joints[i].vel_cmd * joints[i].vel_cmd; 1911 if(v2 > 0.0) 1912 emcmotStatus->current_vel = (*emcmot_hal_data->current_vel) = sqrt(v2); 1913 else 1914 emcmotStatus->current_vel = (*emcmot_hal_data->current_vel) = 0.0; 1915 *(emcmot_hal_data->requested_vel) = 0.0; 1916 } 1917 1918 /* These params can be used to examine any internal variable. */ 1919 /* Change the following lines to assign the variable you want to observe 1920 to one of the debug parameters. You can also comment out these lines 1921 and copy elsewhere if you want to observe an automatic variable that 1922 isn't in scope here. */ 1923 emcmot_hal_data->debug_bit_0 = joints[1].free_tp.active; 1924 emcmot_hal_data->debug_bit_1 = emcmotStatus->enables_new & AF_ENABLED; 1925 emcmot_hal_data->debug_float_0 = emcmotStatus->spindle_status[0].speed; 1926 emcmot_hal_data->debug_float_1 = emcmotStatus->spindleSync; 1927 emcmot_hal_data->debug_float_2 = emcmotStatus->vel; 1928 emcmot_hal_data->debug_float_3 = emcmotStatus->spindle_status[0].net_scale; 1929 emcmot_hal_data->debug_s32_0 = emcmotStatus->overrideLimitMask; 1930 emcmot_hal_data->debug_s32_1 = emcmotStatus->tcqlen; 1931 1932 /* two way handshaking for the spindle encoder */ 1933 for (spindle_num = 0; spindle_num < emcmotConfig->numSpindles; spindle_num++){ 1934 if(emcmotStatus->spindle_status[spindle_num].spindle_index_enable 1935 && !old_motion_index[spindle_num]) { 1936 *emcmot_hal_data->spindle[spindle_num].spindle_index_enable = 1; 1937 rtapi_print_msg(RTAPI_MSG_DBG, "setting index-enable on spindle %d\n", spindle_num); 1938 } 1939 1940 if(!*emcmot_hal_data->spindle[spindle_num].spindle_index_enable 1941 && old_hal_index[spindle_num]) { 1942 emcmotStatus->spindle_status[spindle_num].spindle_index_enable = 0; 1943 } 1944 1945 old_motion_index[spindle_num] = 1946 emcmotStatus->spindle_status[spindle_num].spindle_index_enable; 1947 old_hal_index[spindle_num] = 1948 *emcmot_hal_data->spindle[spindle_num].spindle_index_enable; 1949 } 1950 1951 *(emcmot_hal_data->tooloffset_x) = emcmotStatus->tool_offset.tran.x; 1952 *(emcmot_hal_data->tooloffset_y) = emcmotStatus->tool_offset.tran.y; 1953 *(emcmot_hal_data->tooloffset_z) = emcmotStatus->tool_offset.tran.z; 1954 *(emcmot_hal_data->tooloffset_a) = emcmotStatus->tool_offset.a; 1955 *(emcmot_hal_data->tooloffset_b) = emcmotStatus->tool_offset.b; 1956 *(emcmot_hal_data->tooloffset_c) = emcmotStatus->tool_offset.c; 1957 *(emcmot_hal_data->tooloffset_u) = emcmotStatus->tool_offset.u; 1958 *(emcmot_hal_data->tooloffset_v) = emcmotStatus->tool_offset.v; 1959 *(emcmot_hal_data->tooloffset_w) = emcmotStatus->tool_offset.w; 1960 1961 /* output joint info to HAL for scoping, etc */ 1962 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 1963 /* point to joint struct */ 1964 joint = &joints[joint_num]; 1965 joint_data = &(emcmot_hal_data->joint[joint_num]); 1966 1967 /* apply backlash and motor offset to output */ 1968 joint->motor_pos_cmd = 1969 joint->pos_cmd + joint->backlash_filt + joint->motor_offset; 1970 /* point to HAL data */ 1971 /* write to HAL pins */ 1972 *(joint_data->motor_offset) = joint->motor_offset; 1973 *(joint_data->motor_pos_cmd) = joint->motor_pos_cmd; 1974 *(joint_data->joint_pos_cmd) = joint->pos_cmd; 1975 *(joint_data->joint_pos_fb) = joint->pos_fb; 1976 *(joint_data->amp_enable) = GET_JOINT_ENABLE_FLAG(joint); 1977 1978 *(joint_data->coarse_pos_cmd) = joint->coarse_pos; 1979 *(joint_data->joint_vel_cmd) = joint->vel_cmd; 1980 *(joint_data->joint_acc_cmd) = joint->acc_cmd; 1981 *(joint_data->backlash_corr) = joint->backlash_corr; 1982 *(joint_data->backlash_filt) = joint->backlash_filt; 1983 *(joint_data->backlash_vel) = joint->backlash_vel; 1984 *(joint_data->f_error) = joint->ferror; 1985 *(joint_data->f_error_lim) = joint->ferror_limit; 1986 1987 *(joint_data->free_pos_cmd) = joint->free_tp.pos_cmd; 1988 *(joint_data->free_vel_lim) = joint->free_tp.max_vel; 1989 *(joint_data->free_tp_enable) = joint->free_tp.enable; 1990 *(joint_data->kb_jjog_active) = joint->kb_jjog_active; 1991 *(joint_data->wheel_jjog_active) = joint->wheel_jjog_active; 1992 1993 *(joint_data->active) = GET_JOINT_ACTIVE_FLAG(joint); 1994 *(joint_data->in_position) = GET_JOINT_INPOS_FLAG(joint); 1995 *(joint_data->error) = GET_JOINT_ERROR_FLAG(joint); 1996 *(joint_data->phl) = GET_JOINT_PHL_FLAG(joint); 1997 *(joint_data->nhl) = GET_JOINT_NHL_FLAG(joint); 1998 *(joint_data->f_errored) = GET_JOINT_FERROR_FLAG(joint); 1999 *(joint_data->faulted) = GET_JOINT_FAULT_FLAG(joint); 2000 2001 // conditionally remove outstanding requests to unlock rotaries: 2002 if ( !GET_MOTION_ENABLE_FLAG() && (joint_is_lockable(joint_num))) { 2003 *(joint_data->unlock) = 0; 2004 } 2005 2006 if (IS_EXTRA_JOINT(joint_num) && get_homed(joint_num)) { 2007 // passthru posthome_cmd with motor_offset 2008 // to hal pin: joint.N.motor-pos-cmd 2009 extrajoint_hal_t *ejoint_data; 2010 int e = joint_num - NO_OF_KINS_JOINTS; 2011 ejoint_data = &(emcmot_hal_data->ejoint[e]); 2012 *(joint_data->motor_pos_cmd) = *(ejoint_data->posthome_cmd) 2013 + joint->motor_offset; 2014 continue; 2015 } 2016 } // for joint_num 2017 2018 /* output axis info to HAL for scoping, etc */ 2019 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 2020 /* point to axis struct */ 2021 axis = &axes[axis_num]; 2022 /* point to HAL data */ 2023 axis_data = &(emcmot_hal_data->axis[axis_num]); 2024 /* write to HAL pins */ 2025 *(axis_data->teleop_vel_cmd) = axis->teleop_vel_cmd; 2026 *(axis_data->teleop_pos_cmd) = axis->teleop_tp.pos_cmd; 2027 *(axis_data->teleop_vel_lim) = axis->teleop_tp.max_vel; 2028 *(axis_data->teleop_tp_enable) = axis->teleop_tp.enable; 2029 *(axis_data->kb_ajog_active) = axis->kb_ajog_active; 2030 *(axis_data->wheel_ajog_active) = axis->wheel_ajog_active; 2031 2032 // hal pins: axis.L.pos-cmd reported without applied offsets: 2033 *(axis_data->pos_cmd) = *pcmd_p[axis_num] 2034 - axis->ext_offset_tp.curr_pos; 2035 } 2036 } 2037 2038 static void update_status(void) 2039 { 2040 int joint_num, axis_num, dio, aio; 2041 emcmot_joint_t *joint; 2042 emcmot_joint_status_t *joint_status; 2043 emcmot_axis_t *axis; 2044 emcmot_axis_status_t *axis_status; 2045 #ifdef WATCH_FLAGS 2046 static int old_joint_flags[8]; 2047 static int old_motion_flag; 2048 #endif 2049 2050 /* copy status info from private joint structure to status 2051 struct in shared memory */ 2052 for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) { 2053 /* point to joint data */ 2054 joint = &joints[joint_num]; 2055 /* point to joint status */ 2056 joint_status = &(emcmotStatus->joint_status[joint_num]); 2057 /* copy stuff */ 2058 #ifdef WATCH_FLAGS 2059 /*! \todo FIXME - this is for debugging */ 2060 if ( old_joint_flags[joint_num] != joint->flag ) { 2061 rtapi_print ( "Joint %d flag %04X -> %04X\n", joint_num, old_joint_flags[joint_num], joint->flag ); 2062 old_joint_flags[joint_num] = joint->flag; 2063 } 2064 #endif 2065 joint_status->flag = joint->flag; 2066 joint_status->homing = get_homing(joint_num); 2067 joint_status->homed = get_homed(joint_num); 2068 joint_status->pos_cmd = joint->pos_cmd; 2069 joint_status->pos_fb = joint->pos_fb; 2070 joint_status->vel_cmd = joint->vel_cmd; 2071 joint_status->acc_cmd = joint->acc_cmd; 2072 joint_status->ferror = joint->ferror; 2073 joint_status->ferror_high_mark = joint->ferror_high_mark; 2074 joint_status->backlash = joint->backlash; 2075 joint_status->max_pos_limit = joint->max_pos_limit; 2076 joint_status->min_pos_limit = joint->min_pos_limit; 2077 joint_status->min_ferror = joint->min_ferror; 2078 joint_status->max_ferror = joint->max_ferror; 2079 } 2080 2081 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 2082 /* point to axis data */ 2083 axis = &axes[axis_num]; 2084 /* point to axis status */ 2085 axis_status = &(emcmotStatus->axis_status[axis_num]); 2086 2087 axis_status->teleop_vel_cmd = axis->teleop_vel_cmd; 2088 axis_status->max_pos_limit = axis->max_pos_limit; 2089 axis_status->min_pos_limit = axis->min_pos_limit; 2090 } 2091 emcmotStatus->eoffset_pose.tran.x = (&axes[0])->ext_offset_tp.curr_pos; 2092 emcmotStatus->eoffset_pose.tran.y = (&axes[1])->ext_offset_tp.curr_pos; 2093 emcmotStatus->eoffset_pose.tran.z = (&axes[2])->ext_offset_tp.curr_pos; 2094 emcmotStatus->eoffset_pose.a = (&axes[3])->ext_offset_tp.curr_pos; 2095 emcmotStatus->eoffset_pose.b = (&axes[4])->ext_offset_tp.curr_pos; 2096 emcmotStatus->eoffset_pose.c = (&axes[5])->ext_offset_tp.curr_pos; 2097 emcmotStatus->eoffset_pose.u = (&axes[6])->ext_offset_tp.curr_pos; 2098 emcmotStatus->eoffset_pose.v = (&axes[7])->ext_offset_tp.curr_pos; 2099 emcmotStatus->eoffset_pose.w = (&axes[8])->ext_offset_tp.curr_pos; 2100 2101 emcmotStatus->external_offsets_applied = *(emcmot_hal_data->eoffset_active); 2102 2103 for (dio = 0; dio < emcmotConfig->numDIO; dio++) { 2104 emcmotStatus->synch_di[dio] = *(emcmot_hal_data->synch_di[dio]); 2105 emcmotStatus->synch_do[dio] = *(emcmot_hal_data->synch_do[dio]); 2106 } 2107 2108 for (aio = 0; aio < emcmotConfig->numAIO; aio++) { 2109 emcmotStatus->analog_input[aio] = *(emcmot_hal_data->analog_input[aio]); 2110 emcmotStatus->analog_output[aio] = *(emcmot_hal_data->analog_output[aio]); 2111 } 2112 2113 /*! \todo FIXME - the rest of this function is stuff that was apparently 2114 dropped in the initial move from emcmot.c to control.c. I 2115 don't know how much is still needed, and how much is baggage. 2116 */ 2117 2118 /* motion emcmotDebug->coord_tp status */ 2119 emcmotStatus->depth = tpQueueDepth(&emcmotDebug->coord_tp); 2120 emcmotStatus->activeDepth = tpActiveDepth(&emcmotDebug->coord_tp); 2121 emcmotStatus->id = tpGetExecId(&emcmotDebug->coord_tp); 2122 //KLUDGE add an API call for this 2123 emcmotStatus->reverse_run = emcmotDebug->coord_tp.reverse_run; 2124 emcmotStatus->motionType = tpGetMotionType(&emcmotDebug->coord_tp); 2125 emcmotStatus->queueFull = tcqFull(&emcmotDebug->coord_tp.queue); 2126 2127 /* check to see if we should pause in order to implement 2128 single emcmotDebug->stepping */ 2129 2130 if (emcmotDebug->stepping && emcmotDebug->idForStep != emcmotStatus->id) { 2131 tpPause(&emcmotDebug->coord_tp); 2132 emcmotDebug->stepping = 0; 2133 emcmotStatus->paused = 1; 2134 } 2135 #ifdef WATCH_FLAGS 2136 /*! \todo FIXME - this is for debugging */ 2137 if ( old_motion_flag != emcmotStatus->motionFlag ) { 2138 rtapi_print ( "Motion flag %04X -> %04X\n", old_motion_flag, emcmotStatus->motionFlag ); 2139 old_motion_flag = emcmotStatus->motionFlag; 2140 } 2141 #endif 2142 } 2143 2144 static void sync_teleop_tp_to_carte_pos(int extfactor) 2145 { 2146 int axis_num; 2147 emcmot_axis_t *axis; 2148 2149 // expect extfactor = -1 || 0 || +1 2150 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 2151 axis = &axes[axis_num]; 2152 axis->teleop_tp.curr_pos = *pcmd_p[axis_num] 2153 + extfactor * axis->ext_offset_tp.curr_pos; 2154 } 2155 } //sync_teleop_tp_to_carte_pos() 2156 2157 static void sync_carte_pos_to_teleop_tp(int extfactor) 2158 { 2159 int axis_num; 2160 emcmot_axis_t *axis; 2161 2162 // expect extfactor = -1 || 0 || +1 2163 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 2164 axis = &axes[axis_num]; 2165 *pcmd_p[axis_num] = axis->teleop_tp.curr_pos 2166 + extfactor * axis->ext_offset_tp.curr_pos; 2167 } 2168 } // sync_carte_pos_to_teleop_tp() 2169 2170 static void apply_ext_offsets_to_carte_pos(int extfactor) 2171 { 2172 int axis_num; 2173 emcmot_axis_t *axis; 2174 2175 // expect extfactor = -1 || 0 || +1 2176 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 2177 axis = &axes[axis_num]; 2178 *pcmd_p[axis_num] = *pcmd_p[axis_num] 2179 + extfactor * axis->ext_offset_tp.curr_pos; 2180 } 2181 } // apply_ext_offsets_to_carte_pos() 2182 2183 static void initialize_external_offsets() 2184 { 2185 int axis_num; 2186 emcmot_axis_t *axis; 2187 axis_hal_t *axis_data; 2188 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 2189 axis = &axes[axis_num]; 2190 axis_data = &(emcmot_hal_data->axis[axis_num]); 2191 2192 *(axis_data->external_offset) = 0; 2193 *(axis_data->external_offset_requested) = 0; 2194 axis->ext_offset_tp.pos_cmd = 0; 2195 axis->ext_offset_tp.curr_pos = 0; 2196 axis->ext_offset_tp.curr_vel = 0; 2197 } 2198 } // initialize_external_offsets() 2199 2200 static void plan_external_offsets(void) 2201 { 2202 static int first_pass = 1; 2203 int axis_num; 2204 emcmot_axis_t *axis; 2205 axis_hal_t *axis_data; 2206 int new_eoffset_counts, delta; 2207 static int last_eoffset_enable[EMCMOT_MAX_AXIS]; 2208 2209 *(emcmot_hal_data->eoffset_active) = 0; //set if any enabled 2210 2211 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 2212 axis = &axes[axis_num]; 2213 // coord,teleop updates done in get_pos_cmds() 2214 axis->ext_offset_tp.max_vel = axis->ext_offset_vel_limit; 2215 axis->ext_offset_tp.max_acc = axis->ext_offset_acc_limit; 2216 2217 axis_data = &(emcmot_hal_data->axis[axis_num]); 2218 2219 new_eoffset_counts = *(axis_data->eoffset_counts); 2220 delta = new_eoffset_counts - axis->old_eoffset_counts; 2221 axis->old_eoffset_counts = new_eoffset_counts; 2222 2223 *(axis_data->external_offset) = axis->ext_offset_tp.curr_pos; 2224 axis->ext_offset_tp.enable = 1; 2225 if ( first_pass ) { 2226 *(axis_data->external_offset) = 0; 2227 continue; 2228 } 2229 2230 // Use stopping criterion of simple_tp.c: 2231 ext_offset_epsilon = TINY_DP(axis->ext_offset_tp.max_acc,servo_period); 2232 if (fabs(*(axis_data->external_offset)) > ext_offset_epsilon) { 2233 *(emcmot_hal_data->eoffset_active) = 1; 2234 } 2235 if ( !*(axis_data->eoffset_enable) ) { 2236 axis->ext_offset_tp.enable = 0; 2237 // Detect disabling of eoffsets: 2238 // At very high accel, simple planner may terminate with 2239 // a larger position value than occurs at more realistic accels. 2240 if ( last_eoffset_enable[axis_num] 2241 && (fabs(*(axis_data->external_offset)) > ext_offset_epsilon) 2242 && GET_MOTION_ENABLE_FLAG() 2243 && axis->ext_offset_tp.enable 2244 ) { 2245 #if 1 2246 // to stdout only: 2247 rtapi_print_msg(RTAPI_MSG_NONE, 2248 "*** Axis_%c External Offset=%.4g eps=%.4g\n" 2249 "*** External Offset disabled while NON-zero\n" 2250 "*** To clear: re-enable & zero or use Machine-Off\n", 2251 "XYZABCUVW"[axis_num], 2252 *(axis_data->external_offset), 2253 ext_offset_epsilon); 2254 #else 2255 // as error message: 2256 reportError("Axis_%c External Offset=%.4g eps=%.4g\n" 2257 "External Offset disabled while NON-zero\n" 2258 "To clear: re-enable & zero or use Machine-Off", 2259 "XYZABCUVW"[axis_num], 2260 *(axis_data->external_offset), 2261 ext_offset_epsilon); 2262 #endif 2263 } 2264 last_eoffset_enable[axis_num] = 0; 2265 continue; // Note: if not eoffset_enable 2266 // then planner disabled and no pos_cmd updates 2267 // useful for eoffset_pid hold 2268 } 2269 last_eoffset_enable[axis_num] = 1; 2270 if (*(axis_data->eoffset_clear)) { 2271 axis->ext_offset_tp.pos_cmd = 0; 2272 *(axis_data->external_offset_requested) = 0; 2273 continue; 2274 } 2275 if ( delta == 0 ) { continue; } 2276 if ( !checkAllHomed() ) { continue; } 2277 if ( !GET_MOTION_ENABLE_FLAG() ) { continue; } 2278 2279 axis->ext_offset_tp.pos_cmd += delta * *(axis_data->eoffset_scale); 2280 *(axis_data->external_offset_requested) = axis->ext_offset_tp.pos_cmd; 2281 } // for axis_num 2282 first_pass = 0; 2283 } // plan_external_offsets() 2284 2285 static int update_teleop_with_check(int axis_num,simple_tp_t *the_tp) 2286 { 2287 // 'the_tp' is the planner to update 2288 // the tests herein apply to the sum of the offsets for both 2289 // planners (teleop_tp and ext_offset_tp) 2290 double save_curr_pos; 2291 emcmot_axis_t *axis = &axes[axis_num]; 2292 2293 save_curr_pos = the_tp->curr_pos; 2294 simple_tp_update(the_tp, servo_period ); 2295 2296 //workaround: axis letters not in [TRAJ]COORDINATES 2297 // have min_pos_limit == max_pos_lim == 0 2298 if ( (0 == axis->max_pos_limit) && (0 == axis->min_pos_limit) ) { 2299 return 0; 2300 } 2301 if ( (axis->ext_offset_tp.curr_pos + axis->teleop_tp.curr_pos) 2302 >= axis->max_pos_limit) { 2303 // positive error, restore save_curr_pos 2304 the_tp->curr_pos = save_curr_pos; 2305 the_tp->curr_vel = 0; 2306 return 1; 2307 } 2308 if ( (axis->ext_offset_tp.curr_pos + axis->teleop_tp.curr_pos) 2309 <= axis->min_pos_limit) { 2310 // negative error, restore save_curr_pos 2311 the_tp->curr_pos = save_curr_pos; 2312 the_tp->curr_vel = 0; 2313 return 1; 2314 } 2315 return 0; 2316 } // update_teleop_with_check() 2317 2318 static int update_coord_with_bound(void) 2319 { 2320 int axis_num; 2321 int ans = 0; 2322 emcmot_axis_t *axis; 2323 double save_pos_cmd[EMCMOT_MAX_AXIS]; 2324 double save_offset_cmd[EMCMOT_MAX_AXIS]; 2325 2326 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 2327 axis = &axes[axis_num]; 2328 save_pos_cmd[axis_num] = *pcmd_p[axis_num]; 2329 save_offset_cmd[axis_num] = axis->ext_offset_tp.pos_cmd; 2330 simple_tp_update(&(axis->ext_offset_tp), servo_period ); 2331 } 2332 apply_ext_offsets_to_carte_pos(+1); // add external offsets 2333 2334 for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) { 2335 axis = &axes[axis_num]; 2336 //workaround: axis letters not in [TRAJ]COORDINATES 2337 // have min_pos_limit == max_pos_lim == 0 2338 if ( (0 == axis->max_pos_limit) && (0 == axis->min_pos_limit) ) { 2339 continue; 2340 } 2341 if (axis->ext_offset_tp.curr_pos == 0) { 2342 continue; // don't claim violation if no offset 2343 } 2344 2345 if (*pcmd_p[axis_num] >= axis->max_pos_limit) { 2346 // hold carte_pos_cmd at the limit: 2347 *pcmd_p[axis_num] = axis->max_pos_limit; 2348 // stop growth of offsetting position: 2349 axis->ext_offset_tp.curr_pos = axis->max_pos_limit 2350 - save_pos_cmd[axis_num]; 2351 if (axis->ext_offset_tp.pos_cmd > save_offset_cmd[axis_num]) { 2352 axis->ext_offset_tp.pos_cmd = save_offset_cmd[axis_num]; 2353 } 2354 axis->ext_offset_tp.curr_vel = 0; 2355 ans++; 2356 continue; 2357 } 2358 if (*pcmd_p[axis_num] <= axis->min_pos_limit) { 2359 *pcmd_p[axis_num] = axis->min_pos_limit; 2360 axis->ext_offset_tp.curr_pos = axis->min_pos_limit 2361 - save_pos_cmd[axis_num]; 2362 if (axis->ext_offset_tp.pos_cmd < save_offset_cmd[axis_num]) { 2363 axis->ext_offset_tp.pos_cmd = save_offset_cmd[axis_num]; 2364 } 2365 axis->ext_offset_tp.curr_vel = 0; 2366 ans++; 2367 } 2368 } 2369 if (ans > 0) { return 1; } 2370 return 0; 2371 } // update_coord_with_bound()