1  /********************************************************************
   2  * Description: tp.c
   3  *   Trajectory planner based on TC elements
   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  #include "rtapi.h"              /* rtapi_print_msg */
  14  #include "posemath.h"           /* Geometry types & functions */
  15  #include "tc.h"
  16  #include "tp.h"
  17  #include "emcpose.h"
  18  #include "rtapi_math.h"
  19  #include "mot_priv.h"
  20  #include "motion_debug.h"
  21  #include "motion_types.h"
  22  #include "spherical_arc.h"
  23  #include "blendmath.h"
  24  //KLUDGE Don't include all of emc.hh here, just hand-copy the TERM COND
  25  //definitions until we can break the emc constants out into a separate file.
  26  //#include "emc.hh"
  27  #define EMC_TRAJ_TERM_COND_STOP  0
  28  #define EMC_TRAJ_TERM_COND_EXACT 1
  29  #define EMC_TRAJ_TERM_COND_BLEND 2
  30  
  31  /**
  32   * @section tpdebugflags TP debugging flags
  33   * Enable / disable various debugging functions here.
  34   * These flags control debug printing from RTAPI. These functions are
  35   * admittedly kludged on top of the existing rtapi_print framework. As written,
  36   * though, it's an easy way to selectively compile functions as static or not,
  37   * and selectively compile in assertions and debug printing.
  38   */
  39  
  40  #include "tp_debug.h"
  41  
  42  // FIXME: turn off this feature, which causes blends between rapids to
  43  // use the feed override instead of the rapid override
  44  #undef TP_SHOW_BLENDS
  45  
  46  #define TP_OPTIMIZATION_LAZY
  47  
  48  extern emcmot_status_t *emcmotStatus;
  49  extern emcmot_debug_t *emcmotDebug;
  50  extern emcmot_config_t *emcmotConfig;
  51  
  52  /** static function primitives (ugly but less of a pain than moving code around)*/
  53  STATIC int tpComputeBlendVelocity(
  54          TC_STRUCT const *tc,
  55          TC_STRUCT const *nexttc,
  56          double v_target_this,
  57          double v_target_next,
  58          double *v_blend_this,
  59          double *v_blend_next,
  60          double *v_blend_net);
  61  
  62  STATIC double estimateParabolicBlendPerformance(
  63          TP_STRUCT const *tp,
  64          TC_STRUCT const *tc,
  65          TC_STRUCT const *nexttc);
  66  
  67  STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc);
  68  
  69  STATIC int tpUpdateCycle(TP_STRUCT * const tp,
  70          TC_STRUCT * const tc, TC_STRUCT const * const nexttc);
  71  
  72  STATIC int tpRunOptimization(TP_STRUCT * const tp);
  73  
  74  STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc, int inc_id);
  75  
  76  STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc);
  77  
  78  /**
  79   * @section tpcheck Internal state check functions.
  80   * These functions compartmentalize some of the messy state checks.
  81   * Hopefully this makes changes easier to track as much of the churn will be on small functions.
  82   */
  83  
  84  /**
  85   * Returns true if there is motion along ABC or UVW axes, false otherwise.
  86   */
  87  STATIC int tcRotaryMotionCheck(TC_STRUCT const * const tc) {
  88      switch (tc->motion_type) {
  89          //Note lack of break statements due to every path returning
  90          case TC_RIGIDTAP:
  91              return false;
  92          case TC_LINEAR:
  93              if (tc->coords.line.abc.tmag_zero && tc->coords.line.uvw.tmag_zero) {
  94                  return false;
  95              } else {
  96                  return true;
  97              }
  98          case TC_CIRCULAR:
  99              if (tc->coords.circle.abc.tmag_zero && tc->coords.circle.uvw.tmag_zero) {
 100                  return false;
 101              } else {
 102                  return true;
 103              }
 104          case TC_SPHERICAL:
 105              return true;
 106          default:
 107              tp_debug_print("Unknown motion type!\n");
 108              return false;
 109      }
 110  }
 111  
 112  
 113  /**
 114   * @section tpgetset Internal Get/Set functions
 115   * @brief Calculation / status functions for commonly used values.
 116   * These functions return the "actual" values of things like a trajectory
 117   * segment's feed override, while taking into account the status of tp itself.
 118   */
 119  
 120  
 121  
 122  /**
 123   * Wrapper to bounds-check the tangent kink ratio from HAL.
 124   */
 125  STATIC double tpGetTangentKinkRatio(void) {
 126      const double max_ratio = 0.7071;
 127      const double min_ratio = 0.001;
 128  
 129      return fmax(fmin(emcmotConfig->arcBlendTangentKinkRatio,max_ratio),min_ratio);
 130  }
 131  
 132  
 133  STATIC int tpGetMachineAccelBounds(PmCartesian  * const acc_bound) {
 134      if (!acc_bound) {
 135          return TP_ERR_FAIL;
 136      }
 137  
 138      acc_bound->x = emcmotDebug->axes[0].acc_limit; //0==>x
 139      acc_bound->y = emcmotDebug->axes[1].acc_limit; //1==>y
 140      acc_bound->z = emcmotDebug->axes[2].acc_limit; //2==>z
 141      return TP_ERR_OK;
 142  }
 143  
 144  
 145  STATIC int tpGetMachineVelBounds(PmCartesian  * const vel_bound) {
 146      if (!vel_bound) {
 147          return TP_ERR_FAIL;
 148      }
 149  
 150      vel_bound->x = emcmotDebug->axes[0].vel_limit; //0==>x
 151      vel_bound->y = emcmotDebug->axes[1].vel_limit; //1==>y
 152      vel_bound->z = emcmotDebug->axes[2].vel_limit; //2==>z
 153      return TP_ERR_OK;
 154  }
 155  
 156  STATIC int tpGetMachineActiveLimit(double * const act_limit, PmCartesian const * const bounds) {
 157      if (!act_limit) {
 158          return TP_ERR_FAIL;
 159      }
 160      //Start with max accel value
 161      *act_limit = fmax(fmax(bounds->x,bounds->y),bounds->z);
 162  
 163      // Compare only with active axes
 164      if (bounds->x > 0) {
 165          *act_limit = fmin(*act_limit, bounds->x);
 166      }
 167      if (bounds->y > 0) {
 168          *act_limit = fmin(*act_limit, bounds->y);
 169      }
 170      if (bounds->z > 0) {
 171          *act_limit = fmin(*act_limit, bounds->z);
 172      }
 173      tp_debug_print(" arc blending a_max=%f\n", *act_limit);
 174      return TP_ERR_OK;
 175  }
 176  
 177  
 178  /**
 179   * Get a segment's feed scale based on the current planner state and emcmotStatus.
 180   * @note depends on emcmotStatus for system information.
 181   */
 182  STATIC double tpGetFeedScale(TP_STRUCT const * const tp,
 183          TC_STRUCT const * const tc) {
 184      if (!tc) {
 185          return 0.0;
 186      }
 187      //All reasons to disable feed override go here
 188      bool pausing = tp->pausing && (tc->synchronized == TC_SYNC_NONE || tc->synchronized == TC_SYNC_VELOCITY);
 189      bool aborting = tp->aborting;
 190      if (pausing)  {
 191          tc_debug_print("pausing\n");
 192          return 0.0;
 193      } else if (aborting) {
 194          tc_debug_print("aborting\n");
 195          return 0.0;
 196      } else if (tc->synchronized == TC_SYNC_POSITION ) {
 197          return 1.0;
 198      } else if (tc->is_blending) {
 199          //KLUDGE: Don't allow feed override to keep blending from overruning max velocity
 200          return fmin(emcmotStatus->net_feed_scale, 1.0);
 201      } else {
 202          return emcmotStatus->net_feed_scale;
 203      }
 204  }
 205  
 206  
 207  /**
 208   * Get target velocity for a tc based on the trajectory planner state.
 209   * This gives the requested velocity, capped by the segments maximum velocity.
 210   */
 211  STATIC inline double tpGetRealTargetVel(TP_STRUCT const * const tp,
 212          TC_STRUCT const * const tc) {
 213  
 214      if (!tc) {
 215          return 0.0;
 216      }
 217      // Start with the scaled target velocity based on the current feed scale
 218      double v_target = tc->synchronized ? tc->target_vel : tc->reqvel;
 219      /*tc_debug_print("Initial v_target = %f\n",v_target);*/
 220  
 221      // Get the maximum allowed target velocity, and make sure we're below it
 222      return fmin(v_target * tpGetFeedScale(tp,tc), tpGetMaxTargetVel(tp, tc));
 223  }
 224  
 225  
 226  STATIC inline double getMaxFeedScale(TC_STRUCT const * tc)
 227  {
 228      //All reasons to disable feed override go here
 229      if (tc && tc->synchronized == TC_SYNC_POSITION ) {
 230          return 1.0;
 231      } else {
 232          return emcmotConfig->maxFeedScale;
 233      }
 234  }
 235  
 236  
 237  /**
 238   * Get the worst-case target velocity for a segment based on the trajectory planner state.
 239   * Note that this factors in the user-specified velocity limit.
 240   */
 241  STATIC inline double tpGetMaxTargetVel(TP_STRUCT const * const tp, TC_STRUCT const * const tc)
 242  {
 243      double max_scale = emcmotConfig->maxFeedScale;
 244      if (tc->is_blending) {
 245          //KLUDGE: Don't allow feed override to keep blending from overruning max velocity
 246          max_scale = fmin(max_scale, 1.0);
 247      }
 248      // Get maximum reachable velocity from max feed override
 249      double v_max_target = tc->target_vel * max_scale;
 250  
 251      /* Check if the cartesian velocity limit applies and clip the maximum
 252       * velocity. The vLimit is from the max velocity slider, and should
 253       * restrict the maximum velocity during non-synced moves and velocity
 254       * synchronization. However, position-synced moves have the target velocity
 255       * computed in the TP, so it would disrupt position tracking to apply this
 256       * limit here.
 257       */
 258      if (!tcPureRotaryCheck(tc) && (tc->synchronized != TC_SYNC_POSITION)){
 259          /*tc_debug_print("Cartesian velocity limit active\n");*/
 260          v_max_target = fmin(v_max_target,tp->vLimit);
 261      }
 262  
 263      // Apply maximum segment velocity limit (must always be respected)
 264      return fmin(v_max_target, tc->maxvel);
 265  }
 266  
 267  
 268  /**
 269   * Get final velocity for a tc based on the trajectory planner state.
 270   * This function factors in the feed override and TC limits. It clamps the
 271   * final velocity to the maximum velocity and the next segment's target velocity
 272   */
 273  STATIC inline double tpGetRealFinalVel(TP_STRUCT const * const tp,
 274          TC_STRUCT const * const tc, TC_STRUCT const * const nexttc) {
 275      /* If we're stepping, then it doesn't matter what the optimization says, we want to end at a stop.
 276       * If the term_cond gets changed out from under us, detect this and force final velocity to zero
 277       */
 278      if (emcmotDebug->stepping || tc->term_cond != TC_TERM_COND_TANGENT || tp->reverse_run) {
 279          return 0.0;
 280      } 
 281      
 282      // Get target velocities for this segment and next segment
 283      double v_target_this = tpGetRealTargetVel(tp, tc);
 284      double v_target_next = 0.0;
 285      if (nexttc) {
 286          v_target_next = tpGetRealTargetVel(tp, nexttc);
 287      }
 288  
 289      tc_debug_print("v_target_next = %f\n",v_target_next);
 290      // Limit final velocity to minimum of this and next target velocities
 291      double v_target = fmin(v_target_this, v_target_next);
 292      return fmin(tc->finalvel, v_target);
 293  }
 294  
 295  
 296  /**
 297   * Convert the 2-part spindle position and sign to a signed double.
 298   */
 299  STATIC inline double tpGetSignedSpindlePosition(spindle_status_t *status) {
 300  	int spindle_dir;
 301  	double spindle_pos;
 302  	spindle_dir = status->direction;
 303  	spindle_pos = status->spindleRevs;
 304      if (spindle_dir < 0.0) {
 305          spindle_pos*=-1.0;
 306      }
 307      return spindle_pos;
 308  }
 309  
 310  /**
 311   * @section tpaccess tp class-like API
 312   */
 313  
 314  /**
 315   * Create the trajectory planner structure with an empty queue.
 316   */
 317  int tpCreate(TP_STRUCT * const tp, int _queueSize, TC_STRUCT * const tcSpace)
 318  {
 319      if (0 == tp) {
 320          return TP_ERR_FAIL;
 321      }
 322  
 323      if (_queueSize <= 0) {
 324          tp->queueSize = TP_DEFAULT_QUEUE_SIZE;
 325      } else {
 326          tp->queueSize = _queueSize;
 327      }
 328  
 329      /* create the queue */
 330      if (-1 == tcqCreate(&tp->queue, tp->queueSize, tcSpace)) {
 331          return TP_ERR_FAIL;
 332      }
 333  
 334      /* init the rest of our data */
 335      return tpInit(tp);
 336  }
 337  
 338  /**
 339   * Clears any potential DIO toggles and anychanged.
 340   * If any DIOs need to be changed: dios[i] = 1, DIO needs to get turned on, -1
 341   * = off
 342   */
 343  int tpClearDIOs(TP_STRUCT * const tp) {
 344      //XXX: All IO's will be flushed on next synced aio/dio! Is it ok?
 345      int i;
 346      tp->syncdio.anychanged = 0;
 347      tp->syncdio.dio_mask = 0;
 348      tp->syncdio.aio_mask = 0;
 349      for (i = 0; i < emcmotConfig->numDIO; i++) {
 350          tp->syncdio.dios[i] = 0;
 351      }
 352      for (i = 0; i < emcmotConfig->numAIO; i++) {
 353          tp->syncdio.aios[i] = 0;
 354      }
 355  
 356      return TP_ERR_OK;
 357  }
 358  
 359  /**
 360   *    "Soft initialize" the trajectory planner tp.
 361   *    This is a "soft" initialization in that TP_STRUCT configuration
 362   *    parameters (cycleTime, vMax, and aMax) are left alone, but the queue is
 363   *    cleared, and the flags are set to an empty, ready queue. The currentPos
 364   *    is left alone, and goalPos is set to this position.  This function is
 365   *    intended to put the motion queue in the state it would be if all queued
 366   *    motions finished at the current position.
 367   */
 368  int tpClear(TP_STRUCT * const tp)
 369  {
 370      tcqInit(&tp->queue);
 371      tp->queueSize = 0;
 372      tp->goalPos = tp->currentPos;
 373      tp->nextId = 0;
 374      tp->execId = 0;
 375      tp->motionType = 0;
 376      tp->termCond = TC_TERM_COND_PARABOLIC;
 377      tp->tolerance = 0.0;
 378      tp->done = 1;
 379      tp->depth = tp->activeDepth = 0;
 380      tp->aborting = 0;
 381      tp->pausing = 0;
 382      tp->reverse_run = 0;
 383      tp->synchronized = 0;
 384      tp->uu_per_rev = 0.0;
 385      emcmotStatus->current_vel = 0.0;
 386      emcmotStatus->requested_vel = 0.0;
 387      emcmotStatus->distance_to_go = 0.0;
 388      ZERO_EMC_POSE(emcmotStatus->dtg);
 389      SET_MOTION_INPOS_FLAG(1);
 390  
 391      return tpClearDIOs(tp);
 392  }
 393  
 394  /**
 395   * Fully initialize the tp structure.
 396   * Sets tp configuration to default values and calls tpClear to create a fresh,
 397   * empty queue.
 398   */
 399  int tpInit(TP_STRUCT * const tp)
 400  {
 401      tp->cycleTime = 0.0;
 402      //Velocity limits
 403      tp->vLimit = 0.0;
 404      tp->ini_maxvel = 0.0;
 405      //Accelerations
 406      tp->aLimit = 0.0;
 407      PmCartesian acc_bound;
 408      //FIXME this acceleration bound isn't valid (nor is it used)
 409      tpGetMachineAccelBounds(&acc_bound);
 410      tpGetMachineActiveLimit(&tp->aMax, &acc_bound);
 411      //Angular limits
 412      tp->wMax = 0.0;
 413      tp->wDotMax = 0.0;
 414  
 415      tp->spindle.offset = 0.0;
 416      tp->spindle.revs = 0.0;
 417      tp->spindle.waiting_for_index = MOTION_INVALID_ID;
 418      tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID;
 419  
 420      tp->reverse_run = TC_DIR_FORWARD;
 421  
 422      ZERO_EMC_POSE(tp->currentPos);
 423  
 424      PmCartesian vel_bound;
 425      tpGetMachineVelBounds(&vel_bound);
 426      tpGetMachineActiveLimit(&tp->vMax, &vel_bound);
 427  
 428      return tpClear(tp);
 429  }
 430  
 431  /**
 432   * Set the cycle time for the trajectory planner.
 433   */
 434  int tpSetCycleTime(TP_STRUCT * const tp, double secs)
 435  {
 436      if (0 == tp || secs <= 0.0) {
 437          return TP_ERR_FAIL;
 438      }
 439  
 440      tp->cycleTime = secs;
 441  
 442      return TP_ERR_OK;
 443  }
 444  
 445  /**
 446   * Set requested velocity and absolute maximum velocity (bounded by machine).
 447   * This is called before adding lines or circles, specifying vMax (the velocity
 448   * requested by the F word) and ini_maxvel, the max velocity possible before
 449   * meeting a machine constraint caused by an AXIS's max velocity.  (the TP is
 450   * allowed to go up to this high when feed override >100% is requested)  These
 451   * settings apply to subsequent moves until changed.
 452   */
 453  int tpSetVmax(TP_STRUCT * const tp, double vMax, double ini_maxvel)
 454  {
 455      if (0 == tp || vMax <= 0.0 || ini_maxvel <= 0.0) {
 456          return TP_ERR_FAIL;
 457      }
 458  
 459      tp->vMax = vMax;
 460      tp->ini_maxvel = ini_maxvel;
 461  
 462      return TP_ERR_OK;
 463  }
 464  
 465  /**
 466   * Set the tool tip maximum velocity.
 467   * This is the [TRAJ]MAX_LINEAR_VELOCITY. This should be the max velocity of
 468   * const the TOOL TIP, not necessarily any particular axis. This applies to
 469   * subsequent moves until changed.
 470   */
 471  int tpSetVlimit(TP_STRUCT * const tp, double vLimit)
 472  {
 473      if (!tp) return TP_ERR_FAIL;
 474  
 475      if (vLimit < 0.)
 476          tp->vLimit = 0.;
 477      else
 478          tp->vLimit = vLimit;
 479  
 480      return TP_ERR_OK;
 481  }
 482  
 483  /** Sets the max acceleration for the trajectory planner. */
 484  int tpSetAmax(TP_STRUCT * const tp, double aMax)
 485  {
 486      if (0 == tp || aMax <= 0.0) {
 487          return TP_ERR_FAIL;
 488      }
 489  
 490      tp->aMax = aMax;
 491  
 492      return TP_ERR_OK;
 493  }
 494  
 495  /**
 496   * Sets the id that will be used for the next appended motions.
 497   * nextId is incremented so that the next time a motion is appended its id will
 498   * be one more than the previous one, modulo a signed int. If you want your own
 499   * ids for each motion, call this before each motion you append and stick what
 500   * you want in here.
 501   */
 502  int tpSetId(TP_STRUCT * const tp, int id)
 503  {
 504  
 505      if (!MOTION_ID_VALID(id)) {
 506          rtapi_print_msg(RTAPI_MSG_ERR, "tpSetId: invalid motion id %d\n", id);
 507          return TP_ERR_FAIL;
 508      }
 509  
 510      if (0 == tp) {
 511          return TP_ERR_FAIL;
 512      }
 513  
 514      tp->nextId = id;
 515  
 516      return TP_ERR_OK;
 517  }
 518  
 519  /** Returns the id of the last motion that is currently
 520    executing.*/
 521  int tpGetExecId(TP_STRUCT * const tp)
 522  {
 523      if (0 == tp) {
 524          return TP_ERR_FAIL;
 525      }
 526  
 527      return tp->execId;
 528  }
 529  
 530  /**
 531   * Sets the termination condition for all subsequent queued moves.
 532   * If cond is TC_TERM_COND_STOP, motion comes to a stop before a subsequent move
 533   * begins. If cond is TC_TERM_COND_PARABOLIC, the following move is begun when the
 534   * current move slows below a calculated blend velocity.
 535   */
 536  int tpSetTermCond(TP_STRUCT * const tp, int cond, double tolerance)
 537  {
 538      if (!tp) {
 539          return TP_ERR_FAIL;
 540      }
 541  
 542      switch (cond) {
 543          //Purposeful waterfall for now
 544          case TC_TERM_COND_PARABOLIC:
 545          case TC_TERM_COND_TANGENT:
 546          case TC_TERM_COND_EXACT:
 547          case TC_TERM_COND_STOP:
 548              tp->termCond = cond;
 549              tp->tolerance = tolerance;
 550              break;
 551          default:
 552              //Invalid condition
 553              return  -1;
 554      }
 555  
 556      return TP_ERR_OK;
 557  }
 558  
 559  /**
 560   * Used to tell the tp the initial position.
 561   * It sets the current position AND the goal position to be the same.  Used
 562   * only at TP initialization and when switching modes.
 563   */
 564  int tpSetPos(TP_STRUCT * const tp, EmcPose const * const pos)
 565  {
 566      if (0 == tp) {
 567          return TP_ERR_FAIL;
 568      }
 569  
 570      int res_invalid = tpSetCurrentPos(tp, pos);
 571      if (res_invalid) {
 572          return TP_ERR_FAIL;
 573      }
 574  
 575      tp->goalPos = *pos;
 576      return TP_ERR_OK;
 577  }
 578  
 579  
 580  /**
 581   * Set current position.
 582   * It sets the current position AND the goal position to be the same.  Used
 583   * only at TP initialization and when switching modes.
 584   */
 585  int tpSetCurrentPos(TP_STRUCT * const tp, EmcPose const * const pos)
 586  {
 587      if (0 == tp) {
 588          return TP_ERR_FAIL;
 589      }
 590  
 591      if (emcPoseValid(pos)) {
 592          tp->currentPos = *pos;
 593          return TP_ERR_OK;
 594      } else {
 595          rtapi_print_msg(RTAPI_MSG_ERR, "Tried to set invalid pose in tpSetCurrentPos on id %d!"
 596                  "pos is %.12g, %.12g, %.12g\n",
 597                  tp->execId,
 598                  pos->tran.x,
 599                  pos->tran.y,
 600                  pos->tran.z);
 601          return TP_ERR_INVALID;
 602      }
 603  }
 604  
 605  
 606  int tpAddCurrentPos(TP_STRUCT * const tp, EmcPose const * const disp)
 607  {
 608      if (!tp || !disp) {
 609          return TP_ERR_MISSING_INPUT;
 610      }
 611  
 612      if (emcPoseValid(disp)) {
 613          emcPoseSelfAdd(&tp->currentPos, disp);
 614          return TP_ERR_OK;
 615      } else {
 616          rtapi_print_msg(RTAPI_MSG_ERR, "Tried to set invalid pose in tpAddCurrentPos on id %d!"
 617                  "disp is %.12g, %.12g, %.12g\n",
 618                  tp->execId,
 619                  disp->tran.x,
 620                  disp->tran.y,
 621                  disp->tran.z);
 622          return TP_ERR_INVALID;
 623      }
 624  }
 625  
 626  
 627  /**
 628   * Check for valid tp before queueing additional moves.
 629   */
 630  int tpErrorCheck(TP_STRUCT const * const tp) {
 631  
 632      if (!tp) {
 633          rtapi_print_msg(RTAPI_MSG_ERR, "TP is null\n");
 634          return TP_ERR_FAIL;
 635      }
 636      if (tp->aborting) {
 637          rtapi_print_msg(RTAPI_MSG_ERR, "TP is aborting\n");
 638          return TP_ERR_FAIL;
 639      }
 640      return TP_ERR_OK;
 641  }
 642  
 643  
 644  /**
 645   * Find the "peak" velocity a segment can achieve if its velocity profile is triangular.
 646   * This is used to estimate blend velocity, though by itself is not enough
 647   * (since requested velocity and max velocity could be lower).
 648   */
 649  STATIC double tpCalculateTriangleVel(TC_STRUCT const *tc) {
 650      //Compute peak velocity for blend calculations
 651      double acc_scaled = tcGetTangentialMaxAccel(tc);
 652      double length = tc->target;
 653      if (!tc->finalized) {
 654          // blending may remove up to 1/2 of the segment
 655          length /= 2.0;
 656      }
 657      return findVPeak(acc_scaled, length);
 658  }
 659  
 660  
 661  /**
 662   * Handles the special case of blending into an unfinalized segment.
 663   * The problem here is that the last segment in the queue can always be cut
 664   * short by a blend to the next segment. However, we can only ever consume at
 665   * most 1/2 of the segment. This function computes the worst-case final
 666   * velocity the previous segment can have, if we want to exactly stop at the
 667   * halfway point.
 668   */
 669  STATIC double tpCalculateOptimizationInitialVel(TP_STRUCT const * const tp, TC_STRUCT * const tc)
 670  {
 671      double acc_scaled = tcGetTangentialMaxAccel(tc);
 672      double triangle_vel = findVPeak(acc_scaled, tc->target);
 673      double max_vel = tpGetMaxTargetVel(tp, tc);
 674      tp_debug_json_start(tpCalculateOptimizationInitialVel);
 675      tp_debug_json_double(triangle_vel);
 676      tp_debug_json_end();
 677      return fmin(triangle_vel, max_vel);
 678  }
 679  
 680  
 681  /**
 682   * Initialize a blend arc from its parent lines.
 683   * This copies and initializes properties from the previous and next lines to
 684   * initialize a blend arc. This function does not handle connecting the
 685   * segments together, however.
 686   */
 687  STATIC int tpInitBlendArcFromPrev(TP_STRUCT const * const tp,
 688          TC_STRUCT const * const prev_tc,
 689          TC_STRUCT* const blend_tc,
 690          double vel,
 691          double ini_maxvel,
 692          double acc) {
 693  
 694  #ifdef TP_SHOW_BLENDS
 695      int canon_motion_type = EMC_MOTION_TYPE_ARC;
 696  #else
 697      int canon_motion_type = prev_tc->canon_motion_type;
 698  #endif
 699  
 700      tcInit(blend_tc,
 701              TC_SPHERICAL,
 702              canon_motion_type,
 703              tp->cycleTime,
 704              prev_tc->enables,
 705              false); // NOTE: blend arc never needs the atspeed flag, since the previous line will have it (and cannot be consumed).
 706  
 707      // Copy over state data from TP
 708      tcSetupState(blend_tc, tp);
 709      
 710      // Set kinematics parameters from blend calculations
 711      tcSetupMotion(blend_tc,
 712              vel,
 713              ini_maxvel,
 714              acc);
 715  
 716      // Skip syncdio setup since this blend extends the previous line
 717      blend_tc->syncdio = prev_tc->syncdio; //enqueue the list of DIOs that need toggling
 718  
 719      // find "helix" length for target
 720      double length;
 721      arcLength(&blend_tc->coords.arc.xyz, &length);
 722      tp_info_print("blend tc length = %f\n",length);
 723      blend_tc->target = length;
 724      blend_tc->nominal_length = length;
 725  
 726      // Set the blend arc to be tangent to the next segment
 727      tcSetTermCond(blend_tc, NULL, TC_TERM_COND_TANGENT);
 728  
 729      //NOTE: blend arc radius and everything else is finalized, so set this to 1.
 730      //In the future, radius may be adjustable.
 731      tcFinalizeLength(blend_tc);
 732  
 733      return TP_ERR_OK;
 734  }
 735  
 736  STATIC int tcSetLineXYZ(TC_STRUCT * const tc, PmCartLine const * const line)
 737  {
 738  
 739      //Update targets with new arc length
 740      if (!line || tc->motion_type != TC_LINEAR) {
 741          return TP_ERR_FAIL;
 742      }
 743      if (!tc->coords.line.abc.tmag_zero || !tc->coords.line.uvw.tmag_zero) {
 744          rtapi_print_msg(RTAPI_MSG_ERR, "SetLineXYZ does not supportABC or UVW motion\n");
 745          return TP_ERR_FAIL;
 746      }
 747  
 748      tc->coords.line.xyz = *line;
 749      tc->target = line->tmag;
 750      return TP_ERR_OK;
 751  }
 752  
 753  
 754  static inline int find_max_element(double arr[], int sz)
 755  {
 756      if (sz < 1) {
 757          return -1;
 758      }
 759      // Assumes at least one element
 760      int max_idx = 0;
 761      int idx;
 762      for (idx = 0; idx < sz; ++idx) {
 763          if (arr[idx] > arr[max_idx]) {
 764              max_idx = idx;
 765          }
 766      }
 767      return max_idx;
 768  }
 769  
 770  /**
 771   * Compare performance of blend arc and equivalent tangent speed.
 772   * If we can go faster by assuming the segments are already tangent (and
 773   * slowing down some), then prefer this over using the blend arc. This is
 774   * mostly useful for some odd arc-to-arc cases where the blend arc becomes very
 775   * short (and therefore slow).
 776   */
 777  STATIC tc_blend_type_t tpChooseBestBlend(TP_STRUCT const * const tp,
 778          TC_STRUCT * const prev_tc,
 779          TC_STRUCT * const tc,
 780          TC_STRUCT * const blend_tc)
 781  {
 782      if (!tc || !prev_tc) {
 783          return NO_BLEND;
 784      }
 785  
 786      // Can't blend segments that are explicitly disallowed
 787      switch  (prev_tc->term_cond)
 788      {
 789      case TC_TERM_COND_EXACT:
 790      case TC_TERM_COND_STOP:
 791          return NO_BLEND;
 792      }
 793  
 794      // Compute performance measures ("perf_xxx") for each method. This is
 795      // basically the blend velocity. However, because parabolic blends require
 796      // halving the acceleration of both blended segments, they in effect slow
 797      // down the next and previous blends as well. We model this loss by scaling
 798      // the blend velocity down to find an "equivalent" velocity.
 799      double perf_parabolic = estimateParabolicBlendPerformance(tp, prev_tc, tc) / 2.0;
 800      double perf_tangent = prev_tc->kink_vel;
 801      double perf_arc_blend = blend_tc ? blend_tc->maxvel : 0.0;
 802  
 803      tp_debug_print("Blend performance: parabolic %f, tangent %f, arc_blend %f, ",
 804                     perf_parabolic,
 805                     perf_tangent,
 806                     perf_arc_blend);
 807  
 808      // KLUDGE Order the performance measurements so that they match the enum values
 809      double perf[3] = {perf_parabolic, perf_tangent, perf_arc_blend};
 810      tc_blend_type_t best_blend = find_max_element(perf, 3);
 811  
 812      switch (best_blend) {
 813          case PARABOLIC_BLEND: // parabolic
 814              tp_debug_print("using parabolic blend\n");
 815              tcRemoveKinkProperties(prev_tc, tc);
 816              tcSetTermCond(prev_tc, tc, TC_TERM_COND_PARABOLIC);
 817              break;
 818          case TANGENT_SEGMENTS_BLEND: // tangent
 819              tp_debug_print("using approximate tangent blend\n");
 820              // NOTE: acceleration / velocity reduction is done dynamically in functions that access TC_STRUCT properties
 821              tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT);
 822              break;
 823          case ARC_BLEND: // arc blend
 824              tp_debug_print("using blend arc\n");
 825              tcRemoveKinkProperties(prev_tc, tc);
 826  
 827              break;
 828          case NO_BLEND:
 829              break;
 830      }
 831      return best_blend;
 832  }
 833  
 834  
 835  STATIC tp_err_t tpCreateLineArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc)
 836  {
 837      tp_debug_print("-- Starting LineArc blend arc --\n");
 838  
 839      PmCartesian acc_bound, vel_bound;
 840      
 841      //Get machine limits
 842      tpGetMachineAccelBounds(&acc_bound);
 843      tpGetMachineVelBounds(&vel_bound);
 844  
 845      //Populate blend geometry struct
 846      BlendGeom3 geom;
 847      BlendParameters param;
 848      BlendPoints3 points_approx;
 849      BlendPoints3 points_exact;
 850  
 851      int res_init = blendInit3FromLineArc(&geom, &param,
 852              prev_tc,
 853              tc,
 854              &acc_bound,
 855              &vel_bound,
 856              emcmotConfig->maxFeedScale);
 857  
 858      if (res_init != TP_ERR_OK) {
 859          tp_debug_print("blend init failed with code %d, aborting blend arc\n",
 860                  res_init);
 861          return res_init;
 862      }
 863  
 864      // Check for coplanarity based on binormal and tangents
 865      int coplanar = pmUnitCartsColinear(&geom.binormal,
 866              &tc->coords.circle.xyz.normal);
 867  
 868      if (!coplanar) {
 869          tp_debug_print("aborting arc, not coplanar\n");
 870          return TP_ERR_FAIL;
 871      }
 872  
 873      int res_param = blendComputeParameters(&param);
 874  
 875      int res_points = blendFindPoints3(&points_approx, &geom, &param);
 876      
 877      int res_post = blendLineArcPostProcess(&points_exact,
 878              &points_approx,
 879              &param, 
 880              &geom, &prev_tc->coords.line.xyz,
 881              &tc->coords.circle.xyz);
 882  
 883      //Catch errors in blend setup
 884      if (res_init || res_param || res_points || res_post) {
 885          tp_debug_print("Got %d, %d, %d, %d for init, param, points, post, aborting arc\n",
 886                  res_init,
 887                  res_param,
 888                  res_points,
 889                  res_post);
 890          return TP_ERR_FAIL;
 891      }
 892      
 893      /* If blend calculations were successful, then we're ready to create the
 894       * blend arc.
 895       */
 896  
 897      if (points_exact.trim2 > param.phi2_max) {
 898          tp_debug_print("trim2 %f > phi2_max %f, aborting arc...\n",
 899                  points_exact.trim2,
 900                  param.phi2_max);
 901          return TP_ERR_FAIL;
 902      }
 903  
 904      blendCheckConsume(&param, &points_exact, prev_tc, emcmotConfig->arcBlendGapCycles);
 905      //Store working copies of geometry
 906      PmCartLine line1_temp = prev_tc->coords.line.xyz;
 907      PmCircle circ2_temp = tc->coords.circle.xyz;
 908  
 909      // Change lengths of circles
 910      double new_len1 = line1_temp.tmag - points_exact.trim1;
 911      int res_stretch1 = pmCartLineStretch(&line1_temp,
 912              new_len1,
 913              false);
 914  
 915      double phi2_new = tc->coords.circle.xyz.angle - points_exact.trim2;
 916  
 917      tp_debug_print("phi2_new = %f\n",phi2_new);
 918      int res_stretch2 = pmCircleStretch(&circ2_temp,
 919              phi2_new,
 920              true);
 921      //TODO create blends
 922      if (res_stretch1 || res_stretch2) {
 923          tp_debug_print("segment resize failed, aborting arc\n");
 924          return TP_ERR_FAIL;
 925      }
 926  
 927      //Get exact start and end points to account for spiral in arcs
 928      pmCartLinePoint(&line1_temp,
 929              line1_temp.tmag,
 930              &points_exact.arc_start);
 931      pmCirclePoint(&circ2_temp,
 932              0.0,
 933              &points_exact.arc_end);
 934      //TODO deal with large spiral values, or else detect and fall back?
 935  
 936      blendPoints3Print(&points_exact);
 937      int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz,
 938              &points_exact,
 939              &geom,
 940              &param);
 941      if (res_arc < 0) {
 942          tp_debug_print("arc creation failed, aborting arc\n");
 943          return TP_ERR_FAIL;
 944      }
 945  
 946      // Note that previous restrictions don't allow ABC or UVW movement, so the
 947      // end and start points should be identical
 948      blend_tc->coords.arc.abc = prev_tc->coords.line.abc.end;
 949      blend_tc->coords.arc.uvw = prev_tc->coords.line.uvw.end;
 950  
 951      //set the max velocity to v_plan, since we'll violate constraints otherwise.
 952      tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req,
 953              param.v_plan, param.a_max);
 954  
 955      int res_tangent = checkTangentAngle(&circ2_temp,
 956              &blend_tc->coords.arc.xyz,
 957              &geom,
 958              &param,
 959              tp->cycleTime,
 960              true);
 961  
 962      if (res_tangent < 0) {
 963          tp_debug_print("failed tangent check, aborting arc...\n");
 964          return TP_ERR_FAIL;
 965      }
 966  
 967      if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) {
 968          return TP_ERR_NO_ACTION;
 969      }
 970  
 971      tp_debug_print("Passed all tests, updating segments\n");
 972      //TODO refactor to pass consume to connect function
 973      if (param.consume) {
 974          //Since we're consuming the previous segment, pop the last line off of the queue
 975          int res_pop = tcqPopBack(&tp->queue);
 976          if (res_pop) {
 977              tp_debug_print("failed to pop segment, aborting arc\n");
 978              return TP_ERR_FAIL;
 979          }
 980      } else {
 981          tcSetLineXYZ(prev_tc, &line1_temp);
 982          //KLUDGE the previous segment is still there, so we don't need the at-speed flag on the blend too
 983          blend_tc->atspeed=0;
 984      }
 985      tcSetCircleXYZ(tc, &circ2_temp);
 986  
 987      tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT);
 988  
 989      return TP_ERR_OK;
 990  }
 991  
 992  
 993  STATIC tp_err_t tpCreateArcLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc)
 994  {
 995  
 996      tp_debug_print("-- Starting ArcLine blend arc --\n");
 997      PmCartesian acc_bound, vel_bound;
 998      
 999      //Get machine limits
1000      tpGetMachineAccelBounds(&acc_bound);
1001      tpGetMachineVelBounds(&vel_bound);
1002  
1003      //Populate blend geometry struct
1004      BlendGeom3 geom;
1005      BlendParameters param;
1006      BlendPoints3 points_approx;
1007      BlendPoints3 points_exact;
1008      param.consume = 0;
1009  
1010      int res_init = blendInit3FromArcLine(&geom, &param,
1011              prev_tc,
1012              tc,
1013              &acc_bound,
1014              &vel_bound,
1015              emcmotConfig->maxFeedScale);
1016      if (res_init != TP_ERR_OK) {
1017          tp_debug_print("blend init failed with code %d, aborting blend arc\n",
1018                  res_init);
1019          return res_init;
1020      }
1021  
1022      // Check for coplanarity based on binormal
1023      int coplanar = pmUnitCartsColinear(&geom.binormal,
1024              &prev_tc->coords.circle.xyz.normal);
1025  
1026      if (!coplanar) {
1027          tp_debug_print("aborting arc, not coplanar\n");
1028          return TP_ERR_FAIL;
1029      }
1030  
1031      int res_param = blendComputeParameters(&param);
1032  
1033      int res_points = blendFindPoints3(&points_approx, &geom, &param);
1034      
1035      int res_post = blendArcLinePostProcess(&points_exact,
1036              &points_approx,
1037              &param, 
1038              &geom, &prev_tc->coords.circle.xyz,
1039              &tc->coords.line.xyz);
1040  
1041      //Catch errors in blend setup
1042      if (res_init || res_param || res_points || res_post) {
1043          tp_debug_print("Got %d, %d, %d, %d for init, param, points, post\n",
1044                  res_init,
1045                  res_param,
1046                  res_points,
1047                  res_post);
1048          return TP_ERR_FAIL;
1049      }
1050      
1051      blendCheckConsume(&param, &points_exact, prev_tc, emcmotConfig->arcBlendGapCycles);
1052  
1053      /* If blend calculations were successful, then we're ready to create the
1054       * blend arc.
1055       */
1056  
1057      // Store working copies of geometry
1058      PmCircle circ1_temp = prev_tc->coords.circle.xyz;
1059      PmCartLine line2_temp = tc->coords.line.xyz;
1060  
1061      // Update start and end points of segment copies
1062      double phi1_new = circ1_temp.angle - points_exact.trim1;
1063  
1064      if (points_exact.trim1 > param.phi1_max) {
1065          tp_debug_print("trim1 %f > phi1_max %f, aborting arc...\n",
1066                  points_exact.trim1,
1067                  param.phi1_max);
1068          return TP_ERR_FAIL;
1069      }
1070  
1071      int res_stretch1 = pmCircleStretch(&circ1_temp,
1072              phi1_new,
1073              false);
1074      if (res_stretch1 != TP_ERR_OK) {
1075          return TP_ERR_FAIL;
1076      }
1077  
1078      double new_len2 = tc->target - points_exact.trim2;
1079      int res_stretch2 = pmCartLineStretch(&line2_temp,
1080              new_len2,
1081              true);
1082  
1083      if (res_stretch1 || res_stretch2) {
1084          tp_debug_print("segment resize failed, aborting arc\n");
1085          return TP_ERR_FAIL;
1086      }
1087  
1088      pmCirclePoint(&circ1_temp,
1089              circ1_temp.angle,
1090              &points_exact.arc_start);
1091  
1092      pmCartLinePoint(&line2_temp,
1093              0.0,
1094              &points_exact.arc_end);
1095  
1096      blendPoints3Print(&points_exact);
1097  
1098      int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, &points_exact, &geom, &param);
1099      if (res_arc < 0) {
1100          return TP_ERR_FAIL;
1101      }
1102  
1103      // Note that previous restrictions don't allow ABC or UVW movement, so the
1104      // end and start points should be identical
1105      blend_tc->coords.arc.abc = tc->coords.line.abc.start;
1106      blend_tc->coords.arc.uvw = tc->coords.line.uvw.start;
1107  
1108      //set the max velocity to v_plan, since we'll violate constraints otherwise.
1109      tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req,
1110              param.v_plan, param.a_max);
1111  
1112      int res_tangent = checkTangentAngle(&circ1_temp, &blend_tc->coords.arc.xyz, &geom, &param, tp->cycleTime, false);
1113      if (res_tangent) {
1114          tp_debug_print("failed tangent check, aborting arc...\n");
1115          return TP_ERR_FAIL;
1116      }
1117  
1118      if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) {
1119          return TP_ERR_NO_ACTION;
1120      }
1121  
1122      tp_debug_print("Passed all tests, updating segments\n");
1123  
1124      tcSetCircleXYZ(prev_tc, &circ1_temp);
1125      tcSetLineXYZ(tc, &line2_temp);
1126  
1127      //Cleanup any mess from parabolic
1128      tc->blend_prev = 0;
1129      blend_tc->atspeed=0;
1130      tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT);
1131      return TP_ERR_OK;
1132  }
1133  
1134  STATIC tp_err_t tpCreateArcArcBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc, TC_STRUCT * const tc, TC_STRUCT * const blend_tc)
1135  {
1136  
1137      tp_debug_print("-- Starting ArcArc blend arc --\n");
1138      //TODO type checks
1139      int colinear = pmUnitCartsColinear(&prev_tc->coords.circle.xyz.normal,
1140              &tc->coords.circle.xyz.normal);
1141      if (!colinear) {
1142          // Fail out if not collinear
1143          tp_debug_print("arc abort: not coplanar\n");
1144          return TP_ERR_FAIL;
1145      }
1146  
1147      PmCartesian acc_bound, vel_bound;
1148      
1149      //Get machine limits
1150      tpGetMachineAccelBounds(&acc_bound);
1151      tpGetMachineVelBounds(&vel_bound);
1152  
1153      //Populate blend geometry struct
1154      BlendGeom3 geom;
1155      BlendParameters param;
1156      BlendPoints3 points_approx;
1157      BlendPoints3 points_exact;
1158  
1159      int res_init = blendInit3FromArcArc(&geom, &param,
1160              prev_tc,
1161              tc,
1162              &acc_bound,
1163              &vel_bound,
1164              emcmotConfig->maxFeedScale);
1165  
1166      if (res_init != TP_ERR_OK) {
1167          tp_debug_print("blend init failed with code %d, aborting blend arc\n",
1168                  res_init);
1169          return res_init;
1170      }
1171  
1172      int coplanar1 = pmUnitCartsColinear(&geom.binormal,
1173              &prev_tc->coords.circle.xyz.normal);
1174  
1175      if (!coplanar1) {
1176          tp_debug_print("aborting blend arc, arc id %d is not coplanar with binormal\n", prev_tc->id);
1177          return TP_ERR_FAIL;
1178      }
1179  
1180      int coplanar2 = pmUnitCartsColinear(&geom.binormal,
1181              &tc->coords.circle.xyz.normal);
1182      if (!coplanar2) {
1183          tp_debug_print("aborting blend arc, arc id %d is not coplanar with binormal\n", tc->id);
1184          return TP_ERR_FAIL;
1185      }
1186  
1187  
1188  
1189      int res_param = blendComputeParameters(&param);
1190      int res_points = blendFindPoints3(&points_approx, &geom, &param);
1191      
1192      int res_post = blendArcArcPostProcess(&points_exact,
1193              &points_approx,
1194              &param, 
1195              &geom, &prev_tc->coords.circle.xyz,
1196              &tc->coords.circle.xyz);
1197  
1198      //Catch errors in blend setup
1199      if (res_init || res_param || res_points || res_post) {
1200          tp_debug_print("Got %d, %d, %d, %d for init, param, points, post\n",
1201                  res_init,
1202                  res_param,
1203                  res_points,
1204                  res_post);
1205  
1206          return TP_ERR_FAIL;
1207      }
1208  
1209      blendCheckConsume(&param, &points_exact, prev_tc, emcmotConfig->arcBlendGapCycles);
1210      
1211      /* If blend calculations were successful, then we're ready to create the
1212       * blend arc. Begin work on temp copies of each circle here:
1213       */
1214  
1215      double phi1_new = prev_tc->coords.circle.xyz.angle - points_exact.trim1;
1216      double phi2_new = tc->coords.circle.xyz.angle - points_exact.trim2;
1217  
1218      // TODO pare down this debug output
1219      tp_debug_print("phi1_new = %f, trim1 = %f\n", phi1_new, points_exact.trim1);
1220      tp_debug_print("phi2_new = %f, trim2 = %f\n", phi2_new, points_exact.trim2);
1221  
1222      if (points_exact.trim1 > param.phi1_max) {
1223          tp_debug_print("trim1 %f > phi1_max %f, aborting arc...\n",
1224                  points_exact.trim1,
1225                  param.phi1_max);
1226          return TP_ERR_FAIL;
1227      }
1228  
1229      if (points_exact.trim2 > param.phi2_max) {
1230          tp_debug_print("trim2 %f > phi2_max %f, aborting arc...\n",
1231                  points_exact.trim2,
1232                  param.phi2_max);
1233          return TP_ERR_FAIL;
1234      }
1235  
1236      //Store working copies of geometry
1237      PmCircle circ1_temp = prev_tc->coords.circle.xyz;
1238      PmCircle circ2_temp = tc->coords.circle.xyz;
1239  
1240      int res_stretch1 = pmCircleStretch(&circ1_temp,
1241              phi1_new,
1242              false);
1243      if (res_stretch1 != TP_ERR_OK) {
1244          return TP_ERR_FAIL;
1245      }
1246  
1247      int res_stretch2 = pmCircleStretch(&circ2_temp,
1248              phi2_new,
1249              true);
1250      if (res_stretch1 || res_stretch2) {
1251          tp_debug_print("segment resize failed, aborting arc\n");
1252          return TP_ERR_FAIL;
1253      }
1254  
1255      //Get exact start and end points to account for spiral in arcs
1256      pmCirclePoint(&circ1_temp,
1257              circ1_temp.angle,
1258              &points_exact.arc_start);
1259      pmCirclePoint(&circ2_temp,
1260              0.0,
1261              &points_exact.arc_end);
1262  
1263      tp_debug_print("Modified arc points\n");
1264      blendPoints3Print(&points_exact);
1265      int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, &points_exact, &geom, &param);
1266      if (res_arc < 0) {
1267          return TP_ERR_FAIL;
1268      }
1269  
1270      // Note that previous restrictions don't allow ABC or UVW movement, so the
1271      // end and start points should be identical
1272      blend_tc->coords.arc.abc = prev_tc->coords.circle.abc.end;
1273      blend_tc->coords.arc.uvw = prev_tc->coords.circle.uvw.end;
1274  
1275      //set the max velocity to v_plan, since we'll violate constraints otherwise.
1276      tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req,
1277              param.v_plan, param.a_max);
1278  
1279      int res_tangent1 = checkTangentAngle(&circ1_temp, &blend_tc->coords.arc.xyz, &geom, &param, tp->cycleTime, false);
1280      int res_tangent2 = checkTangentAngle(&circ2_temp, &blend_tc->coords.arc.xyz, &geom, &param, tp->cycleTime, true);
1281      if (res_tangent1 || res_tangent2) {
1282          tp_debug_print("failed tangent check, aborting arc...\n");
1283          return TP_ERR_FAIL;
1284      }
1285  
1286      if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) {
1287          return TP_ERR_NO_ACTION;
1288      }
1289  
1290      tp_debug_print("Passed all tests, updating segments\n");
1291  
1292      tcSetCircleXYZ(prev_tc, &circ1_temp);
1293      tcSetCircleXYZ(tc, &circ2_temp);
1294  
1295      //Cleanup any mess from parabolic
1296      tc->blend_prev = 0;
1297      blend_tc->atspeed=0;
1298      tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT);
1299      return TP_ERR_OK;
1300  }
1301  
1302  
1303  STATIC tp_err_t tpCreateLineLineBlend(TP_STRUCT * const tp, TC_STRUCT * const prev_tc,
1304          TC_STRUCT * const tc, TC_STRUCT * const blend_tc)
1305  {
1306  
1307      tp_debug_print("-- Starting LineLine blend arc --\n");
1308      PmCartesian acc_bound, vel_bound;
1309      
1310      //Get machine limits
1311      tpGetMachineAccelBounds(&acc_bound);
1312      tpGetMachineVelBounds(&vel_bound);
1313      
1314      // Setup blend data structures
1315      BlendGeom3 geom;
1316      BlendParameters param;
1317      BlendPoints3 points;
1318  
1319      int res_init = blendInit3FromLineLine(&geom, &param,
1320              prev_tc,
1321              tc,
1322              &acc_bound,
1323              &vel_bound,
1324              emcmotConfig->maxFeedScale);
1325  
1326      if (res_init != TP_ERR_OK) {
1327          tp_debug_print("blend init failed with code %d, aborting blend arc\n",
1328                  res_init);
1329          return res_init;
1330      }
1331  
1332      int res_blend = blendComputeParameters(&param);
1333      if (res_blend != TP_ERR_OK) {
1334          return res_blend;
1335      }
1336  
1337      blendFindPoints3(&points, &geom, &param);
1338  
1339      blendCheckConsume(&param, &points, prev_tc, emcmotConfig->arcBlendGapCycles);
1340  
1341      // Set up actual blend arc here
1342      int res_arc = arcFromBlendPoints3(&blend_tc->coords.arc.xyz, &points, &geom, &param);
1343      if (res_arc < 0) {
1344          return TP_ERR_FAIL;
1345      }
1346  
1347      // Note that previous restrictions don't allow ABC or UVW movement, so the
1348      // end and start points should be identical
1349      blend_tc->coords.arc.abc = prev_tc->coords.line.abc.end;
1350      blend_tc->coords.arc.uvw = prev_tc->coords.line.uvw.end;
1351  
1352      //set the max velocity to v_plan, since we'll violate constraints otherwise.
1353      tpInitBlendArcFromPrev(tp, prev_tc, blend_tc, param.v_req,
1354              param.v_plan, param.a_max);
1355  
1356      tp_debug_print("blend_tc target_vel = %g\n", blend_tc->target_vel);
1357  
1358      if (tpChooseBestBlend(tp, prev_tc, tc, blend_tc) != ARC_BLEND) {
1359          return TP_ERR_NO_ACTION;
1360      }
1361  
1362      int retval = TP_ERR_FAIL;
1363  
1364      //TODO refactor to pass consume to connect function
1365      if (param.consume) {
1366          //Since we're consuming the previous segment, pop the last line off of the queue
1367          retval = tcqPopBack(&tp->queue);
1368          if (retval) {
1369              //This is unrecoverable since we've already changed the line. Something is wrong if we get here...
1370              rtapi_print_msg(RTAPI_MSG_ERR, "PopBack failed\n");
1371              return TP_ERR_FAIL;
1372          }
1373          //Since the blend arc meets the end of the previous line, we only need
1374          //to "connect" to the next line
1375          retval = tcConnectBlendArc(NULL, tc, &points.arc_start, &points.arc_end);
1376      } else {
1377          //TODO refactor connect function to stretch lines and check for bad stretching
1378          tp_debug_print("keeping previous line\n");
1379          retval = tcConnectBlendArc(prev_tc, tc, &points.arc_start, &points.arc_end);
1380          blend_tc->atspeed=0;
1381      }
1382      return retval;
1383  }
1384  
1385  
1386  /**
1387   * Add a newly created motion segment to the tp queue.
1388   * Returns an error code if the queue operation fails, otherwise adds a new
1389   * segment to the queue and updates the end point of the trajectory planner.
1390   */
1391  STATIC inline int tpAddSegmentToQueue(TP_STRUCT * const tp, TC_STRUCT * const tc, int inc_id) {
1392  
1393      tc->id = tp->nextId;
1394      if (tcqPut(&tp->queue, tc) == -1) {
1395          rtapi_print_msg(RTAPI_MSG_ERR, "tcqPut failed.\n");
1396          return TP_ERR_FAIL;
1397      }
1398      if (inc_id) {
1399          tp->nextId++;
1400      }
1401  
1402      // Store end of current move as new final goal of TP
1403      // KLUDGE: endpoint is garbage for rigid tap since it's supposed to retract past the start point.
1404      if (tc->motion_type != TC_RIGIDTAP) {
1405          tcGetEndpoint(tc, &tp->goalPos);
1406      }
1407      tp->done = 0;
1408      tp->depth = tcqLen(&tp->queue);
1409      //Fixing issue with duplicate id's?
1410      tp_debug_print("Adding TC id %d of type %d, total length %0.08f\n",tc->id,tc->motion_type,tc->target);
1411  
1412      return TP_ERR_OK;
1413  }
1414  
1415  STATIC int tpCheckCanonType(TC_STRUCT * prev_tc, TC_STRUCT * tc)
1416  {
1417      if (!tc || !prev_tc) {
1418          return TP_ERR_FAIL;
1419      }
1420      if ((prev_tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE) ^
1421              (tc->canon_motion_type == EMC_MOTION_TYPE_TRAVERSE)) {
1422          tp_debug_print("Can't blend between rapid and feed move, aborting arc\n");
1423          tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP);
1424      }
1425      return TP_ERR_OK;
1426  }
1427  
1428  STATIC int tpSetupSyncedIO(TP_STRUCT * const tp, TC_STRUCT * const tc) {
1429      if (tp->syncdio.anychanged != 0) {
1430          tc->syncdio = tp->syncdio; //enqueue the list of DIOs that need toggling
1431          tpClearDIOs(tp); // clear out the list, in order to prepare for the next time we need to use it
1432          return TP_ERR_OK;
1433      } else {
1434          tc->syncdio.anychanged = 0;
1435          return TP_ERR_NO_ACTION;
1436      }
1437  
1438  
1439  }
1440  
1441  
1442  /**
1443   * Adds a rigid tap cycle to the motion queue.
1444   */
1445  int tpAddRigidTap(TP_STRUCT * const tp, EmcPose end, double vel, double ini_maxvel,
1446          double acc, unsigned char enables, double scale) {
1447      if (tpErrorCheck(tp)) {
1448          return TP_ERR_FAIL;
1449      }
1450  
1451      tp_info_print("== AddRigidTap ==\n");
1452  
1453      if(!tp->synchronized) {
1454          rtapi_print_msg(RTAPI_MSG_ERR, "Cannot add unsynchronized rigid tap move.\n");
1455          return TP_ERR_FAIL;
1456      }
1457  
1458      TC_STRUCT tc = {0};
1459  
1460      /* Initialize rigid tap move.
1461       * NOTE: rigid tapping does not have a canonical type.
1462       * NOTE: always need atspeed since this is a synchronized movement.
1463       * */
1464      tcInit(&tc,
1465              TC_RIGIDTAP,
1466              0,
1467              tp->cycleTime,
1468              enables,
1469              1);
1470  
1471      // Setup any synced IO for this move
1472      tpSetupSyncedIO(tp, &tc);
1473  
1474      // Copy over state data from the trajectory planner
1475      tcSetupState(&tc, tp);
1476  
1477      // Copy in motion parameters
1478      tcSetupMotion(&tc,
1479              vel,
1480              ini_maxvel,
1481              acc);
1482  
1483      // Setup rigid tap geometry
1484      pmRigidTapInit(&tc.coords.rigidtap,
1485              &tp->goalPos,
1486              &end, scale);
1487      tc.target = pmRigidTapTarget(&tc.coords.rigidtap, tp->uu_per_rev);
1488  
1489      // Force exact stop mode after rigid tapping regardless of TP setting
1490      tcSetTermCond(&tc, NULL, TC_TERM_COND_STOP);
1491  
1492      TC_STRUCT *prev_tc;
1493      //Assume non-zero error code is failure
1494      prev_tc = tcqLast(&tp->queue);
1495      tcFinalizeLength(prev_tc);
1496      tcFlagEarlyStop(prev_tc, &tc);
1497      int retval = tpAddSegmentToQueue(tp, &tc, true);
1498      tpRunOptimization(tp);
1499      return retval;
1500  }
1501  
1502  STATIC blend_type_t tpCheckBlendArcType(
1503          TC_STRUCT const * const prev_tc,
1504          TC_STRUCT const * const tc) {
1505  
1506      if (!prev_tc || !tc) {
1507          tp_debug_print("prev_tc or tc doesn't exist\n");
1508          return BLEND_NONE;
1509      }
1510  
1511      //If exact stop, we don't compute the arc
1512      if (prev_tc->term_cond != TC_TERM_COND_PARABOLIC) {
1513          tp_debug_print("Wrong term cond = %u\n", prev_tc->term_cond);
1514          return BLEND_NONE;
1515      }
1516  
1517      //If we have any rotary axis motion, then don't create a blend arc
1518      if (tcRotaryMotionCheck(tc) || tcRotaryMotionCheck(prev_tc)) {
1519          tp_debug_print("One of the segments has rotary motion, aborting blend arc\n");
1520          return BLEND_NONE;
1521      }
1522  
1523      if (tc->finalized || prev_tc->finalized) {
1524          tp_debug_print("Can't create blend when segment lengths are finalized\n");
1525          return BLEND_NONE;
1526      }
1527  
1528      tp_debug_print("Motion types: prev_tc = %u, tc = %u\n",
1529              prev_tc->motion_type,tc->motion_type);
1530      //If not linear blends, we can't easily compute an arc
1531      if ((prev_tc->motion_type == TC_LINEAR) && (tc->motion_type == TC_LINEAR)) {
1532          return BLEND_LINE_LINE;
1533      } else if (prev_tc->motion_type == TC_LINEAR && tc->motion_type == TC_CIRCULAR) {
1534          return BLEND_LINE_ARC;
1535      } else if (prev_tc->motion_type == TC_CIRCULAR && tc->motion_type == TC_LINEAR) {
1536          return BLEND_ARC_LINE;
1537      } else if (prev_tc->motion_type == TC_CIRCULAR && tc->motion_type == TC_CIRCULAR) {
1538          return BLEND_ARC_ARC;
1539      } else {
1540          return BLEND_NONE;
1541      }
1542  }
1543  
1544  
1545  /**
1546   * Based on the nth and (n-1)th segment, find a safe final velocity for the (n-1)th segment.
1547   * This function also caps the target velocity if velocity ramping is enabled. If we
1548   * don't do this, then the linear segments (with higher tangential
1549   * acceleration) will speed up and slow down to reach their target velocity,
1550   * creating "humps" in the velocity profile.
1551   */
1552  STATIC int tpComputeOptimalVelocity(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT * const prev1_tc) {
1553      //Calculate the maximum starting velocity vs_back of segment tc, given the
1554      //trajectory parameters
1555      double acc_this = tcGetTangentialMaxAccel(tc);
1556  
1557      // Find the reachable velocity of tc, moving backwards in time
1558      double vs_back = pmSqrt(pmSq(tc->finalvel) + 2.0 * acc_this * tc->target);
1559      // Find the reachable velocity of prev1_tc, moving forwards in time
1560  
1561      double vf_limit_this = tc->maxvel;
1562      double vf_limit_prev = prev1_tc->maxvel;
1563      if (prev1_tc->kink_vel >=0  && prev1_tc->term_cond == TC_TERM_COND_TANGENT) {
1564          // Only care about kink_vel with tangent segments
1565          vf_limit_prev = fmin(vf_limit_prev, prev1_tc->kink_vel);
1566      }
1567      //Limit the PREVIOUS velocity by how much we can overshoot into
1568      double vf_limit = fmin(vf_limit_this, vf_limit_prev);
1569  
1570      if (vs_back >= vf_limit ) {
1571          //If we've hit the requested velocity, then prev_tc is definitely a "peak"
1572          vs_back = vf_limit;
1573          prev1_tc->optimization_state = TC_OPTIM_AT_MAX;
1574          tp_debug_print("found peak due to v_limit %f\n", vf_limit);
1575      }
1576  
1577      //Limit tc's target velocity to avoid creating "humps" in the velocity profile
1578      prev1_tc->finalvel = vs_back;
1579  
1580      //Reduce max velocity to match sample rate
1581      double sample_maxvel = tc->target / (tp->cycleTime * TP_MIN_SEGMENT_CYCLES);
1582      tc->maxvel = fmin(tc->maxvel, sample_maxvel);
1583  
1584      tp_info_print(" prev1_tc-> fv = %f, tc->fv = %f\n",
1585              prev1_tc->finalvel, tc->finalvel);
1586  
1587      return TP_ERR_OK;
1588  }
1589  
1590  
1591  /**
1592   * Do "rising tide" optimization to find allowable final velocities for each queued segment.
1593   * Walk along the queue from the back to the front. Based on the "current"
1594   * segment's final velocity, calculate the previous segment's maximum allowable
1595   * final velocity. The depth we walk along the queue is controlled by the
1596   * TP_LOOKAHEAD_DEPTH constant for now. The process safetly aborts early due to
1597   * a short queue or other conflicts.
1598   */
1599  STATIC int tpRunOptimization(TP_STRUCT * const tp) {
1600      // Pointers to the "current", previous, and 2nd previous trajectory
1601      // components. Current in this context means the segment being optimized,
1602      // NOT the currently excecuting segment.
1603  
1604      TC_STRUCT *tc;
1605      TC_STRUCT *prev1_tc;
1606  
1607      int ind, x;
1608      int len = tcqLen(&tp->queue);
1609      //TODO make lookahead depth configurable from the INI file
1610  
1611      int hit_peaks = 0;
1612      // Flag that says we've hit at least 1 non-tangent segment
1613      bool hit_non_tangent = false;
1614  
1615      /* Starting at the 2nd to last element in the queue, work backwards towards
1616       * the front. We can't do anything with the very last element because its
1617       * length may change if a new line is added to the queue.*/
1618  
1619      for (x = 1; x < emcmotConfig->arcBlendOptDepth + 2; ++x) {
1620          tp_info_print("==== Optimization step %d ====\n",x);
1621  
1622          // Update the pointers to the trajectory segments in use
1623          ind = len-x;
1624          tc = tcqItem(&tp->queue, ind);
1625          prev1_tc = tcqItem(&tp->queue, ind-1);
1626  
1627          if ( !prev1_tc || !tc) {
1628              tp_debug_print(" Reached end of queue in optimization\n");
1629              return TP_ERR_OK;
1630          }
1631  
1632          // stop optimizing if we hit a non-tangent segment (final velocity
1633          // stays zero)
1634          if (prev1_tc->term_cond != TC_TERM_COND_TANGENT) {
1635              if (hit_non_tangent) {
1636                  // 2 or more non-tangent segments means we're past where the optimizer can help
1637                  tp_debug_print("Found 2nd non-tangent segment, stopping optimization\n");
1638                  return TP_ERR_OK;
1639              } else  {
1640                  tp_debug_print("Found first non-tangent segment, contining\n");
1641                  hit_non_tangent = true;
1642                  continue;
1643              }
1644          }
1645  
1646          double progress_ratio = prev1_tc->progress / prev1_tc->target;
1647          // can safely decelerate to halfway point of segment from 25% of segment
1648          double cutoff_ratio = BLEND_DIST_FRACTION / 2.0;
1649  
1650          if (progress_ratio >= cutoff_ratio) {
1651              tp_debug_print("segment %d has moved past %f percent progress, cannot blend safely!\n",
1652                      ind-1, cutoff_ratio * 100.0);
1653              return TP_ERR_OK;
1654          }
1655  
1656          //Somewhat pedantic check for other conditions that would make blending unsafe
1657          if (prev1_tc->splitting || prev1_tc->blending_next) {
1658              tp_debug_print("segment %d is already blending, cannot optimize safely!\n",
1659                      ind-1);
1660              return TP_ERR_OK;
1661          }
1662  
1663          tp_info_print("  current term = %u, type = %u, id = %u, accel_mode = %d\n",
1664                  tc->term_cond, tc->motion_type, tc->id, tc->accel_mode);
1665          tp_info_print("  prev term = %u, type = %u, id = %u, accel_mode = %d\n",
1666                  prev1_tc->term_cond, prev1_tc->motion_type, prev1_tc->id, prev1_tc->accel_mode);
1667  
1668          if (tc->atspeed) {
1669              //Assume worst case that we have a stop at this point. This may cause a
1670              //slight hiccup, but the alternative is a sudden hard stop.
1671              tp_debug_print("Found atspeed at id %d\n",tc->id);
1672              tc->finalvel = 0.0;
1673          }
1674  
1675          if (!tc->finalized) {
1676              tp_debug_print("Segment %d, type %d not finalized, continuing\n",tc->id,tc->motion_type);
1677              // use worst-case final velocity that allows for up to 1/2 of a segment to be consumed.
1678  
1679              prev1_tc->finalvel = fmin(prev1_tc->maxvel, tpCalculateOptimizationInitialVel(tp,tc));
1680  
1681              // Fixes acceleration violations when last segment is not finalized, and previous segment is tangent.
1682              if (prev1_tc->kink_vel >=0  && prev1_tc->term_cond == TC_TERM_COND_TANGENT) {
1683                prev1_tc->finalvel = fmin(prev1_tc->finalvel, prev1_tc->kink_vel);
1684              }
1685              tc->finalvel = 0.0;
1686          } else {
1687              tpComputeOptimalVelocity(tp, tc, prev1_tc);
1688          }
1689  
1690          tc->active_depth = x - 2 - hit_peaks;
1691  #ifdef TP_OPTIMIZATION_LAZY
1692          if (tc->optimization_state == TC_OPTIM_AT_MAX) {
1693              hit_peaks++;
1694          }
1695          if (hit_peaks > TP_OPTIMIZATION_CUTOFF) {
1696              return TP_ERR_OK;
1697          }
1698  #endif
1699  
1700      }
1701      tp_debug_print("Reached optimization depth limit\n");
1702      return TP_ERR_OK;
1703  }
1704  
1705  
1706  /**
1707   * Check for tangency between the current segment and previous segment.
1708   * If the current and previous segment are tangent, then flag the previous
1709   * segment as tangent, and limit the current segment's velocity by the sampling
1710   * rate.
1711   */
1712  STATIC int tpSetupTangent(TP_STRUCT const * const tp,
1713          TC_STRUCT * const prev_tc, TC_STRUCT * const tc) {
1714      if (!tc || !prev_tc) {
1715          tp_debug_print("missing tc or prev tc in tangent check\n");
1716          return TP_ERR_FAIL;
1717      }
1718      //If we have ABCUVW movement, then don't check for tangency
1719      if (tcRotaryMotionCheck(tc) || tcRotaryMotionCheck(prev_tc)) {
1720          tp_debug_print("found rotary axis motion\n");
1721          return TP_ERR_FAIL;
1722      }
1723  
1724      if (emcmotConfig->arcBlendOptDepth < 2) {
1725          tp_debug_print("Optimization depth %d too low for tangent optimization\n",
1726                  emcmotConfig->arcBlendOptDepth);
1727          return TP_ERR_FAIL;
1728      }
1729  
1730      if (prev_tc->term_cond == TC_TERM_COND_STOP) {
1731          tp_debug_print("Found exact stop condition\n");
1732          return TP_ERR_FAIL;
1733      }
1734  
1735      PmCartesian prev_tan, this_tan;
1736  
1737      int res_endtan = tcGetEndTangentUnitVector(prev_tc, &prev_tan);
1738      int res_starttan = tcGetStartTangentUnitVector(tc, &this_tan);
1739      if (res_endtan || res_starttan) {
1740          tp_debug_print("Got %d and %d from tangent vector calc\n",
1741                  res_endtan, res_starttan);
1742      }
1743  
1744      tp_debug_print("prev tangent vector: %f %f %f\n", prev_tan.x, prev_tan.y, prev_tan.z);
1745      tp_debug_print("this tangent vector: %f %f %f\n", this_tan.x, this_tan.y, this_tan.z);
1746  
1747      // Assume small angle approximation here
1748      const double SHARP_CORNER_DEG = 2.0;
1749      const double SHARP_CORNER_EPSILON = pmSq(PM_PI * ( SHARP_CORNER_DEG / 180.0));
1750      if (pmCartCartAntiParallel(&prev_tan, &this_tan, SHARP_CORNER_EPSILON))
1751      {
1752          tp_debug_print("Found sharp corner\n");
1753          tcSetTermCond(prev_tc, tc, TC_TERM_COND_STOP);
1754          return TP_ERR_FAIL;
1755      }
1756  
1757      // Calculate instantaneous acceleration required for change in direction
1758      // from v1 to v2, assuming constant speed
1759      double v_max1 = tcGetMaxTargetVel(prev_tc, getMaxFeedScale(prev_tc));
1760      double v_max2 = tcGetMaxTargetVel(tc, getMaxFeedScale(tc));
1761      // Note that this is a minimum since the velocity at the intersection must
1762      // be the slower of the two segments not to violate constraints.
1763      double v_max = fmin(v_max1, v_max2);
1764      tp_debug_print("tangent v_max = %f\n",v_max);
1765  
1766      // Account for acceleration past final velocity during a split cycle
1767      // (e.g. next segment starts accelerating again so the average velocity is higher at the end of the split cycle)
1768      double a_inst = v_max / tp->cycleTime + tc->maxaccel;
1769      // Set up worst-case final velocity
1770      // Compute the actual magnitude of acceleration required given the tangent directions
1771      // Do this by assuming that we decelerate to a stop on the previous segment,
1772      // and simultaneously accelerate up to the maximum speed on the next one.
1773      PmCartesian acc1, acc2, acc_diff;
1774      pmCartScalMult(&prev_tan, a_inst, &acc1);
1775      pmCartScalMult(&this_tan, a_inst, &acc2);
1776      pmCartCartSub(&acc2,&acc1,&acc_diff);
1777  
1778      //TODO store this in TP struct instead?
1779      PmCartesian acc_bound;
1780      tpGetMachineAccelBounds(&acc_bound);
1781  
1782      PmCartesian acc_scale;
1783      findAccelScale(&acc_diff,&acc_bound,&acc_scale);
1784      tp_debug_print("acc_diff: %f %f %f\n",
1785              acc_diff.x,
1786              acc_diff.y,
1787              acc_diff.z);
1788      tp_debug_print("acc_scale: %f %f %f\n",
1789              acc_scale.x,
1790              acc_scale.y,
1791              acc_scale.z);
1792  
1793      //FIXME this ratio is arbitrary, should be more easily tunable
1794      double acc_scale_max = pmCartAbsMax(&acc_scale);
1795      //KLUDGE lumping a few calculations together here
1796      if (prev_tc->motion_type == TC_CIRCULAR || tc->motion_type == TC_CIRCULAR) {
1797          acc_scale_max /= BLEND_ACC_RATIO_TANGENTIAL;
1798      }
1799  
1800      // Controls the tradeoff between reduction of final velocity, and reduction of allowed segment acceleration
1801      // TODO: this should ideally depend on some function of segment length and acceleration for better optimization
1802      const double kink_ratio = tpGetTangentKinkRatio();
1803  
1804      if (acc_scale_max < kink_ratio) {
1805          tp_debug_print(" Kink acceleration within %g, using tangent blend\n", kink_ratio);
1806          tcSetTermCond(prev_tc, tc, TC_TERM_COND_TANGENT);
1807          tcSetKinkProperties(prev_tc, tc, v_max, acc_scale_max);
1808          return TP_ERR_OK;
1809      } else {
1810          tcSetKinkProperties(prev_tc, tc, v_max * kink_ratio / acc_scale_max, kink_ratio);
1811          tp_debug_print("Kink acceleration scale %f above %f, kink vel = %f, blend arc may be faster\n",
1812                  acc_scale_max,
1813                  kink_ratio,
1814                  prev_tc->kink_vel);
1815          // NOTE: acceleration will be reduced later if tangent blend is used
1816          return TP_ERR_NO_ACTION;
1817      }
1818  }
1819  
1820  static bool tpCreateBlendIfPossible(
1821          TP_STRUCT *tp,
1822          TC_STRUCT *prev_tc,
1823          TC_STRUCT *tc,
1824          TC_STRUCT *blend_tc)
1825  {
1826      tp_err_t res_create = TP_ERR_FAIL;
1827      blend_type_t blend_requested = tpCheckBlendArcType(prev_tc, tc);
1828  
1829      switch (blend_requested) {
1830          case BLEND_LINE_LINE:
1831              res_create = tpCreateLineLineBlend(tp, prev_tc, tc, blend_tc);
1832              break;
1833          case BLEND_LINE_ARC:
1834              res_create = tpCreateLineArcBlend(tp, prev_tc, tc, blend_tc);
1835              break;
1836          case BLEND_ARC_LINE:
1837              res_create = tpCreateArcLineBlend(tp, prev_tc, tc, blend_tc);
1838              break;
1839          case BLEND_ARC_ARC:
1840              res_create = tpCreateArcArcBlend(tp, prev_tc, tc, blend_tc);
1841              break;
1842          case BLEND_NONE:
1843          default:
1844              tp_debug_print("intersection type not recognized, aborting arc\n");
1845              res_create = TP_ERR_FAIL;
1846              break;
1847      }
1848  
1849      return res_create == TP_ERR_OK;
1850  }
1851  
1852  
1853  /**
1854   * Handle creating a blend arc when a new line segment is about to enter the queue.
1855   * This function handles the checks, setup, and calculations for creating a new
1856   * blend arc. Essentially all of the blend arc functions are called through
1857   * here to isolate the process.
1858   */
1859  STATIC tc_blend_type_t tpHandleBlendArc(TP_STRUCT * const tp, TC_STRUCT * const tc) {
1860  
1861      tp_debug_print("*****************************************\n** Handle Blend Arc **\n");
1862  
1863      TC_STRUCT *prev_tc;
1864      prev_tc = tcqLast(&tp->queue);
1865  
1866      //If the previous segment has already started, then don't create a blend
1867      //arc for the next pair.
1868      // TODO May be able to lift this restriction if we can ensure that we leave
1869      // 1 timestep's worth of distance in prev_tc
1870      if ( !prev_tc) {
1871          tp_debug_print(" queue empty\n");
1872          return NO_BLEND;
1873      }
1874      if (prev_tc->progress > prev_tc->target / 2.0) {
1875          tp_debug_print(" prev_tc progress (%f) is too large, aborting blend arc\n", prev_tc->progress);
1876          return NO_BLEND;
1877      }
1878  
1879      // Check for tangency between segments and handle any errors
1880      // TODO possibly refactor this into a macro?
1881      int res_tan = tpSetupTangent(tp, prev_tc, tc);
1882      switch (res_tan) {
1883          // Abort blend arc creation in these cases
1884          case TP_ERR_FAIL:
1885              tp_debug_print(" tpSetupTangent failed, aborting blend arc\n");
1886          case TP_ERR_OK:
1887              return res_tan;
1888          case TP_ERR_NO_ACTION:
1889          default:
1890              //Continue with creation
1891              break;
1892      }
1893  
1894      TC_STRUCT blend_tc = {0};
1895  
1896      tc_blend_type_t blend_used = NO_BLEND;
1897  
1898      bool arc_blend_ok = tpCreateBlendIfPossible(tp, prev_tc, tc, &blend_tc);
1899  
1900      if (arc_blend_ok) {
1901          //Need to do this here since the length changed
1902          blend_used = ARC_BLEND;
1903          tpAddSegmentToQueue(tp, &blend_tc, false);
1904      } else {
1905          // If blend arc creation failed early on, catch it here and find the best blend
1906          blend_used = tpChooseBestBlend(tp, prev_tc, tc, NULL) ;
1907      }
1908  
1909      return blend_used;
1910  }
1911  
1912  //TODO final setup steps as separate functions
1913  //
1914  /**
1915   * Add a straight line to the tc queue.
1916   * end of the previous move to the new end specified here at the
1917   * currently-active accel and vel settings from the tp struct.
1918   */
1919  int tpAddLine(TP_STRUCT * const tp, EmcPose end, int canon_motion_type, double vel, double
1920          ini_maxvel, double acc, unsigned char enables, char atspeed, int indexer_jnum) {
1921  
1922      if (tpErrorCheck(tp) < 0) {
1923          return TP_ERR_FAIL;
1924      }
1925      tp_info_print("== AddLine ==\n");
1926  
1927      // Initialize new tc struct for the line segment
1928      TC_STRUCT tc = {0};
1929      tcInit(&tc,
1930              TC_LINEAR,
1931              canon_motion_type,
1932              tp->cycleTime,
1933              enables,
1934              atspeed);
1935  
1936      // Setup any synced IO for this move
1937      tpSetupSyncedIO(tp, &tc);
1938  
1939      // Copy over state data from the trajectory planner
1940      tcSetupState(&tc, tp);
1941  
1942      // Copy in motion parameters
1943      tcSetupMotion(&tc,
1944              vel,
1945              ini_maxvel,
1946              acc);
1947  
1948      // Setup line geometry
1949      pmLine9Init(&tc.coords.line,
1950              &tp->goalPos,
1951              &end);
1952      tc.target = pmLine9Target(&tc.coords.line);
1953      if (tc.target < TP_POS_EPSILON) {
1954          rtapi_print_msg(RTAPI_MSG_DBG,"failed to create line id %d, zero-length segment\n",tp->nextId);
1955          return TP_ERR_ZERO_LENGTH;
1956      }
1957      tc.nominal_length = tc.target;
1958      tcClampVelocityByLength(&tc);
1959  
1960      // For linear move, set joint corresponding to a locking indexer axis
1961      tc.indexer_jnum = indexer_jnum;
1962  
1963      //TODO refactor this into its own function
1964      TC_STRUCT *prev_tc;
1965      prev_tc = tcqLast(&tp->queue);
1966      tpCheckCanonType(prev_tc, &tc);
1967      if (emcmotConfig->arcBlendEnable){
1968          tpHandleBlendArc(tp, &tc);
1969      }
1970      tcFinalizeLength(prev_tc);
1971      tcFlagEarlyStop(prev_tc, &tc);
1972  
1973      int retval = tpAddSegmentToQueue(tp, &tc, true);
1974      //Run speed optimization (will abort safely if there are no tangent segments)
1975      tpRunOptimization(tp);
1976  
1977      return retval;
1978  }
1979  
1980  
1981  /**
1982   * Adds a circular (circle, arc, helix) move from the end of the
1983   * last move to this new position.
1984   *
1985   * @param end is the xyz/abc point of the destination.
1986   *
1987   * see pmCircleInit for further details on how arcs are specified. Note that
1988   * degenerate arcs/circles are not allowed. We are guaranteed to have a move in
1989   * xyz so the target is always the circle/arc/helical length.
1990   */
1991  int tpAddCircle(TP_STRUCT * const tp,
1992          EmcPose end,
1993          PmCartesian center,
1994          PmCartesian normal,
1995          int turn,
1996          int canon_motion_type,
1997          double vel,
1998          double ini_maxvel,
1999          double acc,
2000          unsigned char enables,
2001          char atspeed)
2002  {
2003      if (tpErrorCheck(tp)<0) {
2004          return TP_ERR_FAIL;
2005      }
2006  
2007      tp_info_print("== AddCircle ==\n");
2008      tp_debug_print("ini_maxvel = %f\n",ini_maxvel);
2009  
2010      TC_STRUCT tc = {0};
2011  
2012      tcInit(&tc,
2013              TC_CIRCULAR,
2014              canon_motion_type,
2015              tp->cycleTime,
2016              enables,
2017              atspeed);
2018      // Setup any synced IO for this move
2019      tpSetupSyncedIO(tp, &tc);
2020  
2021      // Copy over state data from the trajectory planner
2022      tcSetupState(&tc, tp);
2023  
2024      // Setup circle geometry
2025      int res_init = pmCircle9Init(&tc.coords.circle,
2026              &tp->goalPos,
2027              &end,
2028              &center,
2029              &normal,
2030              turn);
2031  
2032      if (res_init) return res_init;
2033  
2034      // Update tc target with existing circular segment
2035      tc.target = pmCircle9Target(&tc.coords.circle);
2036      if (tc.target < TP_POS_EPSILON) {
2037          return TP_ERR_ZERO_LENGTH;
2038      }
2039      tp_debug_print("tc.target = %f\n",tc.target);
2040      tc.nominal_length = tc.target;
2041  
2042      // Copy in motion parameters
2043      tcSetupMotion(&tc,
2044              vel,
2045              ini_maxvel,
2046              acc);
2047  
2048      //Reduce max velocity to match sample rate
2049      tcClampVelocityByLength(&tc);
2050  
2051      TC_STRUCT *prev_tc;
2052      prev_tc = tcqLast(&tp->queue);
2053  
2054      tpCheckCanonType(prev_tc, &tc);
2055      if (emcmotConfig->arcBlendEnable){
2056          tpHandleBlendArc(tp, &tc);
2057          findSpiralArcLengthFit(&tc.coords.circle.xyz, &tc.coords.circle.fit);
2058      }
2059      tcFinalizeLength(prev_tc);
2060      tcFlagEarlyStop(prev_tc, &tc);
2061  
2062      int retval = tpAddSegmentToQueue(tp, &tc, true);
2063  
2064      tpRunOptimization(tp);
2065      return retval;
2066  }
2067  
2068  
2069  /**
2070   * Adjusts blend velocity and acceleration to safe limits.
2071   * If we are blending between tc and nexttc, then we need to figure out what a
2072   * safe blend velocity is based on the known trajectory parameters. This
2073   * function updates the TC_STRUCT data with a safe blend velocity.
2074   *
2075   * @note This function will compute the parabolic blend start / end velocities
2076   * regardless of the current terminal condition (useful for planning).
2077   */
2078  STATIC int tpComputeBlendVelocity(
2079          TC_STRUCT const *tc,
2080          TC_STRUCT const *nexttc,
2081          double target_vel_this,
2082          double target_vel_next,
2083          double *v_blend_this,
2084          double *v_blend_next,
2085          double *v_blend_net)
2086  {
2087      /* Pre-checks for valid pointers */
2088      if (!nexttc || !tc || !v_blend_this || !v_blend_next ) {
2089          return TP_ERR_FAIL;
2090      }
2091  
2092      double acc_this = tcGetTangentialMaxAccel(tc);
2093      double acc_next = tcGetTangentialMaxAccel(nexttc);
2094  
2095      double v_reachable_this = fmin(tpCalculateTriangleVel(tc), target_vel_this);
2096      double v_reachable_next = fmin(tpCalculateTriangleVel(nexttc), target_vel_next);
2097  
2098      /* Compute the maximum allowed blend time for each segment.
2099       * This corresponds to the minimum acceleration that will just barely reach
2100       * max velocity as we are 1/2 done the segment.
2101       */
2102  
2103      double t_max_this = tc->target / v_reachable_this;
2104      double t_max_next = nexttc->target / v_reachable_next;
2105      double t_max_reachable = fmin(t_max_this, t_max_next);
2106  
2107      // How long the blend phase would be at maximum acceleration
2108      double t_min_blend_this = v_reachable_this / acc_this;
2109      double t_min_blend_next = v_reachable_next / acc_next;
2110  
2111      double t_max_blend = fmax(t_min_blend_this, t_min_blend_next);
2112      // The longest blend time we can get that's still within the 1/2 segment restriction
2113      double t_blend = fmin(t_max_reachable, t_max_blend);
2114  
2115      // Now, use this blend time to find the best acceleration / velocity for each segment
2116      *v_blend_this = fmin(v_reachable_this, t_blend * acc_this);
2117      *v_blend_next = fmin(v_reachable_next, t_blend * acc_next);
2118  
2119      double theta;
2120  
2121      PmCartesian v1, v2;
2122  
2123      tcGetEndAccelUnitVector(tc, &v1);
2124      tcGetStartAccelUnitVector(nexttc, &v2);
2125      findIntersectionAngle(&v1, &v2, &theta);
2126  
2127      double cos_theta = cos(theta);
2128  
2129      if (tc->tolerance > 0) {
2130          /* see diagram blend.fig.  T (blend tolerance) is given, theta
2131           * is calculated from dot(s1, s2)
2132           *
2133           * blend criteria: we are decelerating at the end of segment s1
2134           * and we pass distance d from the end.
2135           * find the corresponding velocity v when passing d.
2136           *
2137           * in the drawing note d = 2T/cos(theta)
2138           *
2139           * when v1 is decelerating at a to stop, v = at, t = v/a
2140           * so required d = .5 a (v/a)^2
2141           *
2142           * equate the two expressions for d and solve for v
2143           */
2144          double tblend_vel;
2145          /* Minimum value of cos(theta) to prevent numerical instability */
2146          const double min_cos_theta = cos(PM_PI / 2.0 - TP_MIN_ARC_ANGLE);
2147          if (cos_theta > min_cos_theta) {
2148              tblend_vel = 2.0 * pmSqrt(acc_this * tc->tolerance / cos_theta);
2149              *v_blend_this = fmin(*v_blend_this, tblend_vel);
2150              *v_blend_next = fmin(*v_blend_next, tblend_vel);
2151          }
2152      }
2153      if (v_blend_net) {
2154          /*
2155           * Find net velocity in the direction tangent to the blend.
2156           * When theta ~ 0, net velocity in tangent direction is very small.
2157           * When the segments are nearly tangent (theta ~ pi/2), the blend
2158           * velocity is almost entirely in the tangent direction.
2159           */
2160          *v_blend_net = sin(theta) * (*v_blend_this + *v_blend_next) / 2.0;
2161      }
2162  
2163      return TP_ERR_OK;
2164  }
2165  
2166  STATIC double estimateParabolicBlendPerformance(
2167          TP_STRUCT const *tp,
2168          TC_STRUCT const *tc,
2169          TC_STRUCT const *nexttc)
2170  {
2171      double v_this = 0.0, v_next = 0.0;
2172  
2173      // Use maximum possible target velocity to get best-case performance
2174      double target_vel_this = tpGetMaxTargetVel(tp, tc);
2175      double target_vel_next = tpGetMaxTargetVel(tp, nexttc);
2176  
2177      double v_net = 0.0;
2178      tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, &v_net);
2179  
2180      return v_net;
2181  }
2182  
2183  
2184  
2185  /**
2186   * Calculate distance update from velocity and acceleration.
2187   */
2188  STATIC int tcUpdateDistFromAccel(TC_STRUCT * const tc, double acc, double vel_desired, int reverse_run)
2189  {
2190      // If the resulting velocity is less than zero, than we're done. This
2191      // causes a small overshoot, but in practice it is very small.
2192      double v_next = tc->currentvel + acc * tc->cycle_time;
2193      // update position in this tc using trapezoidal integration
2194      // Note that progress can be greater than the target after this step.
2195      if (v_next < 0.0) {
2196          v_next = 0.0;
2197          //KLUDGE: the trapezoidal planner undershoots by half a cycle time, so
2198          //forcing the endpoint here is necessary. However, velocity undershoot
2199          //also occurs during pausing and stopping, which can happen far from
2200          //the end. If we could "cruise" to the endpoint within a cycle at our
2201          //current speed, then assume that we want to be at the end.
2202          if (tcGetDistanceToGo(tc,reverse_run) < (tc->currentvel *  tc->cycle_time)) {
2203              tc->progress = tcGetTarget(tc,reverse_run);
2204          }
2205      } else {
2206          double displacement = (v_next + tc->currentvel) * 0.5 * tc->cycle_time;
2207          // Account for reverse run (flip sign if need be)
2208          double disp_sign = reverse_run ? -1 : 1;
2209          tc->progress += (disp_sign * displacement);
2210  
2211          //Progress has to be within the allowable range
2212          tc->progress = bisaturate(tc->progress, tcGetTarget(tc, TC_DIR_FORWARD), tcGetTarget(tc, TC_DIR_REVERSE));
2213      }
2214      tc->currentvel = v_next;
2215  
2216      // Check if we can make the desired velocity
2217      tc->on_final_decel = (fabs(vel_desired - tc->currentvel) < TP_VEL_EPSILON) && (acc < 0.0);
2218  
2219      return TP_ERR_OK;
2220  }
2221  
2222  STATIC void tpDebugCycleInfo(TP_STRUCT const * const tp, TC_STRUCT const * const tc, TC_STRUCT const * const nexttc, double acc) {
2223  #ifdef TC_DEBUG
2224      // Find maximum allowed velocity from feed and machine limits
2225      double tc_target_vel = tpGetRealTargetVel(tp, tc);
2226      // Store a copy of final velocity
2227      double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc);
2228  
2229      /* Debug Output */
2230      tc_debug_print("tc state: vr = %f, vf = %f, maxvel = %f\n",
2231              tc_target_vel, tc_finalvel, tc->maxvel);
2232      tc_debug_print("          currentvel = %f, fs = %f, tc = %f, term = %d\n",
2233              tc->currentvel, tpGetFeedScale(tp,tc), tc->cycle_time, tc->term_cond);
2234      tc_debug_print("          acc = %f, T = %f, DTG = %.12g\n", acc,
2235              tcGetTarget(tc,tp->reverse_run), tcGetDistanceToGo(tc,tp->reverse_run));
2236      tc_debug_print("          reverse_run = %d\n", tp->reverse_run);
2237      tc_debug_print("          motion type %d\n", tc->motion_type);
2238  
2239      if (tc->on_final_decel) {
2240          rtapi_print(" on final decel\n");
2241      }
2242  #endif
2243  }
2244  
2245  /**
2246   * Compute updated position and velocity for a timestep based on a trapezoidal
2247   * motion profile.
2248   * @param tc trajectory segment being processed.
2249   *
2250   * Creates the trapezoidal velocity profile based on the segment's velocity and
2251   * acceleration limits. The formula has been tweaked slightly to allow a
2252   * non-zero velocity at the instant the target is reached.
2253   */
2254  void tpCalculateTrapezoidalAccel(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc,
2255          double * const acc, double * const vel_desired)
2256  {
2257      tc_debug_print("using trapezoidal acceleration\n");
2258  
2259      // Find maximum allowed velocity from feed and machine limits
2260      double tc_target_vel = tpGetRealTargetVel(tp, tc);
2261      // Store a copy of final velocity
2262      double tc_finalvel = tpGetRealFinalVel(tp, tc, nexttc);
2263  
2264  #ifdef TP_PEDANTIC
2265      if (tc_finalvel > 0.0 && tc->term_cond != TC_TERM_COND_TANGENT) {
2266          rtapi_print_msg(RTAPI_MSG_ERR, "Final velocity of %f with non-tangent segment!\n",tc_finalvel);
2267          tc_finalvel = 0.0;
2268      }
2269  #endif
2270  
2271      /* Calculations for desired velocity based on trapezoidal profile */
2272      double dx = tcGetDistanceToGo(tc, tp->reverse_run);
2273      double maxaccel = tcGetTangentialMaxAccel(tc);
2274  
2275      double discr_term1 = pmSq(tc_finalvel);
2276      double discr_term2 = maxaccel * (2.0 * dx - tc->currentvel * tc->cycle_time);
2277      double tmp_adt = maxaccel * tc->cycle_time * 0.5;
2278      double discr_term3 = pmSq(tmp_adt);
2279  
2280      double discr = discr_term1 + discr_term2 + discr_term3;
2281  
2282      // Descriminant is a little more complicated with final velocity term. If
2283      // descriminant < 0, we've overshot (or are about to). Do the best we can
2284      // in this situation
2285  #ifdef TP_PEDANTIC
2286      if (discr < 0.0) {
2287          rtapi_print_msg(RTAPI_MSG_ERR,
2288                  "discriminant %f < 0 in velocity calculation!\n", discr);
2289      }
2290  #endif
2291      //Start with -B/2 portion of quadratic formula
2292      double maxnewvel = -tmp_adt;
2293  
2294      //If the discriminant term brings our velocity above zero, add it to the total
2295      //We can ignore the calculation otherwise because negative velocities are clipped to zero
2296      if (discr > discr_term3) {
2297          maxnewvel += pmSqrt(discr);
2298      }
2299  
2300      // Find bounded new velocity based on target velocity
2301      // Note that we use a separate variable later to check if we're on final decel
2302      double newvel = saturate(maxnewvel, tc_target_vel);
2303  
2304      // Calculate acceleration needed to reach newvel, bounded by machine maximum
2305      double dt = fmax(tc->cycle_time, TP_TIME_EPSILON);
2306      double maxnewaccel = (newvel - tc->currentvel) / dt;
2307      *acc = saturate(maxnewaccel, maxaccel);
2308      *vel_desired = maxnewvel;
2309  }
2310  
2311  /**
2312   * Calculate "ramp" acceleration for a cycle.
2313   */
2314  STATIC int tpCalculateRampAccel(TP_STRUCT const * const tp,
2315          TC_STRUCT * const tc,
2316          TC_STRUCT const * const nexttc,
2317          double * const acc,
2318          double * const vel_desired)
2319  {
2320      tc_debug_print("using ramped acceleration\n");
2321      // displacement remaining in this segment
2322      double dx = tcGetDistanceToGo(tc, tp->reverse_run);
2323  
2324      if (!tc->blending_next) {
2325          tc->vel_at_blend_start = tc->currentvel;
2326      }
2327  
2328      double vel_final = tpGetRealFinalVel(tp, tc, nexttc);
2329  
2330      /* Check if the final velocity is too low to properly ramp up.*/
2331      if (vel_final < TP_VEL_EPSILON) {
2332          tp_debug_print(" vel_final %f too low for velocity ramping\n", vel_final);
2333          return TP_ERR_FAIL;
2334      }
2335  
2336      double vel_avg = (tc->currentvel + vel_final) / 2.0;
2337  
2338      // Calculate time remaining in this segment assuming constant acceleration
2339      double dt = 1e-16;
2340      if (vel_avg > TP_VEL_EPSILON) {
2341          dt = fmax( dx / vel_avg, 1e-16);
2342      }
2343  
2344      // Calculate velocity change between final and current velocity
2345      double dv = vel_final - tc->currentvel;
2346  
2347      // Estimate constant acceleration required
2348      double acc_final = dv / dt;
2349  
2350      // Saturate estimated acceleration against maximum allowed by segment
2351      double acc_max = tcGetTangentialMaxAccel(tc);
2352  
2353      // Output acceleration and velocity for position update
2354      *acc = saturate(acc_final, acc_max);
2355      *vel_desired = vel_final;
2356  
2357      return TP_ERR_OK;
2358  }
2359  
2360  void tpToggleDIOs(TC_STRUCT * const tc) {
2361  
2362      int i=0;
2363      if (tc->syncdio.anychanged != 0) { // we have DIO's to turn on or off
2364          for (i=0; i < emcmotConfig->numDIO; i++) {
2365              if (!(tc->syncdio.dio_mask & (1 << i))) continue;
2366              if (tc->syncdio.dios[i] > 0) emcmotDioWrite(i, 1); // turn DIO[i] on
2367              if (tc->syncdio.dios[i] < 0) emcmotDioWrite(i, 0); // turn DIO[i] off
2368          }
2369          for (i=0; i < emcmotConfig->numAIO; i++) {
2370              if (!(tc->syncdio.aio_mask & (1 << i))) continue;
2371              emcmotAioWrite(i, tc->syncdio.aios[i]); // set AIO[i]
2372          }
2373          tc->syncdio.anychanged = 0; //we have turned them all on/off, nothing else to do for this TC the next time
2374      }
2375  }
2376  
2377  
2378  /**
2379   * Handle special cases for rigid tapping.
2380   * This function deals with updating the goal position and spindle position
2381   * during a rigid tap cycle. In particular, the target and spindle goal need to
2382   * be carefully handled since we're reversing direction.
2383   */
2384  STATIC void tpUpdateRigidTapState(TP_STRUCT const * const tp,
2385          TC_STRUCT * const tc) {
2386  
2387      static double old_spindlepos;
2388      double new_spindlepos = emcmotStatus->spindle_status[tp->spindle.spindle_num].spindleRevs;
2389      if (emcmotStatus->spindle_status[tp->spindle.spindle_num].direction < 0)
2390      	new_spindlepos = -new_spindlepos;
2391  
2392      switch (tc->coords.rigidtap.state) {
2393          case TAPPING:
2394              tc_debug_print("TAPPING\n");
2395              if (tc->progress >= tc->coords.rigidtap.reversal_target) {
2396                  // command reversal
2397              	emcmotStatus->spindle_status[tp->spindle.spindle_num].speed *= -1.0 * tc->coords.rigidtap.reversal_scale;
2398                  tc->coords.rigidtap.state = REVERSING;
2399              }
2400              break;
2401          case REVERSING:
2402              tc_debug_print("REVERSING\n");
2403              if (new_spindlepos < old_spindlepos) {
2404                  PmCartesian start, end;
2405                  PmCartLine *aux = &tc->coords.rigidtap.aux_xyz;
2406                  // we've stopped, so set a new target at the original position
2407                  tc->coords.rigidtap.spindlerevs_at_reversal = new_spindlepos + tp->spindle.offset;
2408  
2409                  pmCartLinePoint(&tc->coords.rigidtap.xyz, tc->progress, &start);
2410                  end = tc->coords.rigidtap.xyz.start;
2411                  pmCartLineInit(aux, &start, &end);
2412                  rtapi_print_msg(RTAPI_MSG_DBG, "old target = %f", tc->target);
2413                  tc->coords.rigidtap.reversal_target = aux->tmag;
2414                  tc->target = aux->tmag + 10. * tc->uu_per_rev;
2415                  tc->progress = 0.0;
2416                  rtapi_print_msg(RTAPI_MSG_DBG, "new target = %f", tc->target);
2417  
2418                  tc->coords.rigidtap.state = RETRACTION;
2419              }
2420              old_spindlepos = new_spindlepos;
2421              tc_debug_print("Spindlepos = %f\n", new_spindlepos);
2422              break;
2423          case RETRACTION:
2424              tc_debug_print("RETRACTION\n");
2425              if (tc->progress >= tc->coords.rigidtap.reversal_target) {
2426              	emcmotStatus->spindle_status[tp->spindle.spindle_num].speed *= -1 / tc->coords.rigidtap.reversal_scale;
2427                  tc->coords.rigidtap.state = FINAL_REVERSAL;
2428              }
2429              break;
2430          case FINAL_REVERSAL:
2431              tc_debug_print("FINAL_REVERSAL\n");
2432              if (new_spindlepos > old_spindlepos) {
2433                  PmCartesian start, end;
2434                  PmCartLine *aux = &tc->coords.rigidtap.aux_xyz;
2435                  pmCartLinePoint(aux, tc->progress, &start);
2436                  end = tc->coords.rigidtap.xyz.start;
2437                  pmCartLineInit(aux, &start, &end);
2438                  tc->target = aux->tmag;
2439                  tc->progress = 0.0;
2440                  //No longer need spindle sync at this point
2441                  tc->synchronized = 0;
2442                  tc->target_vel = tc->maxvel;
2443  
2444                  tc->coords.rigidtap.state = FINAL_PLACEMENT;
2445              }
2446              old_spindlepos = new_spindlepos;
2447              break;
2448          case FINAL_PLACEMENT:
2449              tc_debug_print("FINAL_PLACEMENT\n");
2450              // this is a regular move now, it'll stop at target above.
2451              break;
2452      }
2453  }
2454  
2455  
2456  /**
2457   * Update emcMotStatus with information about trajectory motion.
2458   * Based on the specified trajectory segment tc, read its progress and status
2459   * flags. Then, update the emcmotStatus structure with this information.
2460   */
2461  STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const tc ) {
2462  
2463  
2464      if (!tp) {
2465          return TP_ERR_FAIL;
2466      }
2467  
2468      if (!tc) {
2469          // Assume that we have no active segment, so we should clear out the status fields
2470          emcmotStatus->distance_to_go = 0;
2471          emcmotStatus->enables_queued = emcmotStatus->enables_new;
2472          emcmotStatus->requested_vel = 0;
2473          emcmotStatus->current_vel = 0;
2474          emcPoseZero(&emcmotStatus->dtg);
2475  
2476          tp->motionType = 0;
2477          tp->activeDepth = 0;
2478          return TP_ERR_STOPPED;
2479      }
2480  
2481      EmcPose tc_pos;
2482      tcGetEndpoint(tc, &tc_pos);
2483  
2484      tc_debug_print("tc id = %u canon_type = %u mot type = %u\n",
2485              tc->id, tc->canon_motion_type, tc->motion_type);
2486      tp->motionType = tc->canon_motion_type;
2487      tp->activeDepth = tc->active_depth;
2488      emcmotStatus->distance_to_go = tc->target - tc->progress;
2489      emcmotStatus->enables_queued = tc->enables;
2490      // report our line number to the guis
2491      tp->execId = tc->id;
2492      emcmotStatus->requested_vel = tc->reqvel;
2493      emcmotStatus->current_vel = tc->currentvel;
2494  
2495      emcPoseSub(&tc_pos, &tp->currentPos, &emcmotStatus->dtg);
2496      return TP_ERR_OK;
2497  }
2498  
2499  
2500  /**
2501   * Do a parabolic blend by updating the nexttc.
2502   * Perform the actual blending process by updating the target velocity for the
2503   * next segment, then running a cycle update.
2504   */
2505  STATIC void tpUpdateBlend(TP_STRUCT * const tp, TC_STRUCT * const tc,
2506          TC_STRUCT * const nexttc) {
2507  
2508      if (!nexttc) {
2509          return;
2510      }
2511      double save_vel = nexttc->target_vel;
2512  
2513      if (tpGetFeedScale(tp, nexttc) > TP_VEL_EPSILON) {
2514          double dv = tc->vel_at_blend_start - tc->currentvel;
2515          double vel_start = fmax(tc->vel_at_blend_start, TP_VEL_EPSILON);
2516          // Clip the ratio at 1 and 0
2517          double blend_progress = fmax(fmin(dv / vel_start, 1.0), 0.0);
2518          double blend_scale = tc->vel_at_blend_start / tc->blend_vel;
2519          nexttc->target_vel = blend_progress * nexttc->blend_vel * blend_scale;
2520          // Mark the segment as blending so we handle the new target velocity properly
2521          nexttc->is_blending = true;
2522      } else {
2523          // Drive the target velocity to zero since we're stopping
2524          nexttc->target_vel = 0.0;
2525      }
2526  
2527      tpUpdateCycle(tp, nexttc, NULL);
2528      //Restore the original target velocity
2529      nexttc->target_vel = save_vel;
2530  }
2531  
2532  
2533  /**
2534   * Cleanup if tc is not valid (empty queue).
2535   * If the program ends, or we hit QUEUE STARVATION, do a soft reset on the trajectory planner.
2536   * TODO merge with tpClear?
2537   */
2538  STATIC void tpHandleEmptyQueue(TP_STRUCT * const tp)
2539  {
2540  
2541      tcqInit(&tp->queue);
2542      tp->goalPos = tp->currentPos;
2543      tp->done = 1;
2544      tp->depth = tp->activeDepth = 0;
2545      tp->aborting = 0;
2546      tp->execId = 0;
2547      tp->motionType = 0;
2548  
2549      tpUpdateMovementStatus(tp, NULL);
2550  
2551      tpResume(tp);
2552  }
2553  
2554  /** Wrapper function to unlock rotary axes */
2555  STATIC void tpSetRotaryUnlock(int axis, int unlock) {
2556      emcmotSetRotaryUnlock(axis, unlock);
2557  }
2558  
2559  /** Wrapper function to check rotary axis lock */
2560  STATIC int tpGetRotaryIsUnlocked(int axis) {
2561      return emcmotGetRotaryIsUnlocked(axis);
2562  }
2563  
2564  
2565  /**
2566   * Cleanup after a trajectory segment is complete.
2567   * If the current move is complete and we're not waiting on the spindle for
2568   * const this move, then pop if off the queue and perform cleanup operations.
2569   * Finally, get the next move in the queue.
2570   */
2571  STATIC int tpCompleteSegment(TP_STRUCT * const tp,
2572          TC_STRUCT * const tc) {
2573  
2574      if (tp->spindle.waiting_for_atspeed == tc->id) {
2575          return TP_ERR_FAIL;
2576      }
2577  
2578      // if we're synced, and this move is ending, save the
2579      // spindle position so the next synced move can be in
2580      // the right place.
2581      if(tc->synchronized != TC_SYNC_NONE) {
2582          tp->spindle.offset += tc->target / tc->uu_per_rev;
2583      } else {
2584          tp->spindle.offset = 0.0;
2585      }
2586  
2587      if(tc->indexer_jnum != -1) {
2588          // this was an indexing move, so before we remove it we must
2589          // relock the joint for the locking indexer axis
2590          tpSetRotaryUnlock(tc->indexer_jnum, 0);
2591          // if it is now locked, fall through and remove the finished move.
2592          // otherwise, just come back later and check again
2593          if(tpGetRotaryIsUnlocked(tc->indexer_jnum)) {
2594              return TP_ERR_FAIL;
2595          }
2596      }
2597  
2598      //Clear status flags associated since segment is done
2599      //TODO stuff into helper function?
2600      tc->active = 0;
2601      tc->remove = 0;
2602      tc->is_blending = 0;
2603      tc->splitting = 0;
2604      tc->cycle_time = tp->cycleTime;
2605      //Velocities are by definition zero for a non-active segment
2606      tc->currentvel = 0.0;
2607      tc->term_vel = 0.0;
2608      //TODO make progress to match target?
2609      // done with this move
2610      if (tp->reverse_run) {
2611          tcqBackStep(&tp->queue);
2612          tp_debug_print("Finished reverse run of tc id %d\n", tc->id);
2613      } else {
2614          int res_pop = tcqPop(&tp->queue);
2615          if (res_pop) rtapi_print_msg(RTAPI_MSG_ERR,"Got error %d from tcqPop!\n", res_pop);
2616          tp_debug_print("Finished tc id %d\n", tc->id);
2617      }
2618  
2619      return TP_ERR_OK;
2620  }
2621  
2622  
2623  /**
2624   * Handle an abort command.
2625   * Based on the current motion state, handle the consequences of an abort command.
2626   */
2627  STATIC tp_err_t tpHandleAbort(TP_STRUCT * const tp, TC_STRUCT * const tc,
2628          TC_STRUCT * const nexttc) {
2629  
2630      if(!tp->aborting) {
2631          //Don't need to do anything if not aborting
2632          return TP_ERR_NO_ACTION;
2633      }
2634      //If the motion has stopped, then it's safe to reset the TP struct.
2635      if( MOTION_ID_VALID(tp->spindle.waiting_for_index) ||
2636              MOTION_ID_VALID(tp->spindle.waiting_for_atspeed) ||
2637              (tc->currentvel == 0.0 && (!nexttc || nexttc->currentvel == 0.0))) {
2638          tcqInit(&tp->queue);
2639          tp->goalPos = tp->currentPos;
2640          tp->done = 1;
2641          tp->depth = tp->activeDepth = 0;
2642          tp->aborting = 0;
2643          tp->execId = 0;
2644          tp->motionType = 0;
2645          tp->synchronized = 0;
2646          tp->reverse_run = 0;
2647          tp->spindle.waiting_for_index = MOTION_INVALID_ID;
2648          tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID;
2649          tpResume(tp);
2650          return TP_ERR_STOPPED;
2651      }  //FIXME consistent error codes
2652      return TP_ERR_SLOWING;
2653  }
2654  
2655  
2656  /**
2657   * Check if the spindle has reached the required speed for a move.
2658   * Returns a "wait" code if the spindle needs to spin up before a move and it
2659   * has not reached the requested speed, or the spindle index has not been
2660   * detected.
2661   */
2662  STATIC tp_err_t tpCheckAtSpeed(TP_STRUCT * const tp, TC_STRUCT * const tc)
2663  {
2664  	int s;
2665      // this is no longer the segment we were waiting_for_index for
2666      if (MOTION_ID_VALID(tp->spindle.waiting_for_index) && tp->spindle.waiting_for_index != tc->id)
2667      {
2668          rtapi_print_msg(RTAPI_MSG_ERR,
2669                  "Was waiting for index on motion id %d, but reached id %d\n",
2670                  tp->spindle.waiting_for_index, tc->id);
2671          tp->spindle.waiting_for_index = MOTION_INVALID_ID;
2672      }
2673  
2674      if (MOTION_ID_VALID(tp->spindle.waiting_for_atspeed) && tp->spindle.waiting_for_atspeed != tc->id)
2675      {
2676  
2677          rtapi_print_msg(RTAPI_MSG_ERR,
2678                  "Was waiting for atspeed on motion id %d, but reached id %d\n",
2679                  tp->spindle.waiting_for_atspeed, tc->id);
2680          tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID;
2681      }
2682  
2683      if (MOTION_ID_VALID(tp->spindle.waiting_for_atspeed)) {
2684          for (s = 0; s < emcmotConfig->numSpindles; s++){
2685              if(!emcmotStatus->spindle_status[s].at_speed) {
2686                  // spindle is still not at the right speed, so wait another cycle
2687                  return TP_ERR_WAITING;
2688              }
2689          }
2690          // not waiting any more
2691          tp->spindle.waiting_for_atspeed = MOTION_INVALID_ID;
2692      }
2693  
2694      if (MOTION_ID_VALID(tp->spindle.waiting_for_index)) {
2695          if (emcmotStatus->spindle_status[tp->spindle.spindle_num].spindle_index_enable) {
2696              /* haven't passed index yet */
2697              return TP_ERR_WAITING;
2698          } else {
2699              rtapi_print_msg(RTAPI_MSG_DBG, "Index seen on spindle %d\n", tp->spindle.spindle_num);
2700              /* passed index, start the move */
2701              emcmotStatus->spindleSync = 1;
2702              tp->spindle.waiting_for_index = MOTION_INVALID_ID;
2703              tc->sync_accel = 1;
2704              tp->spindle.revs = 0;
2705          }
2706      }
2707      return TP_ERR_OK;
2708  }
2709  
2710  /**
2711   * "Activate" a segment being read for the first time.
2712   * This function handles initial setup of a new segment read off of the queue
2713   * for the first time.
2714   */
2715  STATIC tp_err_t tpActivateSegment(TP_STRUCT * const tp, TC_STRUCT * const tc) {
2716  
2717      //Check if already active
2718      if (!tc || tc->active) {
2719          return TP_ERR_OK;
2720      }
2721  
2722      if (!tp) {
2723          return TP_ERR_MISSING_INPUT;
2724      }
2725  
2726      if (tp->reverse_run && (tc->motion_type == TC_RIGIDTAP || tc->synchronized != TC_SYNC_NONE)) {
2727          //Can't activate a segment with synced motion in reverse
2728          return TP_ERR_REVERSE_EMPTY;
2729      }
2730  
2731      /* Based on the INI setting for "cutoff frequency", this calculation finds
2732       * short segments that can have their acceleration be simple ramps, instead
2733       * of a trapezoidal motion. This leads to fewer jerk spikes, at a slight
2734       * performance cost.
2735       * */
2736      double cutoff_time = 1.0 / (fmax(emcmotConfig->arcBlendRampFreq, TP_TIME_EPSILON));
2737  
2738      double length = tcGetDistanceToGo(tc, tp->reverse_run);
2739      // Given what velocities we can actually reach, estimate the total time for the segment under ramp conditions
2740      double segment_time = 2.0 * length / (tc->currentvel + fmin(tc->finalvel,tpGetRealTargetVel(tp,tc)));
2741  
2742  
2743      if (segment_time < cutoff_time &&
2744              tc->canon_motion_type != EMC_MOTION_TYPE_TRAVERSE &&
2745              tc->term_cond == TC_TERM_COND_TANGENT &&
2746              tc->motion_type != TC_RIGIDTAP &&
2747              length != 0)
2748      {
2749          tp_debug_print("segment_time = %f, cutoff_time = %f, ramping\n",
2750                  segment_time, cutoff_time);
2751          tc->accel_mode = TC_ACCEL_RAMP;
2752      }
2753  
2754      // Do at speed checks that only happen once
2755      int needs_atspeed = tc->atspeed ||
2756          (tc->synchronized == TC_SYNC_POSITION && !(emcmotStatus->spindleSync));
2757  
2758      if (needs_atspeed){
2759          int s;
2760          for (s = 0; s < emcmotConfig->numSpindles; s++){
2761              if (!emcmotStatus->spindle_status[s].at_speed) {
2762                  tp->spindle.waiting_for_atspeed = tc->id;
2763                  return TP_ERR_WAITING;
2764              }
2765          }
2766      }
2767  
2768      if (tc->indexer_jnum != -1) {
2769          // request that the joint for the locking indexer axis unlock
2770          tpSetRotaryUnlock(tc->indexer_jnum, 1);
2771          // if it is unlocked, fall through and start the move.
2772          // otherwise, just come back later and check again
2773          if (!tpGetRotaryIsUnlocked(tc->indexer_jnum))
2774              return TP_ERR_WAITING;
2775      }
2776  
2777      // Temporary debug message
2778      tp_debug_print("Activate tc id = %d target_vel = %f req_vel = %f final_vel = %f length = %f\n",
2779              tc->id,
2780              tc->target_vel,
2781              tc->reqvel,
2782              tc->finalvel,
2783              tc->target);
2784  
2785      tc->active = 1;
2786      //Do not change initial velocity here, since tangent blending already sets this up
2787      tp->motionType = tc->canon_motion_type;
2788      tc->blending_next = 0;
2789      tc->on_final_decel = 0;
2790  
2791      if (TC_SYNC_POSITION == tc->synchronized && !(emcmotStatus->spindleSync)) {
2792          tp_debug_print("Setting up position sync\n");
2793          // if we aren't already synced, wait
2794          tp->spindle.waiting_for_index = tc->id;
2795          // ask for an index reset
2796          emcmotStatus->spindle_status[tp->spindle.spindle_num].spindle_index_enable = 1;
2797          tp->spindle.offset = 0.0;
2798          rtapi_print_msg(RTAPI_MSG_DBG, "Waiting on sync. spindle_num %d..\n", tp->spindle.spindle_num);
2799          return TP_ERR_WAITING;
2800      }
2801  
2802      return TP_ERR_OK;
2803  }
2804  
2805  
2806  /**
2807   * Run velocity mode synchronization.
2808   * Update requested velocity to follow the spindle's velocity (scaled by feed rate).
2809   */
2810  STATIC void tpSyncVelocityMode(TP_STRUCT * const tp, TC_STRUCT * const tc, TC_STRUCT * const nexttc) {
2811      double speed = emcmotStatus->spindle_status[tp->spindle.spindle_num].spindleSpeedIn;
2812      double pos_error = fabs(speed) * tc->uu_per_rev;
2813      // Account for movement due to parabolic blending with next segment
2814      if(nexttc) {
2815          pos_error -= nexttc->progress;
2816      }
2817      tc->target_vel = pos_error;
2818  
2819      if (nexttc && nexttc->synchronized) {
2820          //If the next move is synchronized too, then match it's
2821          //requested velocity to the current move
2822          nexttc->target_vel = tc->target_vel;
2823      }
2824  }
2825  
2826  
2827  /**
2828   * Run position mode synchronization.
2829   * Updates requested velocity for a trajectory segment to track the spindle's position.
2830   */
2831  STATIC void tpSyncPositionMode(TP_STRUCT * const tp, TC_STRUCT * const tc,
2832          TC_STRUCT * const nexttc ) {
2833  
2834      double spindle_pos = tpGetSignedSpindlePosition(&emcmotStatus->spindle_status[tp->spindle.spindle_num]);
2835      tp_debug_print("Spindle at %f\n",spindle_pos);
2836      double spindle_vel, target_vel;
2837      double oldrevs = tp->spindle.revs;
2838  
2839      if ((tc->motion_type == TC_RIGIDTAP) && (tc->coords.rigidtap.state == RETRACTION ||
2840                  tc->coords.rigidtap.state == FINAL_REVERSAL)) {
2841              tp->spindle.revs = tc->coords.rigidtap.spindlerevs_at_reversal -
2842                  spindle_pos;
2843      } else {
2844          tp->spindle.revs = spindle_pos;
2845      }
2846  
2847      double pos_desired = (tp->spindle.revs - tp->spindle.offset) * tc->uu_per_rev;
2848      double pos_error = pos_desired - tc->progress;
2849  
2850      if(nexttc) {
2851          pos_error -= nexttc->progress;
2852      }
2853  
2854      if(tc->sync_accel) {
2855          // detect when velocities match, and move the target accordingly.
2856          // acceleration will abruptly stop and we will be on our new target.
2857          // FIX: this is driven by TP cycle time, not the segment cycle time
2858          double dt = fmax(tp->cycleTime, TP_TIME_EPSILON);
2859          spindle_vel = tp->spindle.revs / ( dt * tc->sync_accel++);
2860          target_vel = spindle_vel * tc->uu_per_rev;
2861          if(tc->currentvel >= target_vel) {
2862              tc_debug_print("Hit accel target in pos sync\n");
2863              // move target so as to drive pos_error to 0 next cycle
2864              tp->spindle.offset = tp->spindle.revs - tc->progress / tc->uu_per_rev;
2865              tc->sync_accel = 0;
2866              tc->target_vel = target_vel;
2867          } else {
2868              tc_debug_print("accelerating in pos_sync\n");
2869              // beginning of move and we are behind: accel as fast as we can
2870              tc->target_vel = tc->maxvel;
2871          }
2872      } else {
2873          // we have synced the beginning of the move as best we can -
2874          // track position (minimize pos_error).
2875          tc_debug_print("tracking in pos_sync\n");
2876          double errorvel;
2877          spindle_vel = (tp->spindle.revs - oldrevs) / tp->cycleTime;
2878          target_vel = spindle_vel * tc->uu_per_rev;
2879          errorvel = pmSqrt(fabs(pos_error) * tcGetTangentialMaxAccel(tc));
2880          if(pos_error<0) {
2881              errorvel *= -1.0;
2882          }
2883          tc->target_vel = target_vel + errorvel;
2884      }
2885  
2886      //Finally, clip requested velocity at zero
2887      if (tc->target_vel < 0.0) {
2888          tc->target_vel = 0.0;
2889      }
2890  
2891      if (nexttc && nexttc->synchronized) {
2892          //If the next move is synchronized too, then match it's
2893          //requested velocity to the current move
2894          nexttc->target_vel = tc->target_vel;
2895      }
2896  }
2897  
2898  
2899  /**
2900   * Perform parabolic blending if needed between segments and handle status updates.
2901   * This isolates most of the parabolic blend stuff to make the code path
2902   * between tangent and parabolic blends easier to follow.
2903   */
2904  STATIC int tpDoParabolicBlending(TP_STRUCT * const tp, TC_STRUCT * const tc,
2905          TC_STRUCT * const nexttc) {
2906  
2907      tc_debug_print("in DoParabolicBlend\n");
2908      tpUpdateBlend(tp,tc,nexttc);
2909  
2910      /* Status updates */
2911      //Decide which segment we're in depending on which is moving faster
2912      if(tc->currentvel > nexttc->currentvel) {
2913          tpUpdateMovementStatus(tp, tc);
2914      } else {
2915          tpToggleDIOs(nexttc);
2916          tpUpdateMovementStatus(tp, nexttc);
2917      }
2918  #ifdef TP_SHOW_BLENDS
2919      // hack to show blends in axis
2920      tp->motionType = 0;
2921  #endif
2922  
2923      //Update velocity status based on both tc and nexttc
2924      emcmotStatus->current_vel = tc->currentvel + nexttc->currentvel;
2925  
2926      return TP_ERR_OK;
2927  }
2928  
2929  
2930  /**
2931   * Do a complete update on one segment.
2932   * Handles the majority of updates on a single segment for the current cycle.
2933   */
2934  STATIC int tpUpdateCycle(TP_STRUCT * const tp,
2935          TC_STRUCT * const tc, TC_STRUCT const * const nexttc) {
2936  
2937      //placeholders for position for this update
2938      EmcPose before;
2939  
2940      //Store the current position due to this TC
2941      tcGetPos(tc, &before);
2942  
2943      // Update the start velocity if we're not blending yet
2944      if (!tc->blending_next) {
2945          tc->vel_at_blend_start = tc->currentvel;
2946      }
2947  
2948      // Run cycle update with stored cycle time
2949      int res_accel = 1;
2950      double acc=0, vel_desired=0;
2951      
2952      // If the slowdown is not too great, use velocity ramping instead of trapezoidal velocity
2953      // Also, don't ramp up for parabolic blends
2954      if (tc->accel_mode && tc->term_cond == TC_TERM_COND_TANGENT) {
2955          res_accel = tpCalculateRampAccel(tp, tc, nexttc, &acc, &vel_desired);
2956      }
2957  
2958      // Check the return in case the ramp calculation failed, fall back to trapezoidal
2959      if (res_accel != TP_ERR_OK) {
2960          tpCalculateTrapezoidalAccel(tp, tc, nexttc, &acc, &vel_desired);
2961      }
2962  
2963      tcUpdateDistFromAccel(tc, acc, vel_desired, tp->reverse_run);
2964      tpDebugCycleInfo(tp, tc, nexttc, acc);
2965  
2966      //Check if we're near the end of the cycle and set appropriate changes
2967      tpCheckEndCondition(tp, tc, nexttc);
2968  
2969      EmcPose displacement;
2970  
2971      // Calculate displacement
2972      tcGetPos(tc, &displacement);
2973      emcPoseSelfSub(&displacement, &before);
2974  
2975      //Store displacement (checking for valid pose)
2976      int res_set = tpAddCurrentPos(tp, &displacement);
2977  
2978  #ifdef TC_DEBUG
2979      double mag;
2980      emcPoseMagnitude(&displacement, &mag);
2981      tc_debug_print("cycle movement = %f\n", mag);
2982  #endif
2983  
2984      return res_set;
2985  }
2986  
2987  
2988  /**
2989   * Send default values to status structure.
2990   */
2991  STATIC int tpUpdateInitialStatus(TP_STRUCT const * const tp) {
2992      // Update queue length
2993      emcmotStatus->tcqlen = tcqLen(&tp->queue);
2994      // Set default value for requested speed
2995      emcmotStatus->requested_vel = 0.0;
2996      //FIXME test if we can do this safely
2997      emcmotStatus->current_vel = 0.0;
2998      return TP_ERR_OK;
2999  }
3000  
3001  
3002  /**
3003   * Flag a segment as needing a split cycle.
3004   * In addition to flagging a segment as splitting, do any preparations to store
3005   * data for the next cycle.
3006   */
3007  STATIC inline int tcSetSplitCycle(TC_STRUCT * const tc, double split_time,
3008          double v_f)
3009  {
3010      tp_debug_print("split time for id %d is %.16g\n", tc->id, split_time);
3011      if (tc->splitting != 0 && split_time > 0.0) {
3012          rtapi_print_msg(RTAPI_MSG_ERR,"already splitting on id %d with cycle time %.16g, dx = %.16g, split time %.12g\n",
3013                  tc->id,
3014                  tc->cycle_time,
3015                  tc->target-tc->progress,
3016                  split_time);
3017          return TP_ERR_FAIL;
3018      }
3019      tc->splitting = 1;
3020      tc->cycle_time = split_time;
3021      tc->term_vel = v_f;
3022      return 0;
3023  }
3024  
3025  
3026  /**
3027   * Check remaining time in a segment and calculate split cycle if necessary.
3028   * This function estimates how much time we need to complete the next segment.
3029   * If it's greater than one timestep, then we do nothing and carry on. If not,
3030   * then we flag the segment as "splitting", so that during the next cycle,
3031   * it handles the transition to the next segment.
3032   */
3033  STATIC int tpCheckEndCondition(TP_STRUCT const * const tp, TC_STRUCT * const tc, TC_STRUCT const * const nexttc) {
3034  
3035      //Assume no split time unless we find otherwise
3036      tc->cycle_time = tp->cycleTime;
3037      //Initial guess at dt for next round
3038      double dx = tcGetDistanceToGo(tc, tp->reverse_run);
3039      tc_debug_print("tpCheckEndCondition: dx = %e\n",dx);
3040  
3041      if (dx <= TP_POS_EPSILON) {
3042          //If the segment is close to the target position, then we assume that it's done.
3043          tp_debug_print("close to target, dx = %.12f\n",dx);
3044          //Force progress to land exactly on the target to prevent numerical errors.
3045          tc->progress = tcGetTarget(tc, tp->reverse_run);
3046  
3047          if (!tp->reverse_run) {
3048              tcSetSplitCycle(tc, 0.0, tc->currentvel);
3049          }
3050          if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT || tp->reverse_run) {
3051              tc->remove = 1;
3052          }
3053          return TP_ERR_OK;
3054      } else if (tp->reverse_run) {
3055          return TP_ERR_NO_ACTION;
3056      } else if (tc->term_cond == TC_TERM_COND_STOP || tc->term_cond == TC_TERM_COND_EXACT) {
3057          return TP_ERR_NO_ACTION;
3058      }
3059  
3060  
3061      double v_f = tpGetRealFinalVel(tp, tc, nexttc);
3062      double v_avg = (tc->currentvel + v_f) / 2.0;
3063  
3064      //Check that we have a non-zero "average" velocity between now and the
3065      //finish. If not, it means that we have to accelerate from a stop, which
3066      //will take longer than the minimum 2 timesteps that each segment takes, so
3067      //we're safely far form the end.
3068  
3069      //Get dt assuming that we can magically reach the final velocity at
3070      //the end of the move.
3071      //
3072      //KLUDGE: start with a value below the cutoff
3073      double dt = TP_TIME_EPSILON / 2.0;
3074      if (v_avg > TP_VEL_EPSILON) {
3075          //Get dt from distance and velocity (avoid div by zero)
3076          dt = fmax(dt, dx / v_avg);
3077      } else {
3078          if ( dx > (v_avg * tp->cycleTime) && dx > TP_POS_EPSILON) {
3079              tc_debug_print(" below velocity threshold, assuming far from end\n");
3080              return TP_ERR_NO_ACTION;
3081          }
3082      }
3083  
3084      //Calculate the acceleration this would take:
3085  
3086      double dv = v_f - tc->currentvel;
3087      double a_f = dv / dt;
3088  
3089      //If this is a valid acceleration, then we're done. If not, then we solve
3090      //for v_f and dt given the max acceleration allowed.
3091      double a_max = tcGetTangentialMaxAccel(tc);
3092  
3093      //If we exceed the maximum acceleration, then the dt estimate is too small.
3094      double a = a_f;
3095      int recalc = sat_inplace(&a, a_max);
3096  
3097      //Need to recalculate vf and above
3098      if (recalc) {
3099          tc_debug_print(" recalculating with a_f = %f, a = %f\n", a_f, a);
3100          double disc = pmSq(tc->currentvel / a) + 2.0 / a * dx;
3101          if (disc < 0) {
3102              //Should mean that dx is too big, i.e. we're not close enough
3103              tc_debug_print(" dx = %f, too large, not at end yet\n",dx);
3104              return TP_ERR_NO_ACTION;
3105          }
3106  
3107          if (disc < TP_TIME_EPSILON * TP_TIME_EPSILON) {
3108              tc_debug_print("disc too small, skipping sqrt\n");
3109              dt =  -tc->currentvel / a;
3110          } else if (a > 0) {
3111              tc_debug_print("using positive sqrt\n");
3112              dt = -tc->currentvel / a + pmSqrt(disc);
3113          } else {
3114              tc_debug_print("using negative sqrt\n");
3115              dt = -tc->currentvel / a - pmSqrt(disc);
3116          }
3117  
3118          tc_debug_print(" revised dt = %f\n", dt);
3119          //Update final velocity with actual result
3120          v_f = tc->currentvel + dt * a;
3121      }
3122  
3123      if (dt < TP_TIME_EPSILON) {
3124          //Close enough, call it done
3125          tc_debug_print("revised dt small, finishing tc\n");
3126          tc->progress = tcGetTarget(tc, tp->reverse_run);
3127          tcSetSplitCycle(tc, 0.0, v_f);
3128      } else if (dt < tp->cycleTime ) {
3129          tc_debug_print(" corrected v_f = %f, a = %f\n", v_f, a);
3130          tcSetSplitCycle(tc, dt, v_f);
3131      } else {
3132          tc_debug_print(" dt = %f, not at end yet\n",dt);
3133          return TP_ERR_NO_ACTION;
3134      }
3135      return TP_ERR_OK;
3136  }
3137  
3138  
3139  STATIC int tpHandleSplitCycle(TP_STRUCT * const tp, TC_STRUCT * const tc,
3140          TC_STRUCT * const nexttc)
3141  {
3142      if (tc->remove) {
3143          //Don't need to update since this segment is flagged for removal
3144          return TP_ERR_NO_ACTION;
3145      }
3146  
3147      //Pose data to calculate movement due to finishing current TC
3148      EmcPose before;
3149      tcGetPos(tc, &before);
3150  
3151      tp_debug_print("tc id %d splitting\n",tc->id);
3152      //Shortcut tc update by assuming we arrive at end
3153      tc->progress = tcGetTarget(tc,tp->reverse_run);
3154      //Get displacement from prev. position
3155      EmcPose displacement;
3156      tcGetPos(tc, &displacement);
3157      emcPoseSelfSub(&displacement, &before);
3158  
3159      // Update tp's position (checking for valid pose)
3160      tpAddCurrentPos(tp, &displacement);
3161  
3162  #ifdef TC_DEBUG
3163      double mag;
3164      emcPoseMagnitude(&displacement, &mag);
3165      tc_debug_print("cycle movement = %f\n",mag);
3166  #endif
3167  
3168      // Trigger removal of current segment at the end of the cycle
3169      tc->remove = 1;
3170  
3171      if (!nexttc) {
3172          tp_debug_print("no nexttc in split cycle\n");
3173          return TP_ERR_OK;
3174      }
3175  
3176      switch (tc->term_cond) {
3177          case TC_TERM_COND_TANGENT:
3178              nexttc->cycle_time = tp->cycleTime - tc->cycle_time;
3179              nexttc->currentvel = tc->term_vel;
3180              tp_debug_print("Doing tangent split\n");
3181              break;
3182          case TC_TERM_COND_PARABOLIC:
3183              break;
3184          case TC_TERM_COND_STOP:
3185              break;
3186          case TC_TERM_COND_EXACT:
3187              break;
3188          default:
3189              rtapi_print_msg(RTAPI_MSG_ERR,"unknown term cond %d in segment %d\n",
3190                      tc->term_cond,
3191                      tc->id);
3192      }
3193  
3194      // Run split cycle update with remaining time in nexttc
3195      // KLUDGE: use next cycle after nextc to prevent velocity dip (functions fail gracefully w/ NULL)
3196      int queue_dir_step = tp->reverse_run ? -1 : 1;
3197      TC_STRUCT *next2tc = tcqItem(&tp->queue, queue_dir_step*2);
3198  
3199      tpUpdateCycle(tp, nexttc, next2tc);
3200  
3201      // Update status for the split portion
3202      // FIXME redundant tangent check, refactor to switch
3203      if (tc->cycle_time > nexttc->cycle_time && tc->term_cond == TC_TERM_COND_TANGENT) {
3204          //Majority of time spent in current segment
3205          tpToggleDIOs(tc);
3206          tpUpdateMovementStatus(tp, tc);
3207      } else {
3208          tpToggleDIOs(nexttc);
3209          tpUpdateMovementStatus(tp, nexttc);
3210      }
3211  
3212      return TP_ERR_OK;
3213  }
3214  
3215  STATIC int tpHandleRegularCycle(TP_STRUCT * const tp,
3216          TC_STRUCT * const tc,
3217          TC_STRUCT * const nexttc)
3218  {
3219      if (tc->remove) {
3220          //Don't need to update since this segment is flagged for removal
3221          return TP_ERR_NO_ACTION;
3222      }
3223      //Run with full cycle time
3224      tc_debug_print("Normal cycle\n");
3225      tc->cycle_time = tp->cycleTime;
3226      tpUpdateCycle(tp, tc, nexttc);
3227  
3228      /* Parabolic blending */
3229  
3230      double v_this = 0.0, v_next = 0.0;
3231  
3232      // cap the blend velocity at the current requested speed (factoring in feed override)
3233      double target_vel_this = tpGetRealTargetVel(tp, tc);
3234      double target_vel_next = tpGetRealTargetVel(tp, nexttc);
3235  
3236      tpComputeBlendVelocity(tc, nexttc, target_vel_this, target_vel_next, &v_this, &v_next, NULL);
3237      tc->blend_vel = v_this;
3238      if (nexttc) {
3239          nexttc->blend_vel = v_next;
3240      }
3241  
3242      if (nexttc && tcIsBlending(tc)) {
3243          tpDoParabolicBlending(tp, tc, nexttc);
3244      } else {
3245          //Update status for a normal step
3246          tpToggleDIOs(tc);
3247          tpUpdateMovementStatus(tp, tc);
3248      }
3249      return TP_ERR_OK;
3250  }
3251  
3252  
3253  /**
3254   * Calculate an updated goal position for the next timestep.
3255   * This is the brains of the operation. It's called every TRAJ period and is
3256   * expected to set tp->currentPos to the new machine position. Lots of other
3257   * const tp fields (depth, done, etc) have to be twiddled to communicate the
3258   * status; I think those are spelled out here correctly and I can't clean it up
3259   * without breaking the API that the TP presents to motion.
3260   */
3261  int tpRunCycle(TP_STRUCT * const tp, long period)
3262  {
3263      //Pointers to current and next trajectory component
3264      TC_STRUCT *tc;
3265      TC_STRUCT *nexttc;
3266  
3267      /* Get pointers to current and relevant future segments. It's ok here if
3268       * future segments don't exist (NULL pointers) as we check for this later).
3269       */
3270  
3271      int queue_dir_step = tp->reverse_run ? -1 : 1;
3272      tc = tcqItem(&tp->queue, 0);
3273      nexttc = tcqItem(&tp->queue, queue_dir_step * 1);
3274  
3275      //Set GUI status to "zero" state
3276      tpUpdateInitialStatus(tp);
3277  
3278  #ifdef TC_DEBUG
3279      //Hack debug output for timesteps
3280      // NOTE: need to track every timestep, even those where the trajectory planner is idle
3281      static double time_elapsed = 0;
3282      time_elapsed+=tp->cycleTime;
3283  #endif
3284  
3285      //If we have a NULL pointer, then the queue must be empty, so we're done.
3286      if(!tc) {
3287          tpHandleEmptyQueue(tp);
3288          return TP_ERR_WAITING;
3289      }
3290  
3291      tc_debug_print("-------------------\n");
3292  
3293  
3294      /* If the queue empties enough, assume that the program is near the end.
3295       * This forces the last segment to be "finalized" to let the optimizer run.*/
3296      /*tpHandleLowQueue(tp);*/
3297  
3298      /* If we're aborting or pausing and the velocity has reached zero, then we
3299       * don't need additional planning and can abort here. */
3300      if (tpHandleAbort(tp, tc, nexttc) == TP_ERR_STOPPED) {
3301          return TP_ERR_STOPPED;
3302      }
3303  
3304      //Return early if we have a reason to wait (i.e. not ready for motion)
3305      if (tpCheckAtSpeed(tp, tc) != TP_ERR_OK){
3306          return TP_ERR_WAITING;
3307      }
3308  
3309      int res_activate = tpActivateSegment(tp, tc);
3310      if (res_activate != TP_ERR_OK ) {
3311          return res_activate;
3312      }
3313  
3314      // Preprocess rigid tap move (handles threading direction reversals)
3315      if (tc->motion_type == TC_RIGIDTAP) {
3316          tpUpdateRigidTapState(tp, tc);
3317      }
3318  
3319      /** If synchronized with spindle, calculate requested velocity to track
3320       * spindle motion.*/
3321      switch (tc->synchronized) {
3322          case TC_SYNC_NONE:
3323              emcmotStatus->spindleSync = 0;
3324              break;
3325          case TC_SYNC_VELOCITY:
3326              tp_debug_print("sync velocity\n");
3327              tpSyncVelocityMode(tp, tc, nexttc);
3328              break;
3329          case TC_SYNC_POSITION:
3330              tp_debug_print("sync position\n");
3331              tpSyncPositionMode(tp, tc, nexttc);
3332              break;
3333          default:
3334              tp_debug_print("unrecognized spindle sync state!\n");
3335              break;
3336      }
3337  
3338  #ifdef TC_DEBUG
3339      EmcPose pos_before = tp->currentPos;
3340  #endif
3341  
3342  
3343      tcClearFlags(tc);
3344      tcClearFlags(nexttc);
3345      // Update the current tc
3346      if (tc->splitting) {
3347          tpHandleSplitCycle(tp, tc, nexttc);
3348      } else {
3349          tpHandleRegularCycle(tp, tc, nexttc);
3350      }
3351  
3352  #ifdef TC_DEBUG
3353      double mag;
3354      EmcPose disp;
3355      emcPoseSub(&tp->currentPos, &pos_before, &disp);
3356      emcPoseMagnitude(&disp, &mag);
3357      tc_debug_print("time: %.12e total movement = %.12e vel = %.12e\n",
3358              time_elapsed,
3359              mag, emcmotStatus->current_vel);
3360  
3361      tc_debug_print("tp_displacement = %.12e %.12e %.12e time = %.12e\n",
3362              disp.tran.x,
3363              disp.tran.y,
3364              disp.tran.z,
3365              time_elapsed);
3366  #endif
3367  
3368      // If TC is complete, remove it from the queue.
3369      if (tc->remove) {
3370          tpCompleteSegment(tp, tc);
3371      }
3372  
3373      return TP_ERR_OK;
3374  }
3375  
3376  int tpSetSpindleSync(TP_STRUCT * const tp, int spindle, double sync, int mode) {
3377      if(sync) {
3378          if (mode) {
3379              tp->synchronized = TC_SYNC_VELOCITY;
3380          } else {
3381              tp->synchronized = TC_SYNC_POSITION;
3382          }
3383          tp->uu_per_rev = sync;
3384          tp->spindle.spindle_num = spindle;
3385      } else
3386          tp->synchronized = 0;
3387  
3388      return TP_ERR_OK;
3389  }
3390  
3391  int tpPause(TP_STRUCT * const tp)
3392  {
3393      if (0 == tp) {
3394          return TP_ERR_FAIL;
3395      }
3396      tp->pausing = 1;
3397      return TP_ERR_OK;
3398  }
3399  
3400  int tpResume(TP_STRUCT * const tp)
3401  {
3402      if (0 == tp) {
3403          return TP_ERR_FAIL;
3404      }
3405      tp->pausing = 0;
3406      return TP_ERR_OK;
3407  }
3408  
3409  int tpAbort(TP_STRUCT * const tp)
3410  {
3411      if (0 == tp) {
3412          return TP_ERR_FAIL;
3413      }
3414  
3415      if (!tp->aborting) {
3416          /* const to abort, signal a pause and set our abort flag */
3417          tpPause(tp);
3418          tp->aborting = 1;
3419      }
3420      return tpClearDIOs(tp); //clears out any already cached DIOs
3421  }
3422  
3423  int tpGetMotionType(TP_STRUCT * const tp)
3424  {
3425      return tp->motionType;
3426  }
3427  
3428  int tpGetPos(TP_STRUCT const * const tp, EmcPose * const pos)
3429  {
3430  
3431      if (0 == tp) {
3432          ZERO_EMC_POSE((*pos));
3433          return TP_ERR_FAIL;
3434      } else {
3435          *pos = tp->currentPos;
3436      }
3437  
3438      return TP_ERR_OK;
3439  }
3440  
3441  int tpIsDone(TP_STRUCT * const tp)
3442  {
3443      if (0 == tp) {
3444          return TP_ERR_OK;
3445      }
3446  
3447      return tp->done;
3448  }
3449  
3450  int tpQueueDepth(TP_STRUCT * const tp)
3451  {
3452      if (0 == tp) {
3453          return TP_ERR_OK;
3454      }
3455  
3456      return tp->depth;
3457  }
3458  
3459  int tpActiveDepth(TP_STRUCT * const tp)
3460  {
3461      if (0 == tp) {
3462          return TP_ERR_OK;
3463      }
3464  
3465      return tp->activeDepth;
3466  }
3467  
3468  int tpSetAout(TP_STRUCT * const tp, unsigned char index, double start, double end) {
3469      if (0 == tp) {
3470          return TP_ERR_FAIL;
3471      }
3472      tp->syncdio.anychanged = 1; //something has changed
3473      tp->syncdio.aio_mask |= (1 << index);
3474      tp->syncdio.aios[index] = start;
3475      return TP_ERR_OK;
3476  }
3477  
3478  int tpSetDout(TP_STRUCT * const tp, int index, unsigned char start, unsigned char end) {
3479      if (0 == tp) {
3480          return TP_ERR_FAIL;
3481      }
3482      tp->syncdio.anychanged = 1; //something has changed
3483      tp->syncdio.dio_mask |= (1 << index);
3484      if (start > 0)
3485          tp->syncdio.dios[index] = 1; // the end value can't be set from canon currently, and has the same value as start
3486      else
3487          tp->syncdio.dios[index] = -1;
3488      return TP_ERR_OK;
3489  }
3490  
3491  int tpSetRunDir(TP_STRUCT * const tp, tc_direction_t dir)
3492  {
3493      // Can't change direction while moving
3494      if (tpIsMoving(tp)) {
3495          return TP_ERR_FAIL;
3496      }
3497  
3498      switch (dir) {
3499          case TC_DIR_FORWARD:
3500          case TC_DIR_REVERSE:
3501              tp->reverse_run = dir;
3502              return TP_ERR_OK;
3503          default:
3504              rtapi_print_msg(RTAPI_MSG_ERR,"Invalid direction flag in SetRunDir");
3505              return TP_ERR_FAIL;
3506      }
3507  }
3508  
3509  int tpIsMoving(TP_STRUCT const * const tp)
3510  {
3511  
3512      //TODO may be better to explicitly check velocities on the first 2 segments, but this is messy
3513      if (emcmotStatus->current_vel >= TP_VEL_EPSILON ) {
3514          tp_debug_print("TP moving, current_vel = %.16g\n", emcmotStatus->current_vel);
3515          return true;
3516      } else if (tp->spindle.waiting_for_index != MOTION_INVALID_ID || tp->spindle.waiting_for_atspeed != MOTION_INVALID_ID) {
3517          tp_debug_print("TP moving, waiting for index or atspeed\n");
3518          return true;
3519      }
3520      return false;
3521  }
3522  
3523  
3524  // vim:sw=4:sts=4:et: