1  /********************************************************************
   2  * Description: taskintf.cc
   3  *   Interface functions for motion.
   4  *
   5  *   Derived from a work by Fred Proctor & Will Shackleford
   6  *
   7  * Author:
   8  * License: GPL Version 2
   9  * System: Linux
  10  *    
  11  * Copyright (c) 2004 All rights reserved.
  12  *
  13  ********************************************************************/
  14  
  15  #include <cmath>
  16  #include <float.h>		// DBL_MAX
  17  #include <string.h>		// memcpy() strncpy()
  18  #include <unistd.h>             // unlink()
  19  
  20  #include "usrmotintf.h"		// usrmotInit(), usrmotReadEmcmotStatus(),
  21  				// etc.
  22  #include "motion.h"		// emcmot_command_t,STATUS, etc.
  23  #include "motion_debug.h"
  24  #include "emc.hh"
  25  #include "emccfg.h"		// EMC_INIFILE
  26  #include "emcglb.h"		// EMC_INIFILE
  27  #include "emc_nml.hh"
  28  #include "rcs_print.hh"
  29  #include "timer.hh"
  30  #include "inifile.hh"
  31  #include "iniaxis.hh"
  32  #include "inijoint.hh"
  33  #include "initraj.hh"
  34  #include "inihal.hh"
  35  
  36  value_inihal_data old_inihal_data;
  37  
  38  /* define this to catch isnan errors, for rtlinux FPU register 
  39     problem testing */
  40  #define ISNAN_TRAP
  41  
  42  #ifdef ISNAN_TRAP
  43  #define CATCH_NAN(cond) do {                           \
  44      if (cond) {                                        \
  45          printf("isnan error in %s()\n", __FUNCTION__); \
  46          return -1;                                     \
  47      }                                                  \
  48  } while(0)
  49  #else
  50  #define CATCH_NAN(cond) do {} while(0)
  51  #endif
  52  
  53  
  54  // MOTION INTERFACE
  55  
  56  /*! \todo FIXME - this decl was originally much later in the file, moved
  57  here temporarily for debugging */
  58  static emcmot_status_t emcmotStatus;
  59  
  60  /*
  61    Implementation notes:
  62  
  63    Initing:  the emcmot interface needs to be inited once, but nml_traj_init()
  64    and nml_servo_init() can be called in any order. Similarly, the emcmot
  65    interface needs to be exited once, but nml_traj_exit() and nml_servo_exit()
  66    can be called in any order. They can also be called multiple times. Flags
  67    are used to signify if initing has been done, or if the final exit has
  68    been called.
  69    */
  70  
  71  static struct TrajConfig_t TrajConfig;
  72  static struct JointConfig_t JointConfig[EMCMOT_MAX_JOINTS];
  73  static struct AxisConfig_t AxisConfig[EMCMOT_MAX_AXIS];
  74  
  75  static emcmot_command_t emcmotCommand;
  76  
  77  __attribute__ ((unused))
  78  static int emcmotIoInited = 0;	// non-zero means io called init
  79  static int emcmotion_initialized = 0;	// non-zero means both
  80  						// emcMotionInit called.
  81  
  82  // local status data, not provided by emcmot
  83  static unsigned long localMotionHeartbeat = 0;
  84  static int localMotionCommandType = 0;
  85  static int localMotionEchoSerialNumber = 0;
  86  
  87  //FIXME-AJ: see if needed
  88  //static double localEmcAxisUnits[EMCMOT_MAX_AXIS];
  89  
  90  // axes and joints are numbered 0..NUM-1
  91  
  92  /*
  93    In emcmot, we need to set the cycle time for traj, and the interpolation
  94    rate, in any order, but both need to be done. 
  95   */
  96  
  97  /*! functions involving joints */
  98  
  99  int emcJointSetType(int joint, unsigned char jointType)
 100  {
 101      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 102  	return 0;
 103      }
 104  
 105      JointConfig[joint].Type = jointType;
 106  
 107      if (emc_debug & EMC_DEBUG_CONFIG) {
 108          rcs_print("%s(%d, %d)\n", __FUNCTION__, joint, jointType);
 109      }
 110      return 0;
 111  }
 112  
 113  int emcJointSetUnits(int joint, double units)
 114  {
 115      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 116  	return 0;
 117      }
 118  
 119      JointConfig[joint].Units = units;
 120  
 121      if (emc_debug & EMC_DEBUG_CONFIG) {
 122          rcs_print("%s(%d, %.4f)\n", __FUNCTION__, joint, units);
 123      }
 124      return 0;
 125  }
 126  
 127  int emcJointSetBacklash(int joint, double backlash)
 128  {
 129  #ifdef ISNAN_TRAP
 130      if (std::isnan(backlash)) {
 131  	printf("std::isnan error in emcJointSetBacklash()\n");
 132  	return -1;
 133      }
 134  #endif
 135  
 136      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 137  	return 0;
 138      }
 139  
 140      emcmotCommand.command = EMCMOT_SET_JOINT_BACKLASH;
 141      emcmotCommand.joint = joint;
 142      emcmotCommand.backlash = backlash;
 143  
 144      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 145  
 146      if (emc_debug & EMC_DEBUG_CONFIG) {
 147          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, joint, backlash, retval);
 148      }
 149      return retval;
 150  }
 151  
 152  int emcJointSetMinPositionLimit(int joint, double limit)
 153  {
 154  #ifdef ISNAN_TRAP
 155      if (std::isnan(limit)) {
 156  	printf("isnan error in emcJointSetMinPosition()\n");
 157  	return -1;
 158      }
 159  #endif
 160  
 161      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 162  	return 0;
 163      }
 164  
 165      JointConfig[joint].MinLimit = limit;
 166  
 167      emcmotCommand.command = EMCMOT_SET_JOINT_POSITION_LIMITS;
 168      emcmotCommand.joint = joint;
 169      emcmotCommand.minLimit = JointConfig[joint].MinLimit;
 170      emcmotCommand.maxLimit = JointConfig[joint].MaxLimit;
 171  
 172      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 173  
 174      if (emc_debug & EMC_DEBUG_CONFIG) {
 175          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, joint, limit, retval);
 176      }
 177      return retval;
 178  }
 179  
 180  int emcJointSetMaxPositionLimit(int joint, double limit)
 181  {
 182  #ifdef ISNAN_TRAP
 183      if (std::isnan(limit)) {
 184  	printf("std::isnan error in emcJointSetMaxPosition()\n");
 185  	return -1;
 186      }
 187  #endif
 188  
 189      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 190  	return 0;
 191      }
 192  
 193      JointConfig[joint].MaxLimit = limit;
 194  
 195      emcmotCommand.command = EMCMOT_SET_JOINT_POSITION_LIMITS;
 196      emcmotCommand.joint = joint;
 197      emcmotCommand.minLimit = JointConfig[joint].MinLimit;
 198      emcmotCommand.maxLimit = JointConfig[joint].MaxLimit;
 199  
 200      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 201  
 202      if (emc_debug & EMC_DEBUG_CONFIG) {
 203          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, joint, limit, retval);
 204      }
 205      return retval;
 206  }
 207  
 208  int emcJointSetMotorOffset(int joint, double offset) 
 209  {
 210  #ifdef ISNAN_TRAP
 211      if (std::isnan(offset)) {
 212  	printf("isnan error in emcJointSetMotorOffset()\n");
 213  	return -1;
 214      }
 215  #endif
 216  
 217      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 218  	return 0;
 219      }
 220      emcmotCommand.command = EMCMOT_SET_JOINT_MOTOR_OFFSET;
 221      emcmotCommand.joint = joint;
 222      emcmotCommand.motor_offset = offset;
 223      
 224      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 225  
 226      if (emc_debug & EMC_DEBUG_CONFIG) {
 227          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, joint, offset, retval);
 228      }
 229      return retval;
 230  }
 231  
 232  int emcJointSetFerror(int joint, double ferror)
 233  {
 234  #ifdef ISNAN_TRAP
 235      if (std::isnan(ferror)) {
 236  	printf("isnan error in emcJointSetFerror()\n");
 237  	return -1;
 238      }
 239  #endif
 240  
 241      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 242  	return 0;
 243      }
 244  
 245      emcmotCommand.command = EMCMOT_SET_JOINT_MAX_FERROR;
 246      emcmotCommand.joint = joint;
 247      emcmotCommand.maxFerror = ferror;
 248  
 249      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 250  
 251      if (emc_debug & EMC_DEBUG_CONFIG) {
 252          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, joint, ferror, retval);
 253      }
 254      return retval;
 255  }
 256  
 257  int emcJointSetMinFerror(int joint, double ferror)
 258  {
 259  #ifdef ISNAN_TRAP
 260      if (std::isnan(ferror)) {
 261  	printf("isnan error in emcJointSetMinFerror()\n");
 262  	return -1;
 263      }
 264  #endif
 265  
 266      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 267  	return 0;
 268      }
 269      emcmotCommand.command = EMCMOT_SET_JOINT_MIN_FERROR;
 270      emcmotCommand.joint = joint;
 271      emcmotCommand.minFerror = ferror;
 272  
 273      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 274  
 275      if (emc_debug & EMC_DEBUG_CONFIG) {
 276          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, joint, ferror, retval);
 277      }
 278      return retval;
 279  }
 280  
 281  int emcJointSetHomingParams(int joint, double home, double offset, double home_final_vel,
 282  			   double search_vel, double latch_vel,
 283  			   int use_index, int encoder_does_not_reset,
 284  			   int ignore_limits, int is_shared,
 285  			   int sequence,int volatile_home, int locking_indexer,int absolute_encoder)
 286  {
 287  #ifdef ISNAN_TRAP
 288      if (std::isnan(home) || std::isnan(offset) || std::isnan(home_final_vel) ||
 289  	std::isnan(search_vel) || std::isnan(latch_vel)) {
 290  	printf("isnan error in emcJointSetHomingParams()\n");
 291  	return -1;
 292      }
 293  #endif
 294  
 295      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 296  	return 0;
 297      }
 298  
 299      emcmotCommand.command = EMCMOT_SET_JOINT_HOMING_PARAMS;
 300      emcmotCommand.joint = joint;
 301      emcmotCommand.home = home;
 302      emcmotCommand.offset = offset;
 303      emcmotCommand.home_final_vel = home_final_vel;
 304      emcmotCommand.search_vel = search_vel;
 305      emcmotCommand.latch_vel = latch_vel;
 306      emcmotCommand.flags = 0;
 307      emcmotCommand.home_sequence = sequence;
 308      emcmotCommand.volatile_home = volatile_home;
 309      if (use_index) {
 310  	emcmotCommand.flags |= HOME_USE_INDEX;
 311      }
 312      if (encoder_does_not_reset) {
 313  	emcmotCommand.flags |= HOME_INDEX_NO_ENCODER_RESET;
 314      }
 315      if (ignore_limits) {
 316  	emcmotCommand.flags |= HOME_IGNORE_LIMITS;
 317      }
 318      if (is_shared) {
 319  	emcmotCommand.flags |= HOME_IS_SHARED;
 320      }
 321      if (locking_indexer) {
 322          emcmotCommand.flags |= HOME_UNLOCK_FIRST;
 323      }
 324      if (absolute_encoder) {
 325          switch (absolute_encoder) {
 326            case 0: break;
 327            case 1: emcmotCommand.flags |= HOME_ABSOLUTE_ENCODER;
 328                    emcmotCommand.flags |= HOME_NO_REHOME;
 329                    break;
 330            case 2: emcmotCommand.flags |= HOME_ABSOLUTE_ENCODER;
 331                    emcmotCommand.flags |= HOME_NO_REHOME;
 332                    emcmotCommand.flags |= HOME_NO_FINAL_MOVE;
 333                    break;
 334            default: fprintf(stderr,
 335                     "Unknown option for absolute_encoder <%d>",absolute_encoder);
 336                    break;
 337          }
 338      }
 339  
 340      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 341  
 342      if (emc_debug & EMC_DEBUG_CONFIG) {
 343          rcs_print("%s(%d, %.4f, %.4f, %.4f, %.4f, %.4f, %d, %d, %d, %d, %d) returned %d\n",
 344            __FUNCTION__, joint, home, offset, home_final_vel, search_vel, latch_vel,
 345            use_index, ignore_limits, is_shared, sequence, volatile_home, retval);
 346      }
 347      return retval;
 348  }
 349  
 350  int emcJointUpdateHomingParams(int joint, double home, double offset, int sequence)
 351  {
 352      CATCH_NAN(std::isnan(home) || std::isnan(offset) );
 353  
 354      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 355  	return 0;
 356      }
 357  
 358      emcmotCommand.command = EMCMOT_UPDATE_JOINT_HOMING_PARAMS;
 359      emcmotCommand.joint = joint;
 360      emcmotCommand.home = home;
 361      emcmotCommand.offset = offset;
 362      emcmotCommand.home_sequence = sequence;
 363  
 364      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 365  
 366      if (emc_debug & EMC_DEBUG_CONFIG) {
 367          rcs_print("%s(%d, %.4f, %.4f) returned %d\n",
 368            __FUNCTION__, joint, home, offset,retval);
 369      }
 370      return retval;
 371  }
 372  
 373  int emcJointSetMaxVelocity(int joint, double vel)
 374  {
 375      CATCH_NAN(std::isnan(vel));
 376  
 377      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 378  	return 0;
 379      }
 380  
 381      if (vel < 0.0) {
 382  	vel = 0.0;
 383      }
 384  
 385      JointConfig[joint].MaxVel = vel;
 386  
 387      emcmotCommand.command = EMCMOT_SET_JOINT_VEL_LIMIT;
 388      emcmotCommand.joint = joint;
 389      emcmotCommand.vel = vel;
 390      
 391      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 392  
 393      if (emc_debug & EMC_DEBUG_CONFIG) {
 394          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, joint, vel, retval);
 395      }
 396      return retval;
 397  }
 398  
 399  int emcJointSetMaxAcceleration(int joint, double acc)
 400  {
 401      CATCH_NAN(std::isnan(acc));
 402  
 403      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 404  	return 0;
 405      }
 406      if (acc < 0.0) {
 407  	acc = 0.0;
 408      }
 409      JointConfig[joint].MaxAccel = acc;
 410      //FIXME-AJ: need functions for setting the AXIS_MAX_ACCEL (either from the ini, or from kins..)
 411      emcmotCommand.command = EMCMOT_SET_JOINT_ACC_LIMIT;
 412      emcmotCommand.joint = joint;
 413      emcmotCommand.acc = acc;
 414      
 415      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 416  
 417      if (emc_debug & EMC_DEBUG_CONFIG) {
 418          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, joint, acc, retval);
 419      }
 420      return retval;
 421  }
 422  
 423  /*! functions involving carthesian Axes (X,Y,Z,A,B,C,U,V,W) */
 424      
 425  int emcAxisSetMinPositionLimit(int axis, double limit)
 426  {
 427      CATCH_NAN(std::isnan(limit));
 428  
 429      if (axis < 0 || axis >= EMCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis))) {
 430  	return 0;
 431      }
 432  
 433      AxisConfig[axis].MinLimit = limit;
 434  
 435      emcmotCommand.command = EMCMOT_SET_AXIS_POSITION_LIMITS;
 436      emcmotCommand.axis = axis;
 437      emcmotCommand.minLimit = AxisConfig[axis].MinLimit;
 438      emcmotCommand.maxLimit = AxisConfig[axis].MaxLimit;
 439  
 440      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 441  
 442      if (emc_debug & EMC_DEBUG_CONFIG) {
 443          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, axis, limit, retval);
 444      }
 445      return retval;
 446  }
 447  
 448  int emcAxisSetMaxPositionLimit(int axis, double limit)
 449  {
 450      CATCH_NAN(std::isnan(limit));
 451  
 452      if (axis < 0 || axis >= EMCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis))) {
 453  	return 0;
 454      }
 455  
 456      AxisConfig[axis].MaxLimit = limit;
 457  
 458      emcmotCommand.command = EMCMOT_SET_AXIS_POSITION_LIMITS;
 459      emcmotCommand.axis = axis;
 460      emcmotCommand.minLimit = AxisConfig[axis].MinLimit;
 461      emcmotCommand.maxLimit = AxisConfig[axis].MaxLimit;
 462  
 463      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 464  
 465      if (emc_debug & EMC_DEBUG_CONFIG) {
 466          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, axis, limit, retval);
 467      }
 468      return retval;
 469  }
 470  
 471  int emcAxisSetMaxVelocity(int axis, double vel,double ext_offset_vel)
 472  {
 473      CATCH_NAN(std::isnan(vel));
 474  
 475      if (axis < 0 || axis >= EMCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis))) {
 476  	return 0;
 477      }
 478  
 479      if (vel < 0.0) {
 480  	vel = 0.0;
 481      }
 482  
 483      AxisConfig[axis].MaxVel = vel;
 484  
 485      emcmotCommand.command = EMCMOT_SET_AXIS_VEL_LIMIT;
 486      emcmotCommand.axis = axis;
 487      emcmotCommand.vel = vel;
 488      emcmotCommand.ext_offset_vel = ext_offset_vel;
 489      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 490  
 491      if (emc_debug & EMC_DEBUG_CONFIG) {
 492          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, axis, vel, retval);
 493      }
 494      return retval;
 495  }
 496  
 497  int emcAxisSetMaxAcceleration(int axis, double acc,double ext_offset_acc)
 498  {
 499      CATCH_NAN(std::isnan(acc));
 500  
 501      if (axis < 0 || axis >= EMCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis))) {
 502  	return 0;
 503      }
 504  
 505      if (acc < 0.0) {
 506  	acc = 0.0;
 507      }
 508      
 509      AxisConfig[axis].MaxAccel = acc;    
 510  
 511      emcmotCommand.command = EMCMOT_SET_AXIS_ACC_LIMIT;
 512      emcmotCommand.axis = axis;
 513      emcmotCommand.acc = acc;
 514      emcmotCommand.ext_offset_acc = ext_offset_acc;
 515      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 516  
 517      if (emc_debug & EMC_DEBUG_CONFIG) {
 518          rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, axis, acc, retval);
 519      }
 520      return retval;
 521  }
 522  
 523  int emcAxisSetLockingJoint(int axis, int joint)
 524  {
 525  
 526      if (axis < 0 || axis >= EMCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis))) {
 527  	return 0;
 528      }
 529  
 530      if (joint < 0) {
 531  	joint = -1;
 532      }
 533  
 534      emcmotCommand.command = EMCMOT_SET_AXIS_LOCKING_JOINT;
 535      emcmotCommand.axis    = axis;
 536      emcmotCommand.joint   = joint;
 537      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 538  
 539      if (emc_debug & EMC_DEBUG_CONFIG) {
 540          rcs_print("%s(%d, %d) returned %d\n", __FUNCTION__, axis, joint, retval);
 541      }
 542      return retval;
 543  }
 544  
 545  double emcAxisGetMaxVelocity(int axis)
 546  {
 547      if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
 548          return 0;
 549      }
 550  
 551      return AxisConfig[axis].MaxVel;
 552  }
 553  
 554  double emcAxisGetMaxAcceleration(int axis)
 555  {
 556      if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
 557          return 0;
 558      }
 559  
 560      return AxisConfig[axis].MaxAccel;
 561  }
 562  
 563  int emcAxisUpdate(EMC_AXIS_STAT stat[], int axis_mask)
 564  {
 565      int axis_num;
 566      emcmot_axis_status_t *axis;
 567      
 568      for (axis_num = 0; axis_num < EMCMOT_MAX_AXIS; axis_num++) {
 569          if(!(axis_mask & (1 << axis_num))) continue;
 570          axis = &(emcmotStatus.axis_status[axis_num]);
 571  
 572          stat[axis_num].velocity = axis->teleop_vel_cmd;
 573          stat[axis_num].minPositionLimit = axis->min_pos_limit;
 574          stat[axis_num].maxPositionLimit = axis->max_pos_limit;
 575      }
 576      return 0;
 577  }
 578  
 579  /* This function checks to see if any joint or the traj has
 580     been inited already.  At startup, if none have been inited,
 581     usrmotIniLoad and usrmotInit must be called first.  At
 582     shutdown, after all have been halted, the usrmotExit must
 583     be called.
 584  */
 585  
 586  static int JointOrTrajInited(void)
 587  {
 588      int joint;
 589  
 590      for (joint = 0; joint < EMCMOT_MAX_JOINTS; joint++) {
 591  	if (JointConfig[joint].Inited) {
 592  	    return 1;
 593  	}
 594      }
 595      if (TrajConfig.Inited) {
 596  	return 1;
 597      }
 598      return 0;
 599  }
 600  
 601  int emcJointInit(int joint)
 602  {
 603      int retval = 0;
 604      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 605  	return 0;
 606      }
 607      // init emcmot interface
 608      if (!JointOrTrajInited()) {
 609  	usrmotIniLoad(emc_inifile);
 610  	if (0 != usrmotInit("emc2_task")) {
 611  	    return -1;
 612  	}
 613      }
 614      JointConfig[joint].Inited = 1;
 615      if (0 != iniJoint(joint, emc_inifile)) {
 616  	retval = -1;
 617      }
 618      return retval;
 619  }
 620  
 621  int emcAxisInit(int axis)
 622  {
 623      int retval = 0;
 624  
 625      if (axis < 0 || axis >= EMCMOT_MAX_AXIS) {
 626  	return 0;
 627      }
 628      // init emcmot interface
 629      if (!JointOrTrajInited()) {
 630  	usrmotIniLoad(emc_inifile);
 631  	if (0 != usrmotInit("emc2_task")) {
 632  	    return -1;
 633  	}
 634      }
 635      AxisConfig[axis].Inited = 1;
 636      if (0 != iniAxis(axis, emc_inifile)) {
 637  	retval = -1;
 638      }
 639      return retval;
 640  }
 641  
 642  int emcJointHalt(int joint)
 643  {
 644      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 645  	return 0;
 646      }
 647      /*! \todo FIXME-- refs global emcStatus; should make EMC_JOINT_STAT an arg here */
 648      if (NULL != emcStatus && emcmotion_initialized
 649  	&& JointConfig[joint].Inited) {
 650  	//dumpJoint(joint, emc_inifile, &emcStatus->motion.joint[joint]);
 651      }
 652      JointConfig[joint].Inited = 0;
 653  
 654      if (!JointOrTrajInited()) {
 655  	usrmotExit();		// ours is final exit
 656      }
 657  
 658      return 0;
 659  }
 660  
 661  int emcJointAbort(int joint)
 662  {
 663      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 664  	return 0;
 665      }
 666      emcmotCommand.command = EMCMOT_JOINT_ABORT;
 667      emcmotCommand.joint = joint;
 668  
 669      return usrmotWriteEmcmotCommand(&emcmotCommand);
 670  }
 671  
 672  int emcJointActivate(int joint)
 673  {
 674      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 675  	return 0;
 676      }
 677  
 678      emcmotCommand.command = EMCMOT_JOINT_ACTIVATE;
 679      emcmotCommand.joint = joint;
 680  
 681      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 682  
 683      if (emc_debug & EMC_DEBUG_CONFIG) {
 684          rcs_print("%s(%d) returned %d\n", __FUNCTION__, joint, retval);
 685      }
 686      return retval;
 687  }
 688  
 689  int emcJointDeactivate(int joint)
 690  {
 691      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 692  	return 0;
 693      }
 694  
 695      emcmotCommand.command = EMCMOT_JOINT_DEACTIVATE;
 696      emcmotCommand.joint = joint;
 697  
 698      return usrmotWriteEmcmotCommand(&emcmotCommand);
 699  }
 700  
 701  int emcJointOverrideLimits(int joint)
 702  {
 703      // can have joint < 0, for resuming normal limit checking
 704      if (joint >= EMCMOT_MAX_JOINTS) {
 705  	return 0;
 706      }
 707  
 708      emcmotCommand.command = EMCMOT_OVERRIDE_LIMITS;
 709      emcmotCommand.joint = joint;
 710  
 711      return usrmotWriteEmcmotCommand(&emcmotCommand);
 712  }
 713  
 714  int emcJointEnable(int joint)
 715  {
 716      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 717  	return 0;
 718      }
 719  
 720      emcmotCommand.command = EMCMOT_JOINT_ENABLE_AMPLIFIER;
 721      emcmotCommand.joint = joint;
 722  
 723      return usrmotWriteEmcmotCommand(&emcmotCommand);
 724  }
 725  
 726  int emcJointDisable(int joint)
 727  {
 728      if (joint < 0 || joint >= EMCMOT_MAX_JOINTS) {
 729  	return 0;
 730      }
 731  
 732      emcmotCommand.command = EMCMOT_JOINT_DISABLE_AMPLIFIER;
 733      emcmotCommand.joint = joint;
 734  
 735      return usrmotWriteEmcmotCommand(&emcmotCommand);
 736  }
 737  
 738  int emcJointHome(int joint)
 739  {
 740      if (joint < -1 || joint >= EMCMOT_MAX_JOINTS) {
 741  	return 0;
 742      }
 743  
 744      emcmotCommand.command = EMCMOT_JOINT_HOME;
 745      emcmotCommand.joint = joint;
 746  
 747      return usrmotWriteEmcmotCommand(&emcmotCommand);
 748  }
 749  
 750  int emcJointUnhome(int joint)
 751  {
 752  	if (joint < -2 || joint >= EMCMOT_MAX_JOINTS) {
 753  		return 0;
 754  	}
 755  
 756  	emcmotCommand.command = EMCMOT_JOINT_UNHOME;
 757  	emcmotCommand.joint = joint;
 758  
 759  	return usrmotWriteEmcmotCommand(&emcmotCommand);
 760  }
 761  
 762  int emcJogCont(int nr, double vel, int jjogmode)
 763  {
 764      if (jjogmode) {
 765          if (nr < 0 || nr >= EMCMOT_MAX_JOINTS) { return 0; }
 766          if (vel > JointConfig[nr].MaxVel) {
 767              vel = JointConfig[nr].MaxVel;
 768          } else if (vel < -JointConfig[nr].MaxVel) {
 769              vel = -JointConfig[nr].MaxVel;
 770          }
 771          emcmotCommand.joint = nr;
 772          emcmotCommand.axis = -1;  //NA
 773      } else {
 774          if (nr < 0 || nr >= EMCMOT_MAX_AXIS) { return 0; }
 775          if (vel > AxisConfig[nr].MaxVel) {
 776              vel = AxisConfig[nr].MaxVel;
 777          } else if (vel < -AxisConfig[nr].MaxVel) {
 778              vel = -AxisConfig[nr].MaxVel;
 779          }
 780          emcmotCommand.joint = -1; //NA
 781          emcmotCommand.axis = nr;
 782      }
 783      emcmotCommand.command = EMCMOT_JOG_CONT;
 784      emcmotCommand.vel = vel;
 785  
 786      return usrmotWriteEmcmotCommand(&emcmotCommand);
 787  }
 788  
 789  int emcJogIncr(int nr, double incr, double vel, int jjogmode)
 790  {
 791      if (jjogmode) {
 792          if (nr < 0 || nr >= EMCMOT_MAX_JOINTS) { return 0; }
 793          if (vel > JointConfig[nr].MaxVel) {
 794              vel = JointConfig[nr].MaxVel;
 795          } else if (vel < -JointConfig[nr].MaxVel) {
 796              vel = -JointConfig[nr].MaxVel;
 797          }
 798          emcmotCommand.joint = nr;
 799          emcmotCommand.axis = -1; //NA
 800      } else {
 801          if (nr < 0 || nr >= EMCMOT_MAX_AXIS) { return 0; }
 802          if (vel > AxisConfig[nr].MaxVel) {
 803              vel = AxisConfig[nr].MaxVel;
 804          } else if (vel < -AxisConfig[nr].MaxVel) {
 805              vel = -AxisConfig[nr].MaxVel;
 806          }
 807          emcmotCommand.joint = -1; //NA
 808          emcmotCommand.axis = nr;
 809      }
 810      emcmotCommand.command = EMCMOT_JOG_INCR;
 811      emcmotCommand.vel = vel;
 812      emcmotCommand.offset = incr;
 813  
 814      return usrmotWriteEmcmotCommand(&emcmotCommand);
 815  }
 816  
 817  int emcJogAbs(int nr, double pos, double vel, int jjogmode)
 818  {
 819      if (jjogmode) {        
 820          if (nr < 0 || nr >= EMCMOT_MAX_JOINTS) { return 0; }
 821          if (vel > JointConfig[nr].MaxVel) {
 822              vel = JointConfig[nr].MaxVel;
 823          } else if (vel < -JointConfig[nr].MaxVel) {
 824              vel = -JointConfig[nr].MaxVel;
 825          }
 826          emcmotCommand.joint = nr;
 827          emcmotCommand.axis = -1; //NA
 828      } else {
 829          if (nr < 0 || nr >= EMCMOT_MAX_AXIS) { return 0; }
 830          if (vel > AxisConfig[nr].MaxVel) {
 831              vel = AxisConfig[nr].MaxVel;
 832          } else if (vel < -AxisConfig[nr].MaxVel) {
 833              vel = -AxisConfig[nr].MaxVel;
 834          }
 835          emcmotCommand.joint = -1; //NA
 836          emcmotCommand.axis = nr;
 837      }
 838      emcmotCommand.command = EMCMOT_JOG_ABS;
 839      emcmotCommand.vel = vel;
 840      emcmotCommand.offset = pos;
 841  
 842      return usrmotWriteEmcmotCommand(&emcmotCommand);
 843  }
 844  
 845  int emcJogStop(int nr, int jjogmode)
 846  {
 847      if (jjogmode) {
 848          if (nr < 0 || nr >= EMCMOT_MAX_JOINTS) { return 0; }
 849          emcmotCommand.joint = nr;
 850          emcmotCommand.axis = -1; //NA
 851      } else {
 852          if (nr < 0 || nr >= EMCMOT_MAX_AXIS) { return 0; }
 853          emcmotCommand.joint = -1; //NA
 854          emcmotCommand.axis = nr;
 855      }
 856      emcmotCommand.command = EMCMOT_JOINT_ABORT;
 857      return usrmotWriteEmcmotCommand(&emcmotCommand);
 858  }
 859  
 860  
 861  int emcJointLoadComp(int joint, const char *file, int type)
 862  {
 863      return usrmotLoadComp(joint, file, type);
 864  }
 865  
 866  static emcmot_config_t emcmotConfig;
 867  int get_emcmot_debug_info = 0;
 868  
 869  /*
 870    these globals are set in emcMotionUpdate(), then referenced in
 871    emcJointUpdate(), emcTrajUpdate() to save calls to usrmotReadEmcmotStatus
 872   */
 873  static emcmot_debug_t emcmotDebug;
 874  static char errorString[EMCMOT_ERROR_LEN];
 875  static int new_config = 0;
 876  
 877  /*! \todo FIXME - debugging - uncomment the following line to log changes in
 878     JOINT_FLAG */
 879  // #define WATCH_FLAGS 1
 880  
 881  int emcJointUpdate(EMC_JOINT_STAT stat[], int numJoints)
 882  {
 883  /*! \todo FIXME - this function accesses data that has been
 884     moved.  Once I know what it is used for I'll fix it */
 885  
 886      int joint_num;
 887      emcmot_joint_status_t *joint;
 888  #ifdef WATCH_FLAGS
 889      static int old_joint_flag[8];
 890  #endif
 891  
 892      // check for valid range
 893      if (numJoints <= 0 || numJoints > EMCMOT_MAX_JOINTS) {
 894  	return -1;
 895      }
 896  
 897      for (joint_num = 0; joint_num < numJoints; joint_num++) {
 898  	/* point to joint data */
 899  
 900  	joint = &(emcmotStatus.joint_status[joint_num]);
 901  
 902  	stat[joint_num].jointType = JointConfig[joint_num].Type;
 903  	stat[joint_num].units = JointConfig[joint_num].Units;
 904  	if (new_config) {
 905  	    stat[joint_num].backlash = joint->backlash;
 906  	    stat[joint_num].minPositionLimit = joint->min_pos_limit;
 907  	    stat[joint_num].maxPositionLimit = joint->max_pos_limit;
 908  	    stat[joint_num].minFerror = joint->min_ferror;
 909  	    stat[joint_num].maxFerror = joint->max_ferror;
 910  /*! \todo FIXME - should all homing config params be included here? */
 911  //	    stat[joint_num].homeOffset = joint->home_offset;
 912  	}
 913  	stat[joint_num].output = joint->pos_cmd;
 914  	stat[joint_num].input = joint->pos_fb;
 915  	stat[joint_num].velocity = joint->vel_cmd;
 916  	stat[joint_num].ferrorCurrent = joint->ferror;
 917  	stat[joint_num].ferrorHighMark = joint->ferror_high_mark;
 918  
 919  	stat[joint_num].homing = joint->homing;
 920  	stat[joint_num].homed  = joint->homed;
 921  
 922  	stat[joint_num].fault = (joint->flag & EMCMOT_JOINT_FAULT_BIT ? 1 : 0);
 923  	stat[joint_num].enabled = (joint->flag & EMCMOT_JOINT_ENABLE_BIT ? 1 : 0);
 924  	stat[joint_num].inpos = (joint->flag & EMCMOT_JOINT_INPOS_BIT ? 1 : 0);
 925  
 926  /* FIXME - soft limits are now applied to the command, and should never
 927     happen */
 928  	stat[joint_num].minSoftLimit = 0;
 929  	stat[joint_num].maxSoftLimit = 0;
 930  	stat[joint_num].minHardLimit =
 931  	    (joint->flag & EMCMOT_JOINT_MIN_HARD_LIMIT_BIT ? 1 : 0);
 932  	stat[joint_num].maxHardLimit =
 933  	    (joint->flag & EMCMOT_JOINT_MAX_HARD_LIMIT_BIT ? 1 : 0);
 934  	stat[joint_num].overrideLimits = !!(emcmotStatus.overrideLimitMask);	// one
 935  	// for
 936  	// all
 937  
 938  #ifdef WATCH_FLAGS
 939  	if (old_joint_flag[joint_num] != joint->flag) {
 940  	    printf("joint %d flag: %04X -> %04X\n", joint_num,
 941  		   old_joint_flag[joint_num], joint->flag);
 942  	    old_joint_flag[joint_num] = joint->flag;
 943  	}
 944  #endif
 945  	if (joint->flag & EMCMOT_JOINT_ERROR_BIT) {
 946  	    if (stat[joint_num].status != RCS_ERROR) {
 947  		rcs_print_error("Error on joint %d, command number %d\n",
 948  				joint_num, emcmotStatus.commandNumEcho);
 949  		stat[joint_num].status = RCS_ERROR;
 950  	    }
 951  	} else if (joint->flag & EMCMOT_JOINT_INPOS_BIT) {
 952  	    stat[joint_num].status = RCS_DONE;
 953  	} else {
 954  	    stat[joint_num].status = RCS_EXEC;
 955  	}
 956      }
 957      return 0;
 958  }
 959  
 960  // EMC_TRAJ functions
 961  
 962  int emcTrajSetJoints(int joints)
 963  {
 964      if (joints <= 0 || joints > EMCMOT_MAX_JOINTS) {
 965  	rcs_print("emcTrajSetJoints failing: joints=%d\n",
 966  		joints);
 967  	return -1;
 968      }
 969  
 970      TrajConfig.Joints = joints;
 971      emcmotCommand.command = EMCMOT_SET_NUM_JOINTS;
 972      emcmotCommand.joint = joints;
 973      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
 974  
 975      if (emc_debug & EMC_DEBUG_CONFIG) {
 976          rcs_print("%s(%d) returned %d\n", __FUNCTION__, joints, retval);
 977      }
 978      return retval;
 979  }
 980  
 981  int emcTrajSetAxes(int axismask)
 982  {
 983      int axes = 0;
 984      for(int i=0; i<EMCMOT_MAX_AXIS; i++)
 985          if(axismask & (1<<i)) axes = i+1;
 986  
 987      TrajConfig.DeprecatedAxes = axes;
 988      TrajConfig.AxisMask = axismask;
 989      
 990      if (emc_debug & EMC_DEBUG_CONFIG) {
 991          rcs_print("%s(%d, %d)\n", __FUNCTION__, axes, axismask);
 992      }
 993      return 0;
 994  }
 995  
 996  int emcTrajSetSpindles(int spindles)
 997  {
 998      if (spindles <= 0 || spindles > EMCMOT_MAX_SPINDLES) {
 999  	rcs_print("emcTrajSetSpindles failing: spindles=%d\n",
1000  		spindles);
1001  	return -1;
1002      }
1003  
1004      TrajConfig.Spindles = spindles;
1005      emcmotCommand.command = EMCMOT_SET_NUM_SPINDLES;
1006      emcmotCommand.spindle = spindles;
1007      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
1008  
1009      if (emc_debug & EMC_DEBUG_CONFIG) {
1010          rcs_print("%s(%d) returned %d\n", __FUNCTION__, spindles, retval);
1011      }
1012      return retval;
1013  }
1014  
1015  int emcTrajSetUnits(double linearUnits, double angularUnits)
1016  {
1017      if (linearUnits <= 0.0 || angularUnits <= 0.0) {
1018  	return -1;
1019      }
1020  
1021      TrajConfig.LinearUnits = linearUnits;
1022      TrajConfig.AngularUnits = angularUnits;
1023  
1024      if (emc_debug & EMC_DEBUG_CONFIG) {
1025          rcs_print("%s(%.4f, %.4f)\n", __FUNCTION__, linearUnits, angularUnits);
1026      }
1027      return 0;
1028  }
1029  
1030  int emcTrajSetMode(int mode)
1031  {
1032      switch (mode) {
1033      case EMC_TRAJ_MODE_FREE:
1034  	emcmotCommand.command = EMCMOT_FREE;
1035  	return usrmotWriteEmcmotCommand(&emcmotCommand);
1036  
1037      case EMC_TRAJ_MODE_COORD:
1038  	emcmotCommand.command = EMCMOT_COORD;
1039  	return usrmotWriteEmcmotCommand(&emcmotCommand);
1040  
1041      case EMC_TRAJ_MODE_TELEOP:
1042  	emcmotCommand.command = EMCMOT_TELEOP;
1043  	return usrmotWriteEmcmotCommand(&emcmotCommand);
1044  
1045      default:
1046  	return -1;
1047      }
1048  }
1049  
1050  int emcTrajSetVelocity(double vel, double ini_maxvel)
1051  {
1052      if (vel < 0.0) {
1053  	vel = 0.0;
1054      } else if (vel > TrajConfig.MaxVel) {
1055  	vel = TrajConfig.MaxVel;
1056      }
1057  
1058      if (ini_maxvel < 0.0) {
1059  	    ini_maxvel = 0.0;
1060      } else if (vel > TrajConfig.MaxVel) {
1061  	    ini_maxvel = TrajConfig.MaxVel;
1062      }
1063  
1064      emcmotCommand.command = EMCMOT_SET_VEL;
1065      emcmotCommand.vel = vel;
1066      emcmotCommand.ini_maxvel = ini_maxvel;
1067  
1068      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
1069  
1070      if (emc_debug & EMC_DEBUG_CONFIG) {
1071          rcs_print("%s(%.4f, %.4f) returned %d\n", __FUNCTION__, vel, ini_maxvel, retval);
1072      }
1073      return retval;
1074  }
1075  
1076  int emcTrajSetAcceleration(double acc)
1077  {
1078      if (acc < 0.0) {
1079  	acc = 0.0;
1080      } else if (acc > TrajConfig.MaxAccel) {
1081  	acc = TrajConfig.MaxAccel;
1082      }
1083  
1084      emcmotCommand.command = EMCMOT_SET_ACC;
1085      emcmotCommand.acc = acc;
1086  
1087      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
1088  
1089      if (emc_debug & EMC_DEBUG_CONFIG) {
1090          rcs_print("%s(%.4f) returned %d\n", __FUNCTION__, acc, retval);
1091      }
1092      return retval;
1093  }
1094  
1095  /*
1096    emcmot has no limits on max velocity, acceleration so we'll save them
1097    here and apply them in the functions above
1098    */
1099  int emcTrajSetMaxVelocity(double vel)
1100  {
1101      if (vel < 0.0) {
1102  	vel = 0.0;
1103      }
1104  
1105      TrajConfig.MaxVel = vel;
1106  
1107      emcmotCommand.command = EMCMOT_SET_VEL_LIMIT;
1108      emcmotCommand.vel = vel;
1109  
1110      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
1111  
1112      if (emc_debug & EMC_DEBUG_CONFIG) {
1113          rcs_print("%s(%.4f) returned %d\n", __FUNCTION__, vel, retval);
1114      }
1115      return retval;
1116  }
1117  
1118  int emcTrajSetMaxAcceleration(double acc)
1119  {
1120      if (acc < 0.0) {
1121  	acc = 0.0;
1122      }
1123  
1124      TrajConfig.MaxAccel = acc;
1125  
1126      if (emc_debug & EMC_DEBUG_CONFIG) {
1127          rcs_print("%s(%.4f)\n", __FUNCTION__, acc);
1128      }
1129      return 0;
1130  }
1131  
1132  int emcTrajSetHome(EmcPose home)
1133  {
1134  #ifdef ISNAN_TRAP
1135      if (std::isnan(home.tran.x) || std::isnan(home.tran.y) || std::isnan(home.tran.z) ||
1136  	std::isnan(home.a) || std::isnan(home.b) || std::isnan(home.c) ||
1137  	std::isnan(home.u) || std::isnan(home.v) || std::isnan(home.w)) {
1138  	printf("std::isnan error in emcTrajSetHome()\n");
1139  	return 0;		// ignore it for now, just don't send it
1140      }
1141  #endif
1142  
1143      emcmotCommand.command = EMCMOT_SET_WORLD_HOME;
1144      emcmotCommand.pos = home;
1145  
1146      int retval = usrmotWriteEmcmotCommand(&emcmotCommand);
1147  
1148      if (emc_debug & EMC_DEBUG_CONFIG) {
1149          rcs_print("%s(%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f) returned %d\n", 
1150            __FUNCTION__, home.tran.x, home.tran.y, home.tran.z, home.a, home.b, home.c, 
1151            home.u, home.v, home.w, retval);
1152      }
1153      return retval;
1154  }
1155  
1156  int emcTrajSetScale(double scale)
1157  {
1158      if (scale < 0.0) {
1159  	scale = 0.0;
1160      }
1161  
1162      emcmotCommand.command = EMCMOT_FEED_SCALE;
1163      emcmotCommand.scale = scale;
1164  
1165      return usrmotWriteEmcmotCommand(&emcmotCommand);
1166  }
1167  
1168  int emcTrajSetRapidScale(double scale)
1169  {
1170      if (scale < 0.0) {
1171  	scale = 0.0;
1172      }
1173  
1174      emcmotCommand.command = EMCMOT_RAPID_SCALE;
1175      emcmotCommand.scale = scale;
1176  
1177      return usrmotWriteEmcmotCommand(&emcmotCommand);
1178  }
1179  
1180  int emcTrajSetSpindleScale(int spindle, double scale)
1181  {
1182      if (scale < 0.0) {
1183  	scale = 0.0;
1184      }
1185  
1186      emcmotCommand.command = EMCMOT_SPINDLE_SCALE;
1187      emcmotCommand.scale = scale;
1188      emcmotCommand.spindle = spindle;
1189  
1190      return usrmotWriteEmcmotCommand(&emcmotCommand);
1191  }
1192  
1193  int emcTrajSetFOEnable(unsigned char mode)
1194  {
1195      emcmotCommand.command = EMCMOT_FS_ENABLE;
1196      emcmotCommand.mode = mode;
1197  
1198      return usrmotWriteEmcmotCommand(&emcmotCommand);
1199  }
1200  
1201  int emcTrajSetFHEnable(unsigned char mode)
1202  {
1203      emcmotCommand.command = EMCMOT_FH_ENABLE;
1204      emcmotCommand.mode = mode;
1205  
1206      return usrmotWriteEmcmotCommand(&emcmotCommand);
1207  }
1208  
1209  int emcTrajSetSOEnable(unsigned char mode)
1210  {
1211      emcmotCommand.command = EMCMOT_SS_ENABLE;
1212      emcmotCommand.mode = mode;
1213  
1214      return usrmotWriteEmcmotCommand(&emcmotCommand);
1215  }
1216  
1217  int emcTrajSetAFEnable(unsigned char enable)
1218  {
1219      emcmotCommand.command = EMCMOT_AF_ENABLE;
1220  
1221      if ( enable ) {
1222  	emcmotCommand.flags = 1;
1223      } else {
1224  	emcmotCommand.flags = 0;
1225      }
1226      return usrmotWriteEmcmotCommand(&emcmotCommand);
1227  }
1228  
1229  int emcTrajSetMotionId(int id)
1230  {
1231  
1232      if (EMC_DEBUG_MOTION_TIME & emc_debug) {
1233  	if (id != TrajConfig.MotionId) {
1234  	    rcs_print("Outgoing motion id is %d.\n", id);
1235  	}
1236      }
1237  
1238      TrajConfig.MotionId = id;
1239  
1240      return 0;
1241  }
1242  
1243  int emcTrajInit()
1244  {
1245      int retval = 0;
1246  
1247      TrajConfig.Inited = 0;
1248      TrajConfig.Joints = 0;
1249      TrajConfig.MaxAccel = DBL_MAX;
1250      TrajConfig.DeprecatedAxes = 0;
1251      TrajConfig.AxisMask = 0;
1252      TrajConfig.LinearUnits = 1.0;
1253      TrajConfig.AngularUnits = 1.0;
1254      TrajConfig.MotionId = 0;
1255      TrajConfig.MaxVel = DEFAULT_TRAJ_MAX_VELOCITY;
1256  
1257      // init emcmot interface
1258      if (!JointOrTrajInited()) {
1259  	usrmotIniLoad(emc_inifile);
1260  	if (0 != usrmotInit("emc2_task")) {
1261  	    return -1;
1262  	}
1263      }
1264      TrajConfig.Inited = 1;
1265      // initialize parameters from ini file
1266      if (0 != iniTraj(emc_inifile)) {
1267  	retval = -1;
1268      }
1269      return retval;
1270  }
1271  
1272  int emcTrajHalt()
1273  {
1274      TrajConfig.Inited = 0;
1275  
1276      if (!JointOrTrajInited()) {
1277  	usrmotExit();		// ours is final exit
1278      }
1279  
1280      return 0;
1281  }
1282  
1283  int emcTrajEnable()
1284  {
1285      emcmotCommand.command = EMCMOT_ENABLE;
1286  
1287      return usrmotWriteEmcmotCommand(&emcmotCommand);
1288  }
1289  
1290  int emcTrajDisable()
1291  {
1292      emcmotCommand.command = EMCMOT_DISABLE;
1293  
1294      return usrmotWriteEmcmotCommand(&emcmotCommand);
1295  }
1296  
1297  int emcTrajAbort()
1298  {
1299      emcmotCommand.command = EMCMOT_ABORT;
1300  
1301      return usrmotWriteEmcmotCommand(&emcmotCommand);
1302  }
1303  
1304  int emcTrajPause()
1305  {
1306      emcmotCommand.command = EMCMOT_PAUSE;
1307  
1308      return usrmotWriteEmcmotCommand(&emcmotCommand);
1309  }
1310  
1311  int emcTrajReverse()
1312  {
1313      emcmotCommand.command = EMCMOT_REVERSE;
1314  
1315      return usrmotWriteEmcmotCommand(&emcmotCommand);
1316  }
1317  
1318  int emcTrajForward()
1319  {
1320      emcmotCommand.command = EMCMOT_FORWARD;
1321  
1322      return usrmotWriteEmcmotCommand(&emcmotCommand);
1323  }
1324  
1325  int emcTrajStep()
1326  {
1327      emcmotCommand.command = EMCMOT_STEP;
1328  
1329      return usrmotWriteEmcmotCommand(&emcmotCommand);
1330  }
1331  
1332  int emcTrajResume()
1333  {
1334      emcmotCommand.command = EMCMOT_RESUME;
1335  
1336      return usrmotWriteEmcmotCommand(&emcmotCommand);
1337  }
1338  
1339  int emcTrajDelay(double delay)
1340  {
1341      /* nothing need be done here - it's done in task controller */
1342  
1343      return 0;
1344  }
1345  
1346  double emcTrajGetLinearUnits()
1347  {
1348      return TrajConfig.LinearUnits;
1349  }
1350  
1351  double emcTrajGetAngularUnits()
1352  {
1353      return TrajConfig.AngularUnits;
1354  }
1355  
1356  int emcTrajSetOffset(EmcPose tool_offset)
1357  {
1358      emcmotCommand.command = EMCMOT_SET_OFFSET;
1359      emcmotCommand.tool_offset = tool_offset;
1360      return usrmotWriteEmcmotCommand(&emcmotCommand);
1361  }
1362  
1363  int emcTrajSetSpindleSync(int spindle, double fpr, bool wait_for_index)
1364  {
1365      emcmotCommand.command = EMCMOT_SET_SPINDLESYNC;
1366      emcmotCommand.spindle = spindle;
1367      emcmotCommand.spindlesync = fpr;
1368      emcmotCommand.flags = wait_for_index;
1369      return usrmotWriteEmcmotCommand(&emcmotCommand);
1370  }
1371  
1372  int emcTrajSetTermCond(int cond, double tolerance)
1373  {
1374      emcmotCommand.command = EMCMOT_SET_TERM_COND;
1375      // Direct passthrough since TP can handle the distinction now
1376      emcmotCommand.termCond = cond;
1377      emcmotCommand.tolerance = tolerance;
1378  
1379      return usrmotWriteEmcmotCommand(&emcmotCommand);
1380  }
1381  
1382  int emcTrajLinearMove(EmcPose end, int type, double vel, double ini_maxvel, double acc,
1383                        int indexer_jnum)
1384  {
1385  #ifdef ISNAN_TRAP
1386      if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) ||
1387          std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) ||
1388          std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w)) {
1389  	printf("std::isnan error in emcTrajLinearMove()\n");
1390  	return 0;		// ignore it for now, just don't send it
1391      }
1392  #endif
1393  
1394      emcmotCommand.command = EMCMOT_SET_LINE;
1395  
1396      emcmotCommand.pos = end;
1397  
1398      emcmotCommand.id = TrajConfig.MotionId;
1399      emcmotCommand.motion_type = type;
1400      emcmotCommand.vel = vel;
1401      emcmotCommand.ini_maxvel = ini_maxvel;
1402      emcmotCommand.acc = acc;
1403      emcmotCommand.turn = indexer_jnum;
1404  
1405      return usrmotWriteEmcmotCommand(&emcmotCommand);
1406  }
1407  
1408  int emcTrajCircularMove(EmcPose end, PM_CARTESIAN center,
1409  			PM_CARTESIAN normal, int turn, int type, double vel, double ini_maxvel, double acc)
1410  {
1411  #ifdef ISNAN_TRAP
1412      if (std::isnan(end.tran.x) || std::isnan(end.tran.y) || std::isnan(end.tran.z) ||
1413  	std::isnan(end.a) || std::isnan(end.b) || std::isnan(end.c) ||
1414  	std::isnan(end.u) || std::isnan(end.v) || std::isnan(end.w) ||
1415  	std::isnan(center.x) || std::isnan(center.y) || std::isnan(center.z) ||
1416  	std::isnan(normal.x) || std::isnan(normal.y) || std::isnan(normal.z)) {
1417  	printf("std::isnan error in emcTrajCircularMove()\n");
1418  	return 0;		// ignore it for now, just don't send it
1419      }
1420  #endif
1421  
1422      emcmotCommand.command = EMCMOT_SET_CIRCLE;
1423  
1424      emcmotCommand.pos = end;
1425      emcmotCommand.motion_type = type;
1426  
1427      emcmotCommand.center.x = center.x;
1428      emcmotCommand.center.y = center.y;
1429      emcmotCommand.center.z = center.z;
1430  
1431      emcmotCommand.normal.x = normal.x;
1432      emcmotCommand.normal.y = normal.y;
1433      emcmotCommand.normal.z = normal.z;
1434  
1435      emcmotCommand.turn = turn;
1436      emcmotCommand.id = TrajConfig.MotionId;
1437  
1438      emcmotCommand.vel = vel;
1439      emcmotCommand.ini_maxvel = ini_maxvel;
1440      emcmotCommand.acc = acc;
1441  
1442      return usrmotWriteEmcmotCommand(&emcmotCommand);
1443  }
1444  
1445  int emcTrajClearProbeTrippedFlag()
1446  {
1447      emcmotCommand.command = EMCMOT_CLEAR_PROBE_FLAGS;
1448  
1449      return usrmotWriteEmcmotCommand(&emcmotCommand);
1450  }
1451  
1452  int emcTrajProbe(EmcPose pos, int type, double vel, double ini_maxvel, double acc, unsigned char probe_type)
1453  {
1454  #ifdef ISNAN_TRAP
1455      if (std::isnan(pos.tran.x) || std::isnan(pos.tran.y) || std::isnan(pos.tran.z) ||
1456          std::isnan(pos.a) || std::isnan(pos.b) || std::isnan(pos.c) ||
1457          std::isnan(pos.u) || std::isnan(pos.v) || std::isnan(pos.w)) {
1458  	printf("std::isnan error in emcTrajProbe()\n");
1459  	return 0;		// ignore it for now, just don't send it
1460      }
1461  #endif
1462  
1463      emcmotCommand.command = EMCMOT_PROBE;
1464      emcmotCommand.pos = pos;
1465      emcmotCommand.id = TrajConfig.MotionId;
1466      emcmotCommand.motion_type = type;
1467      emcmotCommand.vel = vel;
1468      emcmotCommand.ini_maxvel = ini_maxvel;
1469      emcmotCommand.acc = acc;
1470      emcmotCommand.probe_type = probe_type;
1471  
1472      return usrmotWriteEmcmotCommand(&emcmotCommand);
1473  }
1474  
1475  int emcTrajRigidTap(EmcPose pos, double vel, double ini_maxvel, double acc, double scale)
1476  {
1477  #ifdef ISNAN_TRAP
1478      if (std::isnan(pos.tran.x) || std::isnan(pos.tran.y) || std::isnan(pos.tran.z)) {
1479  	printf("std::isnan error in emcTrajRigidTap()\n");
1480  	return 0;		// ignore it for now, just don't send it
1481      }
1482  #endif
1483  
1484      emcmotCommand.command = EMCMOT_RIGID_TAP;
1485      emcmotCommand.pos.tran = pos.tran;
1486      emcmotCommand.id = TrajConfig.MotionId;
1487      emcmotCommand.vel = vel;
1488      emcmotCommand.ini_maxvel = ini_maxvel;
1489      emcmotCommand.acc = acc;
1490      emcmotCommand.scale = scale;
1491  
1492      return usrmotWriteEmcmotCommand(&emcmotCommand);
1493  }
1494  
1495  
1496  static int last_id = 0;
1497  static int last_id_printed = 0;
1498  static int last_status = 0;
1499  static double last_id_time;
1500  
1501  int emcTrajUpdate(EMC_TRAJ_STAT * stat)
1502  {
1503      int joint, enables;
1504  
1505      stat->joints = TrajConfig.Joints;
1506      stat->spindles = TrajConfig.Spindles;
1507      stat->deprecated_axes = TrajConfig.DeprecatedAxes;
1508      stat->axis_mask = TrajConfig.AxisMask;
1509      stat->linearUnits = TrajConfig.LinearUnits;
1510      stat->angularUnits = TrajConfig.AngularUnits;
1511  
1512      stat->mode =
1513  	emcmotStatus.
1514  	motionFlag & EMCMOT_MOTION_TELEOP_BIT ? EMC_TRAJ_MODE_TELEOP
1515  	: (emcmotStatus.
1516  	   motionFlag & EMCMOT_MOTION_COORD_BIT ? EMC_TRAJ_MODE_COORD :
1517  	   EMC_TRAJ_MODE_FREE);
1518  
1519      /* enabled if motion enabled and all joints enabled */
1520      stat->enabled = 0;		/* start at disabled */
1521      if (emcmotStatus.motionFlag & EMCMOT_MOTION_ENABLE_BIT) {
1522  	for (joint = 0; joint < TrajConfig.Joints; joint++) {
1523  /*! \todo Another #if 0 */
1524  #if 0				/*! \todo FIXME - the axis flag has been moved to the joint struct */
1525  	    if (!emcmotStatus.axisFlag[axis] & EMCMOT_JOINT_ENABLE_BIT) {
1526  		break;
1527  	    }
1528  #endif
1529  	    /* got here, then all are enabled */
1530  	    stat->enabled = 1;
1531  	}
1532      }
1533  
1534      stat->inpos = emcmotStatus.motionFlag & EMCMOT_MOTION_INPOS_BIT;
1535      stat->queue = emcmotStatus.depth;
1536      stat->activeQueue = emcmotStatus.activeDepth;
1537      stat->queueFull = emcmotStatus.queueFull;
1538      stat->id = emcmotStatus.id;
1539      stat->motion_type = emcmotStatus.motionType;
1540      stat->distance_to_go = emcmotStatus.distance_to_go;
1541      stat->dtg = emcmotStatus.dtg;
1542      stat->current_vel = emcmotStatus.current_vel;
1543      if (EMC_DEBUG_MOTION_TIME & emc_debug) {
1544  	if (stat->id != last_id) {
1545  	    if (last_id != last_id_printed) {
1546  		rcs_print("Motion id %d took %f seconds.\n", last_id,
1547  			  etime() - last_id_time);
1548  		last_id_printed = last_id;
1549  	    }
1550  	    last_id = stat->id;
1551  	    last_id_time = etime();
1552  	}
1553      }
1554  
1555      stat->paused = emcmotStatus.paused;
1556      stat->scale = emcmotStatus.feed_scale;
1557      stat->rapid_scale = emcmotStatus.rapid_scale;
1558  
1559      stat->position = emcmotStatus.carte_pos_cmd;
1560  
1561      stat->actualPosition = emcmotStatus.carte_pos_fb;
1562  
1563      stat->velocity = emcmotStatus.vel;
1564      stat->acceleration = emcmotStatus.acc;
1565      stat->maxAcceleration = TrajConfig.MaxAccel;
1566  
1567      if (emcmotStatus.motionFlag & EMCMOT_MOTION_ERROR_BIT) {
1568  	stat->status = RCS_ERROR;
1569      } else if (stat->inpos && (stat->queue == 0)) {
1570  	stat->status = RCS_DONE;
1571      } else {
1572  	stat->status = RCS_EXEC;
1573      }
1574  
1575      if (EMC_DEBUG_MOTION_TIME & emc_debug) {
1576  	if (stat->status == RCS_DONE && last_status != RCS_DONE
1577  	    && stat->id != last_id_printed) {
1578  	    rcs_print("Motion id %d took %f seconds.\n", last_id,
1579  		      etime() - last_id_time);
1580  	    last_id_printed = last_id = stat->id;
1581  	    last_id_time = etime();
1582  	}
1583      }
1584  
1585      stat->probedPosition = emcmotStatus.probedPos;
1586  
1587      stat->probeval = emcmotStatus.probeVal;
1588      stat->probing = emcmotStatus.probing;
1589      stat->probe_tripped = emcmotStatus.probeTripped;
1590      
1591      if (emcmotStatus.motionFlag & EMCMOT_MOTION_COORD_BIT)
1592          enables = emcmotStatus.enables_queued;
1593      else
1594          enables = emcmotStatus.enables_new;
1595      
1596      stat->feed_override_enabled = enables & FS_ENABLED;
1597      stat->adaptive_feed_enabled = enables & AF_ENABLED;
1598      stat->feed_hold_enabled = enables & FH_ENABLED;
1599  
1600      if (new_config) {
1601  	stat->cycleTime = emcmotConfig.trajCycleTime;
1602  	stat->kinematics_type = emcmotConfig.kinType;
1603  	stat->maxVelocity = emcmotConfig.limitVel;
1604      }
1605  
1606      return 0;
1607  }
1608  
1609  
1610  int setup_inihal(void) {
1611      // Must be called after emcTrajInit(), which loads the number of
1612      // joints from the ini file.
1613      if (emcmotion_initialized != 1) {
1614          rcs_print_error("%s: emcMotionInit() has not completed, can't setup inihal\n", __FUNCTION__);
1615          return -1;
1616      }
1617  
1618      if (ini_hal_init(TrajConfig.Joints)) {
1619          rcs_print_error("%s: ini_hal_init(%d) failed\n", __FUNCTION__, TrajConfig.Joints);
1620          return -1;
1621      }
1622  
1623      if (ini_hal_init_pins(TrajConfig.Joints)) {
1624          rcs_print_error("%s: ini_hal_init_pins(%d) failed\n", __FUNCTION__, TrajConfig.Joints);
1625          return -1;
1626      }
1627  
1628      return 0;
1629  }
1630  
1631  
1632  int emcPositionLoad() {
1633      double positions[EMCMOT_MAX_JOINTS];
1634      IniFile ini;
1635      ini.Open(emc_inifile);
1636      const char *posfile = ini.Find("POSITION_FILE", "TRAJ");
1637      ini.Close();
1638      if(!posfile || !posfile[0]) return 0;
1639      FILE *f = fopen(posfile, "r");
1640      if(!f) return 0;
1641      for(int i=0; i<EMCMOT_MAX_JOINTS; i++) {
1642  	int r = fscanf(f, "%lf", &positions[i]);
1643  	if(r != 1) {
1644              fclose(f);
1645              rcs_print("%s: failed to load joint %d position from %s, ignoring\n", __FUNCTION__, i, posfile);
1646              return -1;
1647          }
1648      }
1649      fclose(f);
1650      int result = 0;
1651      for(int i=0; i<EMCMOT_MAX_JOINTS; i++) {
1652  	if(emcJointSetMotorOffset(i, -positions[i]) != 0) {
1653              rcs_print("%s: failed to set joint %d position (%.6f) from %s, ignoring\n", __FUNCTION__, i, positions[i], posfile);
1654              result = -1;
1655          }
1656      }
1657      return result;
1658  }
1659  
1660  
1661  int emcPositionSave() {
1662      IniFile ini;
1663      const char *posfile;
1664  
1665      ini.Open(emc_inifile);
1666      try {
1667          posfile = ini.Find("POSITION_FILE", "TRAJ");
1668      } catch (IniFile::Exception e) {
1669          ini.Close();
1670          return -1;
1671      }
1672      ini.Close();
1673  
1674      if(!posfile || !posfile[0]) return 0;
1675      // like the var file, make sure the posfile is recreated according to umask
1676      unlink(posfile);
1677      FILE *f = fopen(posfile, "w");
1678      if(!f) return -1;
1679      for(int i=0; i<EMCMOT_MAX_JOINTS; i++) {
1680  	int r = fprintf(f, "%.17f\n", emcmotStatus.joint_status[i].pos_fb);
1681  	if(r < 0) { fclose(f); return -1; }
1682      }
1683      fclose(f);
1684      return 0;
1685  }
1686  
1687  // EMC_MOTION functions
1688  
1689  // This function gets called by Task from emctask_startup().
1690  // emctask_startup() calls this function in a loop, retrying it until
1691  // it succeeds or until the retries time out.
1692  int emcMotionInit()
1693  {
1694      int r;
1695      int joint, axis;
1696      
1697      r = emcTrajInit(); // we want to check Traj first, the sane defaults for units are there
1698      // it also determines the number of existing joints, and axes
1699      if (r != 0) {
1700          rcs_print("%s: emcTrajInit failed\n", __FUNCTION__);
1701          return -1;
1702      }
1703  
1704      for (joint = 0; joint < TrajConfig.Joints; joint++) {
1705  	if (0 != emcJointInit(joint)) {
1706              rcs_print("%s: emcJointInit(%d) failed\n", __FUNCTION__, joint);
1707              return -1;
1708  	}
1709      }
1710  
1711      for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
1712          if (TrajConfig.AxisMask & (1<<axis)) {
1713  	    if (0 != emcAxisInit(axis)) {
1714                  rcs_print("%s: emcAxisInit(%d) failed\n", __FUNCTION__, axis);
1715                  return -1;
1716  	    }
1717  	}
1718      }
1719  
1720      // Ignore errors from emcPositionLoad(), because what are you going to do?
1721      (void)emcPositionLoad();
1722  
1723      emcmotion_initialized = 1;
1724  
1725      return 0;
1726  }
1727  
1728  int emcMotionHalt()
1729  {
1730      int r1, r2, r3, r4, r5;
1731      int t;
1732  
1733      r1 = -1;
1734      for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
1735  	if (0 == emcJointHalt(t)) {
1736  	    r1 = 0;		// at least one is okay
1737  	}
1738      }
1739  
1740      r2 = emcTrajDisable();
1741      r3 = emcTrajHalt();
1742      r4 = emcPositionSave();
1743      r5 = ini_hal_exit();
1744      emcmotion_initialized = 0;
1745  
1746      return (r1 == 0 && r2 == 0 && r3 == 0 && r4 == 0 && r5 == 0) ? 0 : -1;
1747  }
1748  
1749  int emcMotionAbort()
1750  {
1751      int r1;
1752      int r2;
1753      int r3 = 0;
1754      int t;
1755  
1756      r1 = -1;
1757      for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
1758  	if (0 == emcJointAbort(t)) {
1759  	    r1 = 0;		// at least one is okay
1760  	}
1761      }
1762  
1763      r2 = emcTrajAbort();
1764  
1765      return (r1 == 0 && r2 == 0 && r3 == 0) ? 0 : -1;
1766  }
1767  
1768  int emcMotionSetDebug(int debug)
1769  {
1770      emcmotCommand.command = EMCMOT_SET_DEBUG;
1771      emcmotCommand.debug = debug;
1772  
1773      return usrmotWriteEmcmotCommand(&emcmotCommand);
1774  }
1775  
1776  /*! \function emcMotionSetAout()
1777      
1778      This function sends a EMCMOT_SET_AOUT message to the motion controller.
1779      That one plans a AOUT command when motion starts or right now.
1780  
1781      @parameter	index	which output gets modified
1782      @parameter	now	wheather change is imediate or synched with motion
1783      @parameter	start	value set at start of motion
1784      @parameter	end	value set at end of motion
1785  */
1786  int emcMotionSetAout(unsigned char index, double start, double end, unsigned char now)
1787  {
1788      emcmotCommand.command = EMCMOT_SET_AOUT;
1789      emcmotCommand.now = now;
1790      emcmotCommand.out = index;
1791    /*! \todo FIXME-- if this works, set up some dedicated cmd fields instead of
1792       borrowing these */
1793      emcmotCommand.minLimit = start;
1794      emcmotCommand.maxLimit = end;
1795  
1796      return usrmotWriteEmcmotCommand(&emcmotCommand);
1797  }
1798  
1799  /*! \function emcMotionSetDout()
1800      
1801      This function sends a EMCMOT_SET_DOUT message to the motion controller.
1802      That one plans a DOUT command when motion starts or right now.
1803  
1804      @parameter	index	which output gets modified
1805      @parameter	now	wheather change is imediate or synched with motion
1806      @parameter	start	value set at start of motion
1807      @parameter	end	value set at end of motion
1808  */
1809  int emcMotionSetDout(unsigned char index, unsigned char start,
1810  		     unsigned char end, unsigned char now)
1811  {
1812      emcmotCommand.command = EMCMOT_SET_DOUT;
1813      emcmotCommand.now = now;
1814      emcmotCommand.out = index;
1815      emcmotCommand.start = start;
1816      emcmotCommand.end = end;
1817  
1818      return usrmotWriteEmcmotCommand(&emcmotCommand);
1819  }
1820  
1821  int emcSpindleAbort(int spindle)
1822  {
1823      return emcSpindleOff(spindle);
1824  }
1825  
1826  int emcSpindleSpeed(int spindle, double speed, double css_factor, double offset)
1827  {
1828      if (emcmotStatus.spindle_status[spindle].speed == 0){
1829          return 0;} //spindle stopped, not updating speed */
1830  
1831      return emcSpindleOn(spindle, speed, css_factor, offset);
1832  }
1833  
1834  int emcSpindleOrient(int spindle, double orientation, int mode)
1835  {
1836      emcmotCommand.command = EMCMOT_SPINDLE_ORIENT;
1837      emcmotCommand.spindle = spindle;
1838      emcmotCommand.orientation = orientation;
1839      emcmotCommand.mode = mode;
1840      return usrmotWriteEmcmotCommand(&emcmotCommand);
1841  }
1842  
1843  
1844  int emcSpindleOn(int spindle, double speed, double css_factor, double offset, int wait_for_at_speed)
1845  {
1846  
1847      emcmotCommand.command = EMCMOT_SPINDLE_ON;
1848      emcmotCommand.spindle = spindle;
1849      emcmotCommand.vel = speed;
1850      emcmotCommand.ini_maxvel = css_factor;
1851      emcmotCommand.acc = offset;
1852      emcmotCommand.wait_for_spindle_at_speed = wait_for_at_speed;
1853      return usrmotWriteEmcmotCommand(&emcmotCommand);
1854  }
1855  
1856  int emcSpindleOff(int spindle)
1857  {
1858      emcmotCommand.command = EMCMOT_SPINDLE_OFF;
1859      emcmotCommand.spindle = spindle;
1860      return usrmotWriteEmcmotCommand(&emcmotCommand);
1861  }
1862  
1863  int emcSpindleBrakeRelease(int spindle)
1864  {
1865      emcmotCommand.command = EMCMOT_SPINDLE_BRAKE_RELEASE;
1866      emcmotCommand.spindle = spindle;
1867      return usrmotWriteEmcmotCommand(&emcmotCommand);
1868  }
1869  
1870  int emcSpindleBrakeEngage(int spindle)
1871  {
1872      emcmotCommand.command = EMCMOT_SPINDLE_BRAKE_ENGAGE;
1873      emcmotCommand.spindle = spindle;
1874      return usrmotWriteEmcmotCommand(&emcmotCommand);
1875  }
1876  
1877  int emcSpindleIncrease(int spindle)
1878  {
1879      emcmotCommand.command = EMCMOT_SPINDLE_INCREASE;
1880      emcmotCommand.spindle = spindle;
1881      return usrmotWriteEmcmotCommand(&emcmotCommand);
1882  }
1883  
1884  int emcSpindleDecrease(int spindle)
1885  {
1886      emcmotCommand.command = EMCMOT_SPINDLE_DECREASE;
1887      emcmotCommand.spindle = spindle;
1888      return usrmotWriteEmcmotCommand(&emcmotCommand);
1889  }
1890  
1891  int emcSpindleConstant(int spindle)
1892  {
1893      return 0; // nothing to do
1894  }
1895  
1896  int emcSpindleUpdate(EMC_SPINDLE_STAT stat[], int num_spindles){
1897  	int s;
1898  	int enables;
1899      if (emcmotStatus.motionFlag & EMCMOT_MOTION_COORD_BIT)
1900          enables = emcmotStatus.enables_queued;
1901      else
1902          enables = emcmotStatus.enables_new;
1903  
1904      for (s = 0; s < num_spindles; s++){
1905  		stat[s].spindle_override_enabled = enables & SS_ENABLED;
1906  		stat[s].enabled = emcmotStatus.spindle_status[s].speed != 0;
1907  		stat[s].speed = emcmotStatus.spindle_status[s].speed;
1908  		stat[s].brake = emcmotStatus.spindle_status[s].brake;
1909  		stat[s].direction = emcmotStatus.spindle_status[s].direction;
1910  		stat[s].orient_state = emcmotStatus.spindle_status[s].orient_state;
1911  		stat[s].orient_fault = emcmotStatus.spindle_status[s].orient_fault;
1912  		stat[s].spindle_scale = emcmotStatus.spindle_status[s].scale;
1913      }
1914      return 0;
1915  }
1916  
1917  int emcMotionUpdate(EMC_MOTION_STAT * stat)
1918  {
1919      int r1, r2, r3, r4;
1920      int joint;
1921      int error;
1922      int exec;
1923      int dio, aio;
1924  
1925      // read the emcmot status
1926      if (0 != usrmotReadEmcmotStatus(&emcmotStatus)) {
1927  	return -1;
1928      }
1929      new_config = 0;
1930      if (emcmotStatus.config_num != emcmotConfig.config_num) {
1931  	if (0 != usrmotReadEmcmotConfig(&emcmotConfig)) {
1932  	    return -1;
1933  	}
1934  	new_config = 1;
1935      }
1936  
1937      if (get_emcmot_debug_info) {
1938  	if (0 != usrmotReadEmcmotDebug(&emcmotDebug)) {
1939  	    return -1;
1940  	}
1941      }
1942      // read the emcmot error
1943      if (0 != usrmotReadEmcmotError(errorString)) {
1944  	// no error, so ignore
1945      } else {
1946  	// an error to report
1947  	emcOperatorError(0, "%s", errorString);
1948      }
1949  
1950      // save the heartbeat and command number locally,
1951      // for use with emcMotionUpdate
1952      localMotionHeartbeat = emcmotStatus.heartbeat;
1953      localMotionCommandType = emcmotStatus.commandEcho;	/*! \todo FIXME-- not NML one! */
1954      localMotionEchoSerialNumber = emcmotStatus.commandNumEcho;
1955  
1956      r3 = emcTrajUpdate(&stat->traj);
1957      r1 = emcJointUpdate(&stat->joint[0], stat->traj.joints);
1958      r2 = emcAxisUpdate(&stat->axis[0], stat->traj.axis_mask);
1959      r3 = emcTrajUpdate(&stat->traj);
1960      r4 = emcSpindleUpdate(&stat->spindle[0], stat->traj.spindles);
1961      stat->heartbeat = localMotionHeartbeat;
1962      stat->command_type = localMotionCommandType;
1963      stat->echo_serial_number = localMotionEchoSerialNumber;
1964      stat->debug = emcmotConfig.debug;
1965  
1966      for (dio = 0; dio < EMCMOT_MAX_DIO; dio++) {
1967  	stat->synch_di[dio] = emcmotStatus.synch_di[dio];
1968  	stat->synch_do[dio] = emcmotStatus.synch_do[dio];
1969      }
1970  
1971      for (aio = 0; aio < EMCMOT_MAX_AIO; aio++) {
1972  	stat->analog_input[aio] = emcmotStatus.analog_input[aio];
1973  	stat->analog_output[aio] = emcmotStatus.analog_output[aio];
1974      }
1975  
1976      stat->numExtraJoints=emcmotStatus.numExtraJoints;
1977  
1978      // set the status flag
1979      error = 0;
1980      exec = 0;
1981  
1982      // FIXME-AJ: joints not axes
1983      for (joint = 0; joint < stat->traj.joints; joint++) {
1984  	if (stat->joint[joint].status == RCS_ERROR) {
1985  	    error = 1;
1986  	    break;
1987  	}
1988  	if (stat->joint[joint].status == RCS_EXEC) {
1989  	    exec = 1;
1990  	    break;
1991  	}
1992      }
1993      if (stat->traj.status == RCS_ERROR) {
1994  	error = 1;
1995      } else if (stat->traj.status == RCS_EXEC) {
1996  	exec = 1;
1997      }
1998  
1999      if (error) {
2000  	stat->status = RCS_ERROR;
2001      } else if (exec) {
2002  	stat->status = RCS_EXEC;
2003      } else {
2004  	stat->status = RCS_DONE;
2005      }
2006      return (r1 == 0 && r2 == 0 && r3 == 0 && r4 == 0) ? 0 : -1;
2007  }
2008  
2009  int emcSetupArcBlends(int arcBlendEnable,
2010          int arcBlendFallbackEnable,
2011          int arcBlendOptDepth,
2012          int arcBlendGapCycles,
2013          double arcBlendRampFreq,
2014          double arcBlendTangentKinkRatio) {
2015  
2016      emcmotCommand.command = EMCMOT_SETUP_ARC_BLENDS;
2017      emcmotCommand.arcBlendEnable = arcBlendEnable;
2018      emcmotCommand.arcBlendFallbackEnable = arcBlendFallbackEnable;
2019      emcmotCommand.arcBlendOptDepth = arcBlendOptDepth;
2020      emcmotCommand.arcBlendGapCycles = arcBlendGapCycles;
2021      emcmotCommand.arcBlendRampFreq = arcBlendRampFreq;
2022      emcmotCommand.arcBlendTangentKinkRatio = arcBlendTangentKinkRatio;
2023      return usrmotWriteEmcmotCommand(&emcmotCommand);
2024  }
2025  
2026  int emcSetMaxFeedOverride(double maxFeedScale) {
2027      emcmotCommand.command = EMCMOT_SET_MAX_FEED_OVERRIDE;
2028      emcmotCommand.maxFeedScale = maxFeedScale;
2029      return usrmotWriteEmcmotCommand(&emcmotCommand);
2030  }
2031  
2032  int emcSetProbeErrorInhibit(int j_inhibit, int h_inhibit) {
2033      emcmotCommand.command = EMCMOT_SET_PROBE_ERR_INHIBIT;
2034      emcmotCommand.probe_jog_err_inhibit = j_inhibit;
2035      emcmotCommand.probe_home_err_inhibit = h_inhibit;
2036      return usrmotWriteEmcmotCommand(&emcmotCommand);
2037  }
2038  
2039  int emcGetExternalOffsetApplied(void) {
2040      return emcmotStatus.external_offsets_applied;
2041  }
2042  
2043  EmcPose emcGetExternalOffsets(void) {
2044      return emcmotStatus.eoffset_pose;
2045  }