emcstepmot.c File Reference

00921 {
00922   int axis;                     /* loop counter */
00923 #ifdef rtlinux
00924 #ifdef RT_FIFO
00925   int err;
00926 #endif
00927 #endif
00928   int valid;
00929 #ifdef STEPPER_MOTORS
00930   double mag;                   /* temporary value for magnitudes, to
00931                                    avoid calling fabs() all the time */
00932 #endif
00933 
00934   /* copy command from outside into local buffers */
00935 
00936 #if defined (rtlinux) && defined (RT_FIFO)
00937   while ((err =
00938           rtf_get(EMCMOT_COMMAND_RTF, &emcmotCommandBuffer,
00939                   sizeof(EMCMOT_COMMAND))) == sizeof(EMCMOT_COMMAND)) {
00940 
00941 #endif /* rtlinux and RT_FIFO */
00942 
00943     /* check for split read */
00944     if (emcmotCommand->head != emcmotCommand->tail) {
00945       emcmotDebug->split++;
00946       return 0;                 /* not really an error */
00947     }
00948 
00949     if (emcmotCommand->commandNum != emcmotStatus->commandNumEcho) {
00950       /* increment head count-- we'll be modifying emcmotStatus */
00951       emcmotStatus->head++;
00952       emcmotDebug->head++;
00953 
00954       /* got a new command-- echo command and number... */
00955       emcmotStatus->commandEcho = emcmotCommand->command;
00956       emcmotStatus->commandNumEcho = emcmotCommand->commandNum;
00957 
00958       /* clear status value by default */
00959       emcmotStatus->commandStatus = EMCMOT_COMMAND_OK;
00960 
00961       /* log it, if appropriate */
00962       if (emcmotStatus->logStarted &&
00963           emcmotStatus->logType == EMCMOT_LOG_TYPE_CMD) {
00964         ls.item.cmd.time = etime();
00965         ls.item.cmd.command = emcmotCommand->command;
00966         ls.item.cmd.commandNum = emcmotCommand->commandNum;
00967 #if defined (rtlinux) && defined (RT_FIFO)
00968         rtf_put(EMCMOT_LOG_RTF, &ls, sizeof(EMCMOT_LOG_STRUCT));
00969 #else  /* not rtlinux or not RT_FIFO */
00970         emcmotLogAdd(emcmotLog, ls);
00971         emcmotStatus->logPoints = emcmotLog->howmany;
00972 #endif /* else not rtlinux nor RT_FIFO */
00973       }
00974 
00975       /* ...and process command */
00976       switch (emcmotCommand->command) {
00977       case EMCMOT_FREE:
00978         /* change the mode to free axis motion */
00979         /* can be done at any time */
00980         /* reset the coordinating flag to defer transition to
00981            controller cycle */
00982         coordinating = 0;
00983         break;
00984 
00985       case EMCMOT_COORD:
00986         /* change the mode to coordinated axis motion */
00987         /* can be done at any time */
00988         /* set the coordinating flag to defer transition to
00989            controller cycle */
00990         coordinating = 1;
00991         break;
00992 
00993       case EMCMOT_SET_NUM_AXES:
00994         MARK_EMCMOT_CONFIG_CHANGE();
00995         /* set the global NUM_AXES, which must be between 1 and
00996            EMCMOT_MAX_AXIS, inclusive */
00997         axis = emcmotCommand->axis;
00998         /* note that this comparison differs from the check on the
00999            range of 'axis' in most other places, since those checks
01000            are for a value to be used as an index and here it's a value
01001            to be used as a counting number. The indenting is different
01002            here so as not to match macro editing on that other bunch. */
01003         if
01004           (
01005            axis <= 0 ||
01006            axis > EMCMOT_MAX_AXIS
01007            ) {
01008           break;
01009         }
01010         NUM_AXES = axis;
01011         break;
01012 
01013       case EMCMOT_SET_WORLD_HOME:
01014         worldHome = emcmotCommand->pos;
01015         break;
01016 
01017       case EMCMOT_SET_JOINT_HOME:
01018         axis = emcmotCommand->axis;
01019         if (axis < 0 ||
01020             axis >= EMCMOT_MAX_AXIS) {
01021           break;
01022         }
01023         jointHome[axis] = emcmotCommand->offset; /* FIXME-- use 'home' instead */
01024         break;
01025 
01026       case EMCMOT_SET_HOME_OFFSET:
01027         axis = emcmotCommand->axis;
01028         if (axis < 0 ||
01029             axis >= EMCMOT_MAX_AXIS) {
01030           break;
01031         }
01032         emcmotConfig->homeOffset[axis] = emcmotCommand->offset;
01033         break;
01034 
01035       case EMCMOT_OVERRIDE_LIMITS:
01036         if (emcmotCommand->axis < 0) {
01037           /* don't override limits */
01038           emcmotStatus->overrideLimits = 0;
01039         }
01040         else {
01041           emcmotStatus->overrideLimits = 1;
01042         }
01043         overriding = 0;
01044         for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01045           SET_AXIS_ERROR_FLAG(axis, 0);
01046         }
01047         break;
01048 
01049       case EMCMOT_SET_TRAJ_CYCLE_TIME:
01050         MARK_EMCMOT_CONFIG_CHANGE();
01051         /* set the cycle time for trajectory calculations */
01052         /* really should be done only at startup before
01053            controller is run, but at least it requires
01054            no active motions and the interpolators need
01055            to be cleared */
01056         setTrajCycleTime(emcmotCommand->cycleTime);
01057         break;
01058 
01059       case EMCMOT_SET_SERVO_CYCLE_TIME:
01060         MARK_EMCMOT_CONFIG_CHANGE();
01061         /* set the cycle time for servo calculations, which is the
01062            period for emcmotController execution */
01063         /* really should be done only at startup before
01064            controller is run, but at least it requires
01065            no active motions and the interpolators need
01066            to be cleared */
01067         setServoCycleTime(emcmotCommand->cycleTime);
01068 #ifdef rtlinux
01069 #ifdef USE_RTL2
01070         pthread_make_periodic_np(emcmotTask,
01071                                  gethrtime(),
01072                                  (hrtime_t)  (HRTICKS_PER_SEC *
01073                                              emcmotConfig->servoCycleTime));
01074 #else
01075         rt_task_make_periodic(&emcmotTask,
01076                               rt_get_time(),
01077                               (RTIME) (RT_TICKS_PER_SEC *
01078                                        emcmotConfig->servoCycleTime));
01079 #endif
01080 #endif /* rtlinux */
01081         break;
01082 
01083       case EMCMOT_SET_POSITION_LIMITS:
01084         MARK_EMCMOT_CONFIG_CHANGE();
01085         /* set the position limits for the axis */
01086         /* can be done at any time */
01087         axis = emcmotCommand->axis;
01088         if (axis < 0 ||
01089             axis >= EMCMOT_MAX_AXIS) {
01090           break;
01091         }
01092         emcmotConfig->minLimit[axis] = emcmotCommand->minLimit;
01093         emcmotConfig->maxLimit[axis] = emcmotCommand->maxLimit;
01094         break;
01095 
01096       case EMCMOT_SET_OUTPUT_LIMITS:
01097         MARK_EMCMOT_CONFIG_CHANGE();
01098         /* set the output limits for the axis */
01099         /* can be done at any time */
01100         axis = emcmotCommand->axis;
01101         if (axis < 0 ||
01102             axis >= EMCMOT_MAX_AXIS) {
01103           break;
01104         }
01105 
01106         emcmotConfig->minOutput[axis] = emcmotCommand->minLimit;
01107         emcmotConfig->maxOutput[axis] = emcmotCommand->maxLimit;
01108         break;
01109 
01110       case EMCMOT_SET_OUTPUT_SCALE:
01111         axis = emcmotCommand->axis;
01112         if (axis < 0 ||
01113             axis >= EMCMOT_MAX_AXIS ||
01114             emcmotCommand->scale == 0) {
01115           break;
01116         }
01117         emcmotStatus->outputScale[axis] = emcmotCommand->scale;
01118         emcmotStatus->outputOffset[axis] = emcmotCommand->offset;
01119         inverseOutputScale[axis] = 1.0 / emcmotStatus->outputScale[axis];
01120         break;
01121 
01122       case EMCMOT_SET_INPUT_SCALE:
01123         /*
01124           change the scale factor for the position input, e.g.,
01125           encoder counts per unit. Note that this is not a good idea
01126           once things have gotten underway, since the axis will
01127           jump servo to the "new" position, the gains will no longer
01128           be appropriate, etc.
01129         */
01130         axis = emcmotCommand->axis;
01131         if (axis < 0 ||
01132             axis >= EMCMOT_MAX_AXIS ||
01133             emcmotCommand->scale == 0.0) {
01134           break;
01135         }
01136 
01137         /* adjust last saved input value to match this one, so we
01138            don't get a spurious following error */
01139         oldInput[axis] = oldInput[axis] * emcmotStatus->inputScale[axis] +
01140           emcmotStatus->inputOffset[axis]; /* temp calc */
01141         oldInput[axis] = (oldInput[axis] - emcmotCommand->offset) /
01142           emcmotCommand->scale;
01143 
01144         /* now make them active */
01145         emcmotStatus->inputScale[axis] = emcmotCommand->scale;
01146         emcmotStatus->inputOffset[axis] = emcmotCommand->offset;
01147         inverseInputScale[axis] = 1.0 / emcmotStatus->inputScale[axis];
01148 
01149 #ifdef STEPPER_MOTORS
01150         /* change the units on the stepper counts; note that this
01151            is a signed value. De-sign it if you want to consider
01152            just its magnitude. */
01153         smStepsPerUnit[axis] = emcmotCommand->scale;
01154         mag = fabs(smStepsPerUnit[axis]);
01155         smMagUnitsPerStep[axis] = 1.0 / mag;
01156         /* update smVPmax so that stepper task runs at proper rate */
01157         if (mag > smMaxStepsPerUnit) {
01158           smVPmax = smVPmax * mag / smMaxStepsPerUnit;
01159           smMaxStepsPerUnit = mag;
01160         }
01161         /* set the period to twice the max pulse rate. It need only
01162            be the same as the pulse rate, but this makes it the same
01163            as before */
01164         smCycleTime = 0.5/smVPmax;
01165         smCyclesPerAxis = (int) (emcmotConfig->servoCycleTime / smCycleTime);
01166 #ifdef USE_RTL2
01167         pthread_make_periodic_np(smTask,
01168                               gethrtime(),
01169                               ((hrtime_t) (HRTICKS_PER_SEC * smCycleTime)));
01170 #else
01171         rt_task_make_periodic(&smTask,
01172                               rt_get_time(),
01173                               ((RTIME) (RT_TICKS_PER_SEC * smCycleTime)));
01174 #endif
01175 #endif
01176         break;
01177 
01178         /*
01179           Max and min ferror work like this:
01180           limiting ferror is determined by slope of ferror line,
01181           = maxFerror/limitVel
01182           -> limiting ferror = maxFerror/limitVel * vel.
01183           If ferror < minFerror then OK else
01184           if ferror < limiting ferror then OK else ERROR
01185          */
01186 
01187       case EMCMOT_SET_MAX_FERROR:
01188         MARK_EMCMOT_CONFIG_CHANGE();
01189         axis = emcmotCommand->axis;
01190         if (axis < 0 ||
01191             axis >= EMCMOT_MAX_AXIS ||
01192             emcmotCommand->maxFerror < 0.0) {
01193           break;
01194         }
01195         emcmotConfig->maxFerror[axis] = emcmotCommand->maxFerror;
01196         break;
01197 
01198       case EMCMOT_SET_MIN_FERROR:
01199         MARK_EMCMOT_CONFIG_CHANGE();
01200         axis = emcmotCommand->axis;
01201         if (axis < 0 ||
01202             axis >= EMCMOT_MAX_AXIS ||
01203             emcmotCommand->minFerror < 0.0) {
01204           break;
01205         }
01206         emcmotConfig->minFerror[axis] = emcmotCommand->minFerror;
01207         break;
01208 
01209       case EMCMOT_JOG_CONT:
01210         /* do a continuous jog, implemented as an incremental
01211            jog to the software limit, or the full range of travel
01212            if software limits don't yet apply because we're not homed */
01213 
01214         /* check axis range */
01215         axis = emcmotCommand->axis;
01216         if (axis < 0 ||
01217             axis >= EMCMOT_MAX_AXIS) {
01218           break;
01219         }
01220 
01221         /* requires no motion, in free mode, enable on */
01222         if (GET_MOTION_COORD_FLAG() ||
01223             ! GET_MOTION_INPOS_FLAG() ||
01224             ! GET_MOTION_ENABLE_FLAG()) {
01225           SET_AXIS_ERROR_FLAG(axis, 1);
01226           break;
01227         }
01228 
01229         /* don't jog further onto limits */
01230         if (! checkJog(axis, emcmotCommand->vel)) {
01231           SET_AXIS_ERROR_FLAG(axis, 1);
01232           break;
01233         }
01234 
01235         if (emcmotCommand->vel > 0.0) {
01236           if (GET_AXIS_HOMED_FLAG(axis)) {
01237             freePose.tran.x = emcmotConfig->maxLimit[axis];
01238           }
01239           else {
01240             freePose.tran.x = AXRANGE(axis);
01241           }
01242         }
01243         else {
01244           if (GET_AXIS_HOMED_FLAG(axis)) {
01245             freePose.tran.x = emcmotConfig->minLimit[axis];
01246           }
01247           else {
01248             freePose.tran.x = - AXRANGE(axis);
01249           }
01250         }
01251 
01252         tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel));
01253         tpAddLine(&freeAxis[axis], freePose);
01254         SET_AXIS_ERROR_FLAG(axis, 0);
01255 
01256         break;
01257 
01258       case EMCMOT_JOG_INCR:
01259         /* do an incremental jog */
01260 
01261         /* check axis range */
01262         axis = emcmotCommand->axis;
01263         if (axis < 0 ||
01264             axis >= EMCMOT_MAX_AXIS) {
01265           break;
01266         }
01267 
01268         /* requires no motion, in free mode, enable on */
01269         if (GET_MOTION_COORD_FLAG() ||
01270             ! GET_MOTION_INPOS_FLAG() ||
01271             ! GET_MOTION_ENABLE_FLAG()) {
01272           SET_AXIS_ERROR_FLAG(axis, 1);
01273           break;
01274         }
01275 
01276         /* don't jog further onto limits */
01277         if (! checkJog(axis, emcmotCommand->vel)) {
01278           SET_AXIS_ERROR_FLAG(axis, 1);
01279           break;
01280         }
01281 
01282         if (emcmotCommand->vel > 0.0) {
01283           freePose.tran.x = jointPos[axis] + emcmotCommand->offset; /* FIXME-- use 'goal' instead */
01284           if (GET_AXIS_HOMED_FLAG(axis)) {
01285             if (freePose.tran.x > emcmotConfig->maxLimit[axis]) {
01286               freePose.tran.x = emcmotConfig->maxLimit[axis];
01287             }
01288           }
01289         }
01290         else {
01291           freePose.tran.x = jointPos[axis] - emcmotCommand->offset; /* FIXME-- use 'goal' instead */
01292           if (GET_AXIS_HOMED_FLAG(axis)) {
01293             if (freePose.tran.x < emcmotConfig->minLimit[axis]) {
01294               freePose.tran.x = emcmotConfig->minLimit[axis];
01295             }
01296           }
01297         }
01298 
01299         tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel));
01300         tpAddLine(&freeAxis[axis], freePose);
01301         SET_AXIS_ERROR_FLAG(axis, 0);
01302 
01303         break;
01304 
01305       case EMCMOT_JOG_ABS:
01306         /* do an absolute jog */
01307 
01308         /* check axis range */
01309         axis = emcmotCommand->axis;
01310         if (axis < 0 ||
01311             axis >= EMCMOT_MAX_AXIS) {
01312           break;
01313         }
01314 
01315         /* requires no motion, in free mode, enable on */
01316         if (GET_MOTION_COORD_FLAG() ||
01317             ! GET_MOTION_INPOS_FLAG() ||
01318             ! GET_MOTION_ENABLE_FLAG()) {
01319           SET_AXIS_ERROR_FLAG(axis, 1);
01320           break;
01321         }
01322 
01323         /* don't jog further onto limits */
01324         if (! checkJog(axis, emcmotCommand->vel)) {
01325           SET_AXIS_ERROR_FLAG(axis, 1);
01326           break;
01327         }
01328 
01329         freePose.tran.x = emcmotCommand->offset; /* FIXME-- use 'goal' instead */
01330         if (GET_AXIS_HOMED_FLAG(axis)) {
01331           if (freePose.tran.x > emcmotConfig->maxLimit[axis]) {
01332             freePose.tran.x = emcmotConfig->maxLimit[axis];
01333           }
01334           else if (freePose.tran.x < emcmotConfig->minLimit[axis]) {
01335             freePose.tran.x = emcmotConfig->minLimit[axis];
01336           }
01337         }
01338 
01339         tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel));
01340         tpAddLine(&freeAxis[axis], freePose);
01341         SET_AXIS_ERROR_FLAG(axis, 0);
01342 
01343         break;
01344 
01345       case EMCMOT_SET_TERM_COND:
01346         /* sets termination condition for motion queue */
01347         tpSetTermCond(&queue, emcmotCommand->termCond);
01348         break;
01349 
01350       case EMCMOT_SET_LINE:
01351         /* queue up a linear move */
01352         /* requires coordinated mode, enable off, not on limits */
01353         if (! GET_MOTION_COORD_FLAG() ||
01354             ! GET_MOTION_ENABLE_FLAG()) {
01355           reportError("Need to be enabled, in coord mode for linear move");
01356           emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
01357           SET_MOTION_ERROR_FLAG(1);
01358           break;
01359         }
01360         else if (! inRange(emcmotCommand->pos)) {
01361           reportError("Linear move %d out of range",
01362                       emcmotCommand->id);
01363           emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01364           tpAbort(&queue);
01365           SET_MOTION_ERROR_FLAG(1);
01366           break;
01367         }
01368         else if (! checkLimits()) {
01369           reportError("Can't do linear move with limits exceeded");
01370           emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01371           tpAbort(&queue);
01372           SET_MOTION_ERROR_FLAG(1);
01373           break;
01374         }
01375 
01376         /* append it to the queue */
01377         tpSetId(&queue, emcmotCommand->id);
01378         if (-1 == tpAddLine(&queue, emcmotCommand->pos)) {
01379           reportError("Can't add linear move");
01380           emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
01381           tpAbort(&queue);
01382           SET_MOTION_ERROR_FLAG(1);
01383           break;
01384         }
01385         else {
01386           SET_MOTION_ERROR_FLAG(0);
01387         }
01388         break;
01389 
01390       case EMCMOT_SET_CIRCLE:
01391         /* queue up a circular move */
01392         /* requires coordinated mode, enable on, not on limits */
01393         if (! GET_MOTION_COORD_FLAG() ||
01394             ! GET_MOTION_ENABLE_FLAG()) {
01395           reportError("Need to be enabled, in coord mode for circular move");
01396           emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
01397           SET_MOTION_ERROR_FLAG(1);
01398           break;
01399         }
01400         else if (! inRange(emcmotCommand->pos)) {
01401           reportError("Circular move %d out of range",
01402                       emcmotCommand->id);
01403           emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01404           tpAbort(&queue);
01405           SET_MOTION_ERROR_FLAG(1);
01406           break;
01407         }
01408         else if (! checkLimits()) {
01409           reportError("Can't do circular move with limits exceeded");
01410           emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01411           tpAbort(&queue);
01412           SET_MOTION_ERROR_FLAG(1);
01413           break;
01414         }
01415 
01416         /* append it to the queue */
01417         tpSetId(&queue, emcmotCommand->id);
01418         if (-1 ==
01419             tpAddCircle(&queue, emcmotCommand->pos,
01420                         emcmotCommand->center, emcmotCommand->normal,
01421                         emcmotCommand->turn)) {
01422           reportError("Can't add circular move");
01423           emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
01424           tpAbort(&queue);
01425           SET_MOTION_ERROR_FLAG(1);
01426           break;
01427         }
01428         else {
01429           SET_MOTION_ERROR_FLAG(0);
01430         }
01431         break;
01432 
01433       case EMCMOT_SET_VEL:
01434         /* set the velocity for subsequent moves */
01435         /* can do it at any time */
01436         emcmotStatus->vel = emcmotCommand->vel;
01437         for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01438           tpSetVmax(&freeAxis[axis], emcmotStatus->vel);
01439         }
01440         tpSetVmax(&queue, emcmotStatus->vel);
01441         break;
01442 
01443 
01444       case EMCMOT_SET_AXIS_VEL_LIMIT:
01445         /* check axis range */
01446         axis = emcmotCommand->axis;
01447         if (axis < 0 ||
01448             axis >= EMCMOT_MAX_AXIS) {
01449           break;
01450         }
01451         tpSetVlimit(&freeAxis[axis], emcmotCommand->vel);
01452         emcmotConfig->axisLimitVel[axis] = emcmotCommand->vel;
01453         // bigVel[axis = 10 * emcmotCommand->vel;
01454         break;
01455         
01456       case EMCMOT_SET_VEL_LIMIT:
01457         /* set the absolute max velocity for all subsequent moves */
01458         /* can do it at any time */
01459 #ifdef STEPPER_MOTORS
01460         /* update smVPmax so that stepper task runs at proper rate,
01461            before we lose emcmotStatus->vel from overwrite by new value */
01462         smVPmax = smVPmax * emcmotCommand->vel / emcmotConfig->limitVel;
01463         /* set the period to twice the max pulse rate. It need only
01464            be the same as the pulse rate, but this makes it the same
01465            as before */
01466         smCycleTime = 0.5/smVPmax;
01467         smCyclesPerAxis = (int) (emcmotConfig->servoCycleTime / smCycleTime);
01468 #ifdef USE_RTL2
01469         pthread_make_periodic_np(smTask,
01470                               gethrtime(),
01471                               ((hrtime_t) (HRTICKS_PER_SEC * smCycleTime)));
01472 #else
01473         rt_task_make_periodic(&smTask,
01474                               rt_get_time(),
01475                               ((RTIME) (RT_TICKS_PER_SEC * smCycleTime)));
01476 #endif
01477 #endif
01478         emcmotConfig->limitVel = emcmotCommand->vel;
01479         for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01480           tpSetVlimit(&freeAxis[axis], emcmotConfig->limitVel);
01481         }
01482         tpSetVlimit(&queue, emcmotConfig->limitVel);
01483         /* set the debounce vel */
01484         bigVel = 10.0 * emcmotConfig->limitVel;
01485         break;
01486 
01487       case EMCMOT_SET_HOMING_VEL:
01488         /* set the homing velocity */
01489         /* can do it at any time */
01490         /* sign of vel should set polarity, and mag-sign are recorded */
01491 
01492         /* check axis range */
01493         axis = emcmotCommand->axis;
01494         if (axis < 0 ||
01495             axis >= EMCMOT_MAX_AXIS) {
01496           break;
01497         }
01498 
01499         if (emcmotCommand->vel < 0.0) {
01500           emcmotConfig->homingVel[axis] = - emcmotCommand->vel;
01501           SET_AXIS_HOMING_POLARITY(axis, 0);
01502         }
01503         else {
01504           emcmotConfig->homingVel[axis] = emcmotCommand->vel;
01505           SET_AXIS_HOMING_POLARITY(axis, 1);
01506         }
01507         break;
01508 
01509       case EMCMOT_SET_ACC:
01510         /* set the max acceleration */
01511         /* can do it at any time */
01512         emcmotStatus->acc = emcmotCommand->acc;
01513         for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01514           tpSetAmax(&freeAxis[axis], emcmotStatus->acc);
01515         }
01516         tpSetAmax(&queue, emcmotStatus->acc);
01517         break;
01518 
01519       case EMCMOT_PAUSE:
01520         /* pause the motion */
01521         /* can happen at any time */
01522         for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01523           tpPause(&freeAxis[axis]);
01524         }
01525         tpPause(&queue);
01526         emcmotStatus->paused = 1;
01527         break;
01528 
01529       case EMCMOT_RESUME:
01530         /* resume paused motion */
01531         /* can happen at any time */
01532         stepping = 0;
01533         for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01534           tpResume(&freeAxis[axis]);
01535         }
01536         tpResume(&queue);
01537         emcmotStatus->paused = 0;
01538         break;
01539 
01540       case EMCMOT_STEP:
01541         /* resume paused motion until id changes */
01542         /* can happen at any time */
01543         idForStep = emcmotStatus->id;
01544         stepping = 1;
01545         for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01546           tpResume(&freeAxis[axis]);
01547         }
01548         tpResume(&queue);
01549         emcmotStatus->paused = 0;
01550         break;
01551 
01552       case EMCMOT_SCALE:
01553         /* override speed */
01554         /* can happen at any time */
01555         if (emcmotCommand->scale < 0.0) {
01556           emcmotCommand->scale = 0.0; /* clamp it */
01557         }
01558         for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01559           tpSetVscale(&freeAxis[axis], emcmotCommand->scale);
01560           emcmotStatus->axVscale[axis] = emcmotCommand->scale;
01561         }
01562         tpSetVscale(&queue, emcmotCommand->scale);
01563         emcmotStatus->qVscale = emcmotCommand->scale;
01564         break;
01565 
01566       case EMCMOT_ABORT:
01567         /* abort motion */
01568         /* can happen at any time */
01569         /* check for coord or free space motion active */
01570         if (GET_MOTION_COORD_FLAG()) {
01571           tpAbort(&queue);
01572           SET_MOTION_ERROR_FLAG(0);
01573         }
01574         else {
01575           /* check axis range */
01576           axis = emcmotCommand->axis;
01577           if (axis < 0 ||
01578               axis >= EMCMOT_MAX_AXIS) {
01579             break;
01580           }
01581           tpAbort(&freeAxis[axis]);
01582           SET_AXIS_HOMING_FLAG(axis, 0);
01583           SET_AXIS_ERROR_FLAG(axis, 0);
01584         }
01585         break;
01586 
01587       case EMCMOT_DISABLE:
01588         /* go into disable */
01589         /* can happen at any time */
01590         /* reset the enabling flag to defer disable until
01591            controller cycle (it *will* be honored) */
01592         enabling = 0;
01593         break;
01594 
01595       case EMCMOT_ENABLE:
01596         /* come out of disable */
01597         /* can happen at any time */
01598         /* set the enabling flag to defer enable
01599            until controller cycle */
01600         enabling = 1;
01601         break;
01602 
01603       case EMCMOT_SET_PID:
01604         /* configure the PID gains */
01605         axis = emcmotCommand->axis;
01606         if (axis < 0 ||
01607             axis >= EMCMOT_MAX_AXIS) {
01608           break;
01609         }
01610         /* force it, instead of callind pidSetGains(), which would require
01611            that we link in pid.o, which we don't really need to do */
01612         emcmotConfig->pid[axis].p = emcmotCommand->pid.p;
01613         emcmotConfig->pid[axis].i = emcmotCommand->pid.i;
01614         emcmotConfig->pid[axis].d = emcmotCommand->pid.d;
01615         emcmotConfig->pid[axis].ff0 = emcmotCommand->pid.ff0;
01616         emcmotConfig->pid[axis].ff1 = emcmotCommand->pid.ff1;
01617         emcmotConfig->pid[axis].ff2 = emcmotCommand->pid.ff2;
01618         emcmotConfig->pid[axis].backlash = emcmotCommand->pid.backlash;
01619         emcmotConfig->pid[axis].bias = emcmotCommand->pid.bias;
01620         emcmotConfig->pid[axis].maxError = emcmotCommand->pid.maxError;
01621         emcmotConfig->pid[axis].deadband = emcmotCommand->pid.deadband;
01622         break;
01623 
01624       case EMCMOT_ACTIVATE_AXIS:
01625         /* make axis active, so that amps will be enabled when
01626            system is enabled or disabled */
01627         /* can be done at any time */
01628         axis = emcmotCommand->axis;
01629         if (axis < 0 ||
01630             axis >= EMCMOT_MAX_AXIS) {
01631           break;
01632         }
01633         SET_AXIS_ACTIVE_FLAG(axis, 1);
01634         break;
01635 
01636       case EMCMOT_DEACTIVATE_AXIS:
01637         /* make axis inactive, so that amps won't be affected when
01638            system is enabled or disabled */
01639         /* can be done at any time */
01640         axis = emcmotCommand->axis;
01641         if (axis < 0 ||
01642             axis >= EMCMOT_MAX_AXIS) {
01643           break;
01644         }
01645         SET_AXIS_ACTIVE_FLAG(axis, 0);
01646         break;
01647 
01648       case EMCMOT_ENABLE_AMPLIFIER:
01649         /* enable the amplifier directly, but don't enable calculations */
01650         /* can be done at any time */
01651         axis = emcmotCommand->axis;
01652         if (axis < 0 ||
01653             axis >= EMCMOT_MAX_AXIS) {
01654           break;
01655         }
01656         extAmpEnable(axis, GET_AXIS_ENABLE_POLARITY(axis));
01657         break;
01658 
01659       case EMCMOT_DISABLE_AMPLIFIER:
01660         /* disable the axis calculations and amplifier, but don't
01661            disable calculations */
01662         /* can be done at any time */
01663         axis = emcmotCommand->axis;
01664         if (axis < 0 ||
01665             axis >= EMCMOT_MAX_AXIS) {
01666           break;
01667         }
01668         extAmpEnable(axis, ! GET_AXIS_ENABLE_POLARITY(axis));
01669         break;
01670 
01671       case EMCMOT_OPEN_LOG:
01672         /* open a data log */
01673         axis = emcmotCommand->axis;
01674         valid = 0;
01675         if (emcmotCommand->logSize > 0 &&
01676             emcmotCommand->logSize <= EMCMOT_LOG_MAX) {
01677           /* handle log-specific data */
01678           switch (emcmotCommand->logType) {
01679           case EMCMOT_LOG_TYPE_AXIS_POS:
01680           case EMCMOT_LOG_TYPE_AXIS_VEL:
01681           case EMCMOT_LOG_TYPE_POS_VOLTAGE:
01682             if (axis >= 0 &&
01683                 axis < EMCMOT_MAX_AXIS) {
01684               loggingAxis = axis;
01685               valid = 1;
01686             }
01687             break;
01688 
01689           default:
01690             valid = 1;
01691             break;
01692           }
01693         }
01694 
01695         if (valid) {
01696           /* success */
01697 #if defined (rtlinux) && defined (RT_FIFO)
01698           if (-1 ==
01699               rtf_create(EMCMOT_LOG_RTF,
01700                          emcmotCommand->logSize *
01701                          sizeof(EMCMOT_LOG_STRUCT))) {
01702             /* error-- can't open log */
01703             break;
01704           }
01705 #else  /* not rtlinux or not RT_FIFO */
01706           emcmotLogInit(emcmotLog,
01707                         emcmotCommand->logType,
01708                         emcmotCommand->logSize);
01709 #endif /* else not rtlinux or not RT_FIFO */
01710           emcmotStatus->logOpen = 1;
01711           emcmotStatus->logStarted = 0;
01712           emcmotStatus->logSize = emcmotCommand->logSize;
01713           emcmotStatus->logSkip = emcmotCommand->logSkip;
01714           emcmotStatus->logType = emcmotCommand->logType;
01715         }
01716         break;
01717 
01718       case EMCMOT_START_LOG:
01719         /* start logging */
01720         /* first ignore triggered log types */
01721         if (emcmotStatus->logType == EMCMOT_LOG_TYPE_POS_VOLTAGE) {
01722           break;
01723         }
01724         if (emcmotStatus->logOpen) {
01725           emcmotStatus->logStarted = 1;
01726           logSkip = 0;
01727         }
01728         break;
01729 
01730       case EMCMOT_STOP_LOG:
01731         /* stop logging */
01732         emcmotStatus->logStarted = 0;
01733         break;
01734 
01735       case EMCMOT_CLOSE_LOG:
01736 #if defined (rtlinux) && defined (RT_FIFO)
01737         rtf_destroy(EMCMOT_LOG_RTF);
01738 #endif /* rtlinux and RT_FIFO */
01739         emcmotStatus->logOpen = 0;
01740         emcmotStatus->logStarted = 0;
01741         emcmotStatus->logSize = 0;
01742         emcmotStatus->logSkip = 0;
01743         emcmotStatus->logType = 0;
01744         break;
01745 
01746       case EMCMOT_DAC_OUT:
01747         /* write output to dacs directly */
01748         /* will only persist if amplifiers are disabled */
01749         axis = emcmotCommand->axis;
01750         if (axis < 0 ||
01751             axis >= EMCMOT_MAX_AXIS) {
01752           break;
01753         }
01754         /* trigger log, if active */
01755         if (emcmotStatus->logType == EMCMOT_LOG_TYPE_POS_VOLTAGE &&
01756             loggingAxis == axis &&
01757             emcmotStatus->logOpen != 0) {
01758           emcmotStatus->logStarted = 1;
01759           logSkip = 0;
01760         }
01761         emcmotStatus->output[axis] = emcmotCommand->dacOut;
01762         break;
01763 
01764       case EMCMOT_HOME:
01765         /* home the specified axis */
01766         /* need to be in free mode, enable on */
01767         /* homing is basically a slow incremental jog to full range */
01768         axis = emcmotCommand->axis;
01769         if (axis < 0 ||
01770             axis >= EMCMOT_MAX_AXIS) {
01771           break;
01772         }
01773         if (GET_MOTION_COORD_FLAG() ||
01774             ! GET_MOTION_ENABLE_FLAG()) {
01775           break;
01776         }
01777 
01778         if (GET_AXIS_HOMING_POLARITY(axis)) {
01779           freePose.tran.x = + 2.0 * AXRANGE(axis);
01780         }
01781         else {
01782           freePose.tran.x = - 2.0 * AXRANGE(axis);
01783         }
01784 
01785         tpSetVmax(&freeAxis[axis], emcmotConfig->homingVel[axis]);
01786         tpAddLine(&freeAxis[axis], freePose);
01787         waitingForLatch[axis] = 1;
01788         SET_AXIS_HOMING_FLAG(axis, 1);
01789         SET_AXIS_HOMED_FLAG(axis, 0);
01790         break;
01791 
01792       case EMCMOT_ENABLE_WATCHDOG:
01793         wdEnabling = 1;
01794         wdWait = emcmotCommand->wdWait;
01795         if (wdWait < 0) {
01796           wdWait = 0;
01797         }
01798         break;
01799 
01800       case EMCMOT_DISABLE_WATCHDOG:
01801         wdEnabling = 0;
01802         break;
01803 
01804       case EMCMOT_SET_POLARITY:
01805         axis = emcmotCommand->axis;
01806         if (axis < 0 ||
01807             axis >= EMCMOT_MAX_AXIS) {
01808           break;
01809         }
01810         if (emcmotCommand->level) {
01811           /* normal */
01812           emcmotConfig->axisPolarity[axis] |= emcmotCommand->axisFlag;
01813         }
01814         else {
01815           /* inverted */
01816           emcmotConfig->axisPolarity[axis] &= ~emcmotCommand->axisFlag;
01817         }
01818         break;
01819 
01820 #ifdef ENABLE_PROBING
01821       case EMCMOT_SET_PROBE_INDEX:
01822         emcmotConfig->probeIndex = emcmotCommand->probeIndex;
01823         break;
01824 
01825       case EMCMOT_SET_PROBE_POLARITY:
01826         emcmotConfig->probePolarity = emcmotCommand->level;
01827         break;
01828 
01829       case EMCMOT_CLEAR_PROBE_FLAGS:
01830         emcmotStatus->probeTripped = 0;
01831         emcmotStatus->probing = 1;
01832         break;
01833 
01834       case EMCMOT_PROBE:
01835         /* most of this is taken from EMCMOT_SET_LINE */
01836         /* queue up a linear move */
01837         /* requires coordinated mode, enable off, not on limits */
01838         if (! GET_MOTION_COORD_FLAG() ||
01839             ! GET_MOTION_ENABLE_FLAG()) {
01840           reportError("Need to be enabled, in coord mode for linear move");
01841           emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
01842           SET_MOTION_ERROR_FLAG(1);
01843           break;
01844         }
01845         else if (! inRange(emcmotCommand->pos)) {
01846           reportError("Linear move %d out of range",
01847                       emcmotCommand->id);
01848           emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01849           tpAbort(&queue);
01850           SET_MOTION_ERROR_FLAG(1);
01851           break;
01852         }
01853         else if (! checkLimits()) {
01854           reportError("Can't do linear move with limits exceeded");
01855           emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01856           tpAbort(&queue);
01857           SET_MOTION_ERROR_FLAG(1);
01858           break;
01859         }
01860 
01861         /* append it to the queue */
01862         tpSetId(&queue, emcmotCommand->id);
01863         if (-1 == tpAddLine(&queue, emcmotCommand->pos)) {
01864           reportError("Can't add linear move");
01865           emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
01866           tpAbort(&queue);
01867           SET_MOTION_ERROR_FLAG(1);
01868           break;
01869         }
01870         else {
01871           emcmotStatus->probeTripped = 0;
01872           emcmotStatus->probing = 1;
01873           SET_MOTION_ERROR_FLAG(0);
01874         }
01875         break;
01876 #endif /* ENABLE_PROBING */
01877 
01878       default:
01879         reportError("Unrecognized command %d", emcmotCommand->command);
01880         emcmotStatus->commandStatus = EMCMOT_COMMAND_UNKNOWN_COMMAND;
01881         break;
01882       } /* end of: command switch */
01883 
01884       /* synch tail count */
01885       emcmotStatus->tail = emcmotStatus->head;
01886       emcmotConfig->tail = emcmotConfig->head;
01887       emcmotDebug->tail = emcmotDebug->head;
01888 
01889     } /* end of: if-new-command */
01890 
01891 #if defined (rtlinux) && defined (RT_FIFO)
01892   } /* end of: while-msg loop for RT FIFO */
01893 
01894   if (err != 0) {
01895     return -1;
01896   }
01897 
01898 #endif /* rtlinux and RT_FIFO */
01899 
01900   return 0;
01901 }

01918 {
01919   double start, end, delta;     /* time stamping */
01920   int homeFlag;                 /* result of ext read to home switch */
01921   int axis;                     /* axis loop counter */
01922   int t;                        /* loop counter if we're in axis loop */
01923   int isLimit;                  /* result of ext read to limit sw */
01924   int whichCycle;               /* 0=none, 1=servo, 2=traj */
01925   int fault;
01926   double numres;
01927   double thisFerror[EMCMOT_MAX_AXIS];
01928   double limitFerror;           /* current scaled ferror */
01929   double magFerror;
01930 #ifdef STEPPER_MOTORS
01931   double bcomp[EMCMOT_MAX_AXIS]; /* backlash comp value */
01932   char bcompdir[EMCMOT_MAX_AXIS]; /* 0=none, 1=pos, -1=neg */
01933   double bcompincr[EMCMOT_MAX_AXIS]; /* incremental backlash comp value */
01934   double dsteps;                /* steps value (double) for stepper task */
01935   double oldbcomp;
01936   /* various parameters for acc/dec backlash comp */
01937   char bac_done[EMCMOT_MAX_AXIS];
01938   double bac_d[EMCMOT_MAX_AXIS];
01939   double bac_di[EMCMOT_MAX_AXIS];
01940   double bac_D[EMCMOT_MAX_AXIS];
01941   double bac_halfD[EMCMOT_MAX_AXIS];
01942   double bac_incrincr[EMCMOT_MAX_AXIS];
01943   double bac_incr[EMCMOT_MAX_AXIS];
01944 #endif
01945   char finalHome[EMCMOT_MAX_AXIS];
01946 
01947   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01948 #ifdef STEPPER_MOTORS
01949     bcomp[axis] = 0.0;
01950     bcompdir[axis] = 0;
01951     bcompincr[axis] = 0.0;
01952     bac_done[axis] = 1;
01953     bac_d[axis] = 0.0;
01954     bac_di[axis] = 0.0;
01955     bac_D[axis] = 0.0;
01956     bac_halfD[axis] = 0.0;
01957     bac_incrincr[axis] = 0.0;
01958     bac_incr[axis] = 0.0;
01959 #endif
01960     finalHome[axis] = 0;
01961   }
01962 
01963 #ifdef rtlinux
01964   for (;;) {
01965 #endif /* rtlinux */
01966 
01967 #ifdef rtlinux
01968     /* toggle sound port watchdog */
01969 
01970     if (wdEnabling && ! wdEnabled) {
01971       /* just turned WD on */
01972       wdCount = wdWait;
01973       wdToggle = 0;
01974       wdEnabled = 1;
01975     }
01976     if (! wdEnabling && wdEnabled) {
01977       /* just turned WD off */
01978       /* clear the bit */
01979       soundByte = inb(SOUND_PORT);
01980       soundByte &= ~SOUND_MASK;
01981       outb(soundByte, SOUND_PORT);
01982       wdEnabled = 0;
01983     }
01984 
01985     if (wdEnabled && wdCount-- <= 0) {
01986       soundByte = inb(SOUND_PORT);
01987       wdToggle = ! wdToggle;
01988       if (wdToggle) {
01989         soundByte |= SOUND_MASK;
01990       }
01991       else {
01992         soundByte &= ~SOUND_MASK;
01993       }
01994       outb(soundByte, SOUND_PORT);
01995       wdCount = wdWait;
01996     }
01997 #endif /* rtlinux */
01998 
01999     /* record start time */
02000     start = etime();
02001 
02002     /* READ COMMAND: */
02003 
02004 #ifndef RT_FIFO
02005     emcmotCommandHandler();
02006 
02007 #endif /* not RT_FIFO */
02008 
02009     /* increment head count */
02010     emcmotStatus->head++;
02011     emcmotDebug->head++;
02012 
02013     /* READ INPUTS: */
02014 
02015 #ifndef STEPPER_MOTORS
02016     /* latch all encoder feedback into raw input array, done outside
02017        of for-loop on joints below, since it's a single call for all
02018        joints */
02019     extEncoderReadAll(EMCMOT_MAX_AXIS, rawInput);
02020 #endif
02021 
02022     /* process input and read limit switches */
02023     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02024 #ifdef STEPPER_MOTORS
02025       /* READ INPUTS, deferred from above */
02026       rawInput[axis] = (double) emcmotDebug->stepperCount[axis] -
02027         bcompincr[axis] * smStepsPerUnit[axis];
02028 #endif
02029 
02030       /* save old cycle's values */
02031       oldInput[axis] = emcmotStatus->input[axis];
02032       /* set input, scaled according to input = (raw - offset) / scale */
02033       emcmotStatus->input[axis] =
02034         (rawInput[axis] - emcmotStatus->inputOffset[axis]) *
02035         inverseInputScale[axis];
02036 
02037 #ifndef STEPPER_MOTORS
02038       /* debounce bad feedback */
02039       if (fabs(emcmotStatus->input[axis] - oldInput[axis]) /
02040           emcmotConfig->servoCycleTime > bigVel) {
02041         /* bad input value-- debounce with the last value */
02042         emcmotStatus->input[axis] = oldInput[axis];
02043       }
02044 #endif
02045 
02046       /* read limit switches and amp fault from external
02047          interface, and set 'enabling' to zero if tripped
02048          to cause immediate stop */
02049 
02050       /* handle limits */
02051       if (GET_AXIS_ACTIVE_FLAG(axis)) {
02052         extMaxLimitSwitchRead(axis, &isLimit);
02053         if (isLimit == GET_AXIS_PHL_POLARITY(axis)) {
02054           if (++maxLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE) {
02055             SET_AXIS_PHL_FLAG(axis, 1);
02056             maxLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE;
02057             if (emcmotStatus->overrideLimits ||
02058                 isHoming()) {
02059             }
02060             else {
02061               SET_AXIS_ERROR_FLAG(axis, 1);
02062               enabling = 0;
02063             }
02064           }
02065         }
02066         else {
02067           SET_AXIS_PHL_FLAG(axis, 0);
02068           maxLimitSwitchCount[axis] = 0;
02069         }
02070       }
02071       if (GET_AXIS_ACTIVE_FLAG(axis)) {
02072         extMinLimitSwitchRead(axis, &isLimit);
02073         if (isLimit == GET_AXIS_NHL_POLARITY(axis)) {
02074           if (++minLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE) {
02075             SET_AXIS_NHL_FLAG(axis, 1);
02076             minLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE;
02077             if (emcmotStatus->overrideLimits ||
02078                 isHoming()) {
02079             }
02080             else {
02081               SET_AXIS_ERROR_FLAG(axis, 1);
02082               enabling = 0;
02083             }
02084           }
02085         }
02086         else {
02087           SET_AXIS_NHL_FLAG(axis, 0);
02088           minLimitSwitchCount[axis] = 0;
02089         }
02090       }
02091 
02092       if (GET_AXIS_ACTIVE_FLAG(axis) &&
02093           GET_AXIS_ENABLE_FLAG(axis)) {
02094         extAmpFault(axis, &fault);
02095         if (fault == GET_AXIS_FAULT_POLARITY(axis)) {
02096           if (++ampFaultCount[axis] > AMP_FAULT_DEBOUNCE) {
02097             SET_AXIS_ERROR_FLAG(axis, 1);
02098             SET_AXIS_FAULT_FLAG(axis, 1);
02099             ampFaultCount[axis] = AMP_FAULT_DEBOUNCE;
02100             enabling = 0;
02101           }
02102         }
02103         else {
02104           SET_AXIS_FAULT_FLAG(axis, 0);
02105           ampFaultCount[axis] = 0;
02106         }
02107       }
02108 
02109       /* read home switch and set flag if tripped. Handling of home
02110          sequence is done later. */
02111       if (GET_AXIS_ACTIVE_FLAG(axis)) {
02112         extHomeSwitchRead(axis, &homeFlag);
02113         if (homeFlag == GET_AXIS_HOME_SWITCH_POLARITY(axis)) {
02114           SET_AXIS_HOME_SWITCH_FLAG(axis, 1);
02115         }
02116         else {
02117           SET_AXIS_HOME_SWITCH_FLAG(axis, 0);
02118         }
02119       }
02120 
02121     } /* end of: loop on axes, for reading inputs, setting limit and
02122            home switch flags */
02123 
02124     /* now we're outside the axis loop, having just read input, scaled it,
02125          read the limit and home switches and amp faults. We need to abort
02126          all motion if we're on limits, handle homing sequence, and handle
02127          mode and state transitions. */
02128 
02129     /* RUN STATE LOGIC: */
02130 
02131     /* check for disabling */
02132     if (! enabling && GET_MOTION_ENABLE_FLAG()) {
02133       /* clear out the motion queue and interpolators */
02134       tpClear(&queue);
02135       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02136         tpClear(&freeAxis[axis]);
02137         cubicDrain(&cubic[axis]);
02138         if (GET_AXIS_ACTIVE_FLAG(axis)) {
02139           extAmpEnable(axis, ! GET_AXIS_ENABLE_POLARITY(axis));
02140           SET_AXIS_ENABLE_FLAG(axis, 0);
02141           SET_AXIS_HOMING_FLAG(axis, 0);
02142           emcmotStatus->output[axis] = 0.0;
02143         }
02144         /* don't clear the axis error flag, since that may signify why
02145            we just went into disabled state */
02146       }
02147       /* reset the trajectory interpolation counter, so that the kinematics
02148          functions called during the disabled state run at the nominal
02149          trajectory rate rather than the servo rate. It's loaded with
02150          emcmotConfig->interpolationRate when it goes to zero. */
02151       interpolationCounter = 0;
02152 #ifdef STEPPER_MOTORS
02153       smInhibit = 1;
02154 #endif
02155       SET_MOTION_ENABLE_FLAG(0);
02156       /* don't clear the motion error flag, since that may signify why
02157          we just went into disabled state */
02158     }
02159 
02160     /* check for enabling */
02161     if (enabling && ! GET_MOTION_ENABLE_FLAG()) {
02162       tpSetPos(&queue, emcmotStatus->pos);
02163       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02164         freePose.tran.x = jointPos[axis];
02165         tpSetPos(&freeAxis[axis], freePose);
02166 #ifndef STEPPER_MOTORS
02167         pidReset(&emcmotConfig->pid[axis]);
02168 #endif
02169         if (GET_AXIS_ACTIVE_FLAG(axis)) {
02170           extAmpEnable(axis, GET_AXIS_ENABLE_POLARITY(axis));
02171           SET_AXIS_ENABLE_FLAG(axis, 1);
02172           SET_AXIS_HOMING_FLAG(axis, 0);
02173         }
02174         /* clear any outstanding axis errors when going into enabled state */
02175         SET_AXIS_ERROR_FLAG(axis, 0);
02176       }
02177 #ifdef STEPPER_MOTORS
02178       smInhibit = 0;
02179 #endif
02180       SET_MOTION_ENABLE_FLAG(1);
02181       /* clear any outstanding motion errors when going into enabled state */
02182       SET_MOTION_ERROR_FLAG(0);
02183     }
02184 
02185     /* check for entering coordinated mode */
02186     if (coordinating && ! GET_MOTION_COORD_FLAG()) {
02187       if (GET_MOTION_INPOS_FLAG()) {
02188         /* update coordinated queue position */
02189         tpSetPos(&queue, emcmotStatus->pos);
02190         /* drain the cubics so they'll synch up */
02191         for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02192           cubicDrain(&cubic[axis]);
02193         }
02194         /* clear the override limits flags */
02195         overriding = 0;
02196         emcmotStatus->overrideLimits = 0;
02197         SET_MOTION_COORD_FLAG(1);
02198         SET_MOTION_ERROR_FLAG(0);
02199       }
02200       else {
02201         /* not in position-- don't honor mode change */
02202         coordinating = 0;
02203       }
02204     }
02205 
02206     /* check entering free space mode */
02207     if (! coordinating && GET_MOTION_COORD_FLAG()) {
02208       if (GET_MOTION_INPOS_FLAG()) {
02209         for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02210           /* update free planner positions */
02211           freePose.tran.x = jointPos[axis];
02212           tpSetPos(&freeAxis[axis], freePose);
02213           /* drain the cubics so they'll synch up */
02214           cubicDrain(&cubic[axis]);
02215         }
02216         SET_MOTION_COORD_FLAG(0);
02217         SET_MOTION_ERROR_FLAG(0);
02218       }
02219       else {
02220         /* not in position-- don't honor mode change */
02221         coordinating = 1;
02222       }
02223     }
02224 
02225     /* check for homing sequences */
02226     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02227       if (GET_AXIS_HOMING_FLAG(axis)) {
02228         if (tpIsDone(&freeAxis[axis])) {
02229           /* check for decel or final move */
02230           if (finalHome[axis]) {
02231             /* reset flag-- we're back at home */
02232             finalHome[axis] = 0;
02233 
02234             /* rework the home kinematics values-- this could be done
02235                after each message to set world or joint kinematics, but
02236                we'll defer it to here so it happens just prior to when
02237                it's needed. Note that the nominal value of these may be
02238                changed, if the kinematics need to. */
02239             kinematicsHome(&worldHome, jointHome, &fflags, &iflags);
02240 
02241             /* set input offset to value such that resulting input
02242                is the jointHome[] value, with:
02243                input = (raw - offset) / scale ->
02244                offset = raw - input * scale ->
02245                offset = raw - jointHome * scale */
02246 
02247             emcmotStatus->inputOffset[axis] = rawInput[axis] -
02248               jointHome[axis] * emcmotStatus->inputScale[axis];
02249 
02250             /* recompute inputs to match so we don't have a
02251                momentary jump */
02252             emcmotStatus->input[axis] = jointHome[axis];
02253             oldInput[axis] = emcmotStatus->input[axis];
02254 
02255             /* reset interpolator so that it doesn't jump */
02256             cubicOffset(&cubic[axis], jointHome[axis] - coarseJointPos[axis]);
02257 
02258             /* set axis position to jointHome upon homing */
02259             jointPos[axis] = jointHome[axis];
02260             freePose.tran.x = jointHome[axis];
02261             tpSetPos(&freeAxis[axis], freePose);
02262 
02263             SET_AXIS_HOMING_FLAG(axis, 0);
02264             SET_AXIS_HOMED_FLAG(axis, 1);
02265 
02266             /* set allHomed flag if all active axes are homed; this
02267                  will signify that kinematics functions can be called */
02268             allHomed = 1;
02269             for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
02270               if (GET_AXIS_ACTIVE_FLAG(t) &&
02271                   ! GET_AXIS_HOMED_FLAG(t)) {
02272                 /* one active axis is not homed */
02273                 allHomed = 0;
02274                 break;
02275               }
02276             }
02277             /* FIXME-- this only works with no forward kins */
02278             if (allHomed) {
02279               emcmotStatus->pos = worldHome;
02280               emcmotStatus->actualPos = worldHome;
02281             }
02282           }
02283           else {
02284             /* just finished decel, now we'll do final home */
02285             finalHome[axis] = 1;
02286             /* add move back to latched position + backoff */
02287             freePose.tran.x =
02288               (saveLatch[axis] - emcmotStatus->inputOffset[axis]) *
02289               inverseInputScale[axis] +
02290               emcmotConfig->homeOffset[axis];
02291             tpAddLine(&freeAxis[axis], freePose);
02292           }
02293         } /* end of: if axis is done either decel or final home */
02294       } /* end of: if axis is homing */
02295     } /* end of: axis loop that checks for homing */
02296 
02297     /* RUN MOTION CALCULATIONS: */
02298 
02299     /* run axis interpolations and outputs, but only if we're
02300        enabled. This section is "suppressed" if we're not
02301        enabled, although the read/write of encoders/dacs is still
02302        done. */
02303     whichCycle = 0;
02304     if (GET_MOTION_ENABLE_FLAG()) {
02305       /* set whichCycle to be at least a servo cycle, for calc time logging */
02306       whichCycle = 1;
02307 
02308       /* check to see if the interpolators are empty */
02309       while (cubicNeedNextPoint(&cubic[0])) {
02310         /* they're empty, so pull next point(s) off Cartesian or
02311            joint planner, depending upon coord or free mode. */
02312 
02313         /* check to see whether we're in coordinated or free mode, to
02314            decide which motion planner to call */
02315         if (GET_MOTION_COORD_FLAG()) {
02316           /* we're in coordinated mode-- pull a pose off the Cartesian
02317              trajectory planner, run it through the inverse kinematics,
02318              and spline up the joint points for interpolation in
02319              servo cycles. */
02320 
02321           /* set whichCycle to be a Cartesian trajectory cycle,
02322              for calc time logging */
02323           whichCycle = 2;
02324 
02325           /* run coordinated trajectory planning cycle */
02326           tpRunCycle(&queue);
02327 
02328           /* set new commanded traj pos */
02329           emcmotStatus->pos = tpGetPos(&queue);
02330 
02331           /* convert to joints */
02332           kinematicsInverse(&emcmotStatus->pos, coarseJointPos, &iflags, &fflags);
02333 
02334           /* spline joints up-- note that we may be adding points that
02335              fail soft limits, but we'll abort at the end of this cycle
02336              so it doesn't really matter */
02337           for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02338             cubicAddPoint(&cubic[axis], coarseJointPos[axis]);
02339           }
02340 
02341           /* call forward kinematics on input points for actual pos,
02342              at trajectory rate to save bandwidth */
02343           /* FIXME */
02344           if (EMCMOT_NO_FORWARD_KINEMATICS) {
02345             /* fake it by setting actual pos to commanded pos */
02346             emcmotStatus->actualPos = emcmotStatus->pos;
02347           }
02348           else {
02349             kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);
02350           }
02351 
02352           /* now emcmotStatus->actualPos, emcmotStatus->pos, and
02353              coarseJointPos[] are set */
02354 
02355         } /* end of: coord mode */
02356         else {
02357           /* we're in free mode-- run joint planning cycles */
02358           for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02359             /* set whichCycle to be a joint trajectory cycle,
02360                for calc time logging */
02361             /* note that this may include one or more joint trajectory
02362                cycles, so calc time may be inherently variable */
02363             whichCycle = 2;
02364 
02365             /* run joint trajectory planning cycle */
02366             tpRunCycle(&freeAxis[axis]);
02367 
02368             /* set new coarse joint position. FIXME-- this uses only
02369                the tran.x field of the TP_STRUCT, which is overkill.
02370                We need a TP_STRUCT with a single scalar element. */
02371             coarseJointPos[axis] = tpGetPos(&freeAxis[axis]).tran.x;
02372 
02373             /* spline joint up-- note that we may be adding a point that
02374                fails soft limit, but we'll abort at the end of this cycle
02375                so it doesn't really matter */
02376             cubicAddPoint(&cubic[axis], coarseJointPos[axis]);
02377 
02378           } /* end of: axis for loop for joint planning cycle */
02379 
02380           if (EMCMOT_NO_FORWARD_KINEMATICS) {
02381             /* can't do anything */
02382           }
02383           else {
02384             /* call forward kinematics on input points for actual pos,
02385                at trajectory rate to save bandwidth */
02386             kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);
02387 
02388             /* push coarseJointPos[] through the forward kins to
02389              set emcmotStatus->pos, which are not computed in free
02390              mode as they are in coord mode */
02391           /* Note: in systems where the forward kins are costly to compute,
02392              emcmotStatus->pos can be set to emcmotStatus->actualPos to
02393              save one call to the forward kins */
02394             kinematicsForward(coarseJointPos, &emcmotStatus->pos, &fflags, &iflags);
02395           }
02396 
02397           /* now emcmotStatus->actualPos, emcmotStatus->pos, and
02398              coarseJointPos[] are set */
02399 
02400         } /* end of: free mode trajectory planning */
02401       } /* end of: while (cubicNeedNextPoint(0)) */
02402 
02403       /* we're still in motion enabled section. For coordinated mode,
02404          the Cartesian trajectory cycle has been computed, if necessary,
02405          run through the inverse kinematics, and the joints have been
02406          splined up for interpolation. For free mode, the joint trajectory
02407          cycles have been computed, if necessary, and the joints have
02408          been splined up for interpolation. We still need to push the
02409          actual input through the forward kinematics, for actual pos.
02410 
02411          Effects:
02412 
02413          For coord mode, emcmotStatus->pos contains the commanded
02414          Cartesian pose, coarseJointPos[] contains the results of the
02415          inverse kinematics at the coarse (trajectory) rate, and the
02416          interpolators are not empty.
02417 
02418          For free mode, emcmotStatus->pos is unchanged, and needs to
02419          be updated via the forward kinematics. FIXME-- make sure this
02420          happens, and note where in this comment. coarseJointPos[]
02421          contains the results of the joint trajectory calculations
02422          at the coarse (trajectory) rate, and the interpolators are
02423          not empty.
02424       */
02425 
02426       /* check for soft joint limits. If so, abort all motion. The
02427          interpolators will pick this up further down and begin
02428          planning abort and stop. */
02429       onLimit = 0;
02430       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02431         SET_AXIS_PSL_FLAG(axis, 0);
02432         SET_AXIS_NSL_FLAG(axis, 0);
02433         if (GET_AXIS_HOMED_FLAG(axis)) {
02434           if (coarseJointPos[axis] > emcmotConfig->maxLimit[axis]) {
02435             SET_AXIS_ERROR_FLAG(axis, 1);
02436             SET_AXIS_PSL_FLAG(axis, 1);
02437             onLimit = 1;
02438           }
02439           else if (coarseJointPos[axis] < emcmotConfig->minLimit[axis]) {
02440             SET_AXIS_ERROR_FLAG(axis, 1);
02441             SET_AXIS_NSL_FLAG(axis, 1);
02442           }
02443         }
02444       }
02445 
02446       /* reset wasOnLimit flag iff all joints are free of
02447        soft limits, as seen in the flag bits set last cycle.
02448        No need to do this for hard limits, since wasOnLimit
02449        only prevents flurry of aborts while on a soft limit
02450        and hard limits don't abort, they disable. */
02451       if (onLimit) {
02452         if (! wasOnLimit) {
02453           /* abort everything, regardless of coord or free mode */
02454           tpAbort(&queue);
02455           for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02456             tpAbort(&freeAxis[axis]);
02457             wasOnLimit = 1;
02458           }
02459         }
02460         /* else we were on a limit, so inhibit firing of aborts */
02461       }
02462       else {
02463         /* not on a limit, so clear wasOnLimit so aborts fire next
02464            time we are on a limit */
02465         wasOnLimit = 0;
02466       }
02467 
02468       if (whichCycle == 2) {
02469         /* we're on a trajectory cycle, either Cartesian or joint planning, so
02470            log per-traj-cycle data if logging active */
02471         logIt = 0;
02472 
02473         if (emcmotStatus->logStarted &&
02474             emcmotStatus->logSkip >= 0) {
02475 
02476           /* calculate current vel, accel, for logging */
02477           newVel.tran.x = (emcmotStatus->pos.tran.x - oldPos.tran.x) /
02478             emcmotConfig->trajCycleTime;
02479           newVel.tran.y = (emcmotStatus->pos.tran.y - oldPos.tran.y) /
02480             emcmotConfig->trajCycleTime;
02481           newVel.tran.z = (emcmotStatus->pos.tran.z - oldPos.tran.z) /
02482             emcmotConfig->trajCycleTime;
02483           oldPos = emcmotStatus->pos;
02484           newAcc.tran.x = (newVel.tran.x - oldVel.tran.x) /
02485             emcmotConfig->trajCycleTime;
02486           newAcc.tran.y = (newVel.tran.y - oldVel.tran.y) /
02487             emcmotConfig->trajCycleTime;
02488           newAcc.tran.z = (newVel.tran.z - oldVel.tran.z) /
02489             emcmotConfig->trajCycleTime;
02490           oldVel = newVel;
02491 
02492           /* save the type with the log item */
02493           ls.type = emcmotStatus->logType;
02494 
02495           /* now log type-specific data */
02496           switch (emcmotStatus->logType) {
02497 
02498           case EMCMOT_LOG_TYPE_TRAJ_POS:
02499             if (logSkip-- <= 0) {
02500               ls.item.trajPos.time = start;
02501               ls.item.trajPos.pos = emcmotStatus->pos.tran;
02502               logSkip = emcmotStatus->logSkip;
02503               logIt = 1;
02504             }
02505             break;
02506 
02507           case EMCMOT_LOG_TYPE_TRAJ_VEL:
02508             if (logSkip-- <= 0) {
02509               ls.item.trajVel.time = start;
02510               ls.item.trajVel.vel = newVel.tran;
02511               pmCartMag(newVel.tran, &ls.item.trajVel.mag);
02512               logSkip = emcmotStatus->logSkip;
02513               logIt = 1;
02514             }
02515             break;
02516 
02517           case EMCMOT_LOG_TYPE_TRAJ_ACC:
02518             if (logSkip-- <= 0) {
02519               ls.item.trajAcc.time = start;
02520               ls.item.trajAcc.acc = newAcc.tran;
02521               pmCartMag(newAcc.tran, &ls.item.trajAcc.mag);
02522               logSkip = emcmotStatus->logSkip;
02523               logIt = 1;
02524             }
02525             break;
02526 
02527           default:
02528             break;
02529           } /* end of: switch on log type */
02530 
02531           /* now log it */
02532 #if defined (rtlinux) && defined (RT_FIFO)
02533           if (logIt) {
02534             rtf_put(EMCMOT_LOG_RTF, &ls, sizeof(EMCMOT_LOG_STRUCT));
02535             /* FIXME-- update emcmotStatus->logPoints */
02536             logIt = 0;
02537           }
02538 #else
02539           if (logIt) {
02540             emcmotLogAdd(emcmotLog, ls);
02541             emcmotStatus->logPoints = emcmotLog->howmany;
02542             logIt = 0;
02543           }
02544 #endif
02545         } /* end of: if (emcmotStatus->logStarted &&
02546                          emcmotStatus->logSkip >= 0) */
02547       } /* end of: if (whichCycle == 2), for trajectory cycle logging */
02548 
02549       /* run interpolation and compensation */
02550       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02551         /* interpolate */
02552         oldJointPos[axis] = jointPos[axis];
02553         jointPos[axis] = cubicInterpolate(&cubic[axis], 0, 0, 0, 0);
02554         /* round position to input resolution */
02555         numres = jointPos[axis] * emcmotStatus->inputScale[axis];
02556         if (numres < 0.0) {
02557           jointPos[axis] = inverseInputScale[axis] * ((int) (numres - 0.5));
02558         }
02559         else {
02560           jointPos[axis] = inverseInputScale[axis] * ((int) (numres + 0.5));
02561         }
02562         jointVel[axis] = (jointPos[axis] - oldJointPos[axis]) /
02563           emcmotConfig->servoCycleTime;
02564 
02565         /* run output calculations */
02566         if (GET_AXIS_ACTIVE_FLAG(axis)) {
02567 #ifdef STEPPER_MOTORS
02568           /* backlash compensation for steppers*/
02569           /* This is pretty screwed up, since backlash was originally
02570              part of the PID structure. We need to duplicate the
02571              correction method here, and use the backlash value from
02572              the PID structure since that's where it ends up.
02573              FIXME-- make backlash part of the EMC status proper, not
02574              the PID structure */
02575           if (jointPos[axis] - oldJointPos[axis] > 0.0) {
02576             oldbcomp = bcomp[axis];
02577             bcomp[axis] = + emcmotConfig->pid[axis].backlash / 2.0;
02578             if (bcompdir[axis] != + 1) {
02579               bac_done[axis] = 0;
02580               bac_di[axis] = bcompincr[axis];
02581               bac_d[axis] = 0;
02582               bac_D[axis] = bcomp[axis] - oldbcomp;
02583               bac_halfD[axis] = 0.5 * bac_D[axis];
02584               bac_incrincr[axis] = emcmotStatus->acc *
02585                 emcmotConfig->servoCycleTime * emcmotConfig->servoCycleTime;
02586               bac_incr[axis] = - 0.5 * bac_incrincr[axis];
02587               bcompdir[axis] = + 1;
02588             }
02589           }
02590           else if (jointPos[axis] - oldJointPos[axis] < 0.0) {
02591             oldbcomp = bcomp[axis];
02592             bcomp[axis] = - emcmotConfig->pid[axis].backlash / 2.0;
02593             if (bcompdir[axis] != - 1) {
02594               bac_done[axis] = 0;
02595               bac_di[axis] = bcompincr[axis];
02596               bac_d[axis] = 0;
02597               bac_D[axis] = bcomp[axis] - oldbcomp;
02598               bac_halfD[axis] = 0.5 * bac_D[axis];
02599               bac_incrincr[axis] = emcmotStatus->acc *
02600                 emcmotConfig->servoCycleTime * emcmotConfig->servoCycleTime;
02601               bac_incr[axis] = - 0.5 * bac_incrincr[axis];
02602               bcompdir[axis] = - 1;
02603             }
02604           }
02605           /* else no motion, so leave bcomp what it was */
02606 
02607           /* mete out the backlash according to an acc/dec profile */
02608           if (! bac_done[axis]) {
02609             if (bac_D[axis] > 0.0) {
02610               bcompincr[axis] = bac_di[axis] + bac_d[axis];
02611               if (bac_d[axis] < bac_halfD[axis]) {
02612                 bac_incr[axis] += bac_incrincr[axis];
02613                 bac_d[axis] += bac_incr[axis];
02614               }
02615               else if (bac_d[axis] < bac_D[axis]) {
02616                 bac_incr[axis] -= bac_incrincr[axis];
02617                 bac_d[axis] += bac_incr[axis];
02618               }
02619               if (bac_d[axis] >= bac_D[axis] || bac_incr[axis] <= 0.0) {
02620                 bac_done[axis] = 1;
02621               }
02622             }
02623             else {
02624               bcompincr[axis] = bac_di[axis] + bac_d[axis];
02625               if (bac_d[axis] > bac_halfD[axis]) {
02626                 bac_incr[axis] += bac_incrincr[axis];
02627                 bac_d[axis] -= bac_incr[axis];
02628               }
02629               else if (bac_d[axis] > bac_D[axis]) {
02630                 bac_incr[axis] -= bac_incrincr[axis];
02631                 bac_d[axis] -= bac_incr[axis];
02632               }
02633               if (bac_d[axis] <= bac_D[axis] || bac_incr[axis] <= 0.0) {
02634                 bac_done[axis] = 1;
02635               }
02636             }
02637           } /* end of: if not bac_done[axis] */
02638 
02639           /* fill in global smSteps[] for stepper task */
02640           /* first get steps, as a floating point */
02641           dsteps = (jointPos[axis] + bcompincr[axis]) *
02642             smStepsPerUnit[axis] + emcmotStatus->inputOffset[axis];
02643           /* round to nearest integer */
02644           if (dsteps < 0.0) {
02645             smSteps[axis] = (int) (dsteps - 0.5);
02646           }
02647           else {
02648             smSteps[axis] = (int) (dsteps + 0.5);
02649           }
02650 
02651           /* flag this as the start of a new chunk for the stepper task */
02652           smNewCycle[axis] = 1;
02653 #else
02654           /* COMPENSATE: */
02655 
02656           /*
02657             compensation means compute output
02658             'emcmotStatus->output[]' based on desired position
02659             'jointPos[]' and input 'emcmotStatus->input[]'.
02660 
02661             Currently the source calls for PID compensation.
02662             FIXME-- add wrapper for compensator, with ptr to
02663             emcmotStatus struct, with semantics that ->output[]
02664             needs to be filled.
02665           */
02666 
02667           /* here is PID compensation */
02668           emcmotStatus->output[axis] =
02669             pidRunCycle(&emcmotConfig->pid[axis],
02670                         emcmotStatus->input[axis],
02671                         jointPos[axis]);
02672 #endif
02673 
02674           /* COMPUTE FOLLOWING ERROR: */
02675 
02676           /* compute signed following error and magnitude */
02677           thisFerror[axis] = jointPos[axis] - emcmotStatus->input[axis];
02678           emcmotDebug->ferrorCurrent[axis] = thisFerror[axis];
02679           magFerror = fabs(thisFerror[axis]);
02680 
02681           /* record the max ferror for this axis */
02682           if (emcmotDebug->ferrorHighMark[axis] < magFerror) {
02683             emcmotDebug->ferrorHighMark[axis] = magFerror;
02684           }
02685 
02686           /* compute the scaled ferror for a move of this speed */
02687           limitFerror = emcmotConfig->maxFerror[axis] /
02688             emcmotConfig->limitVel *
02689             jointVel[axis];
02690           if (limitFerror < emcmotConfig->minFerror[axis]) {
02691             limitFerror = emcmotConfig->minFerror[axis];
02692           }
02693 
02694           /* abort if this ferror is greater than the scaled ferror */
02695           if (magFerror > limitFerror) {
02696             /* abort! abort! following error exceeded */
02697             SET_AXIS_ERROR_FLAG(axis, 1);
02698             SET_AXIS_FERROR_FLAG(axis, 1);
02699             if (enabling) {
02700               // report the following error just this once
02701               reportError("Axis %d following error", axis);
02702             }
02703             enabling = 0;
02704           }
02705           else {
02706             SET_AXIS_FERROR_FLAG(axis, 0);
02707           }
02708         } /* end of: if (GET_AXIS_ACTIVE_FLAG(axis)) */
02709         else {
02710           /* axis is not active-- leave the pid output where it is--
02711              if axis is not active one can still write to the dac */
02712         }
02713 
02714 #ifndef STEPPER_MOTORS
02715         /* CLAMP OUTPUT: */
02716 
02717         /*
02718           clamp output means take 'emcmotStatus->output[]' and
02719           limit to range
02720           'emcmotConfig->minOutput[] .. emcmotConfig->maxOutput[]'
02721         */
02722         if (emcmotStatus->output[axis] < emcmotConfig->minOutput[axis]) {
02723           emcmotStatus->output[axis] = emcmotConfig->minOutput[axis];
02724         }
02725         else if (emcmotStatus->output[axis] > emcmotConfig->maxOutput[axis]) {
02726           emcmotStatus->output[axis] = emcmotConfig->maxOutput[axis];
02727         }
02728 #endif
02729 
02730         /* CHECK FOR LATCH CONDITION: */
02731 
02732         /*
02733           check for latch condition means if we're waiting for a
02734           latched index pulse, and we see the pulse and the home
02735           switch, we read the raw input and abort. The offset is set
02736           above in the homing section by noting that if we're homing,
02737           and waitingForLatch[] is cleared, we latched.
02738 
02739           This presumes an encoder index pulse.
02740           FIXME-- remove explicit calls to encoder index pulse, to
02741           allow for open-loop control latching via switches only.
02742         */
02743         if (waitingForLatch[axis]) {
02744           if (GET_AXIS_HOME_SWITCH_FLAG(axis) ||
02745               GET_AXIS_PHL_FLAG(axis) ||
02746               GET_AXIS_NHL_FLAG(axis)) {
02747             /* read encoder index */
02748 #ifdef STEPPER_MOTORS
02749             /* force an "encoder latch" */
02750             latchFlag[axis] = 1;
02751 #else
02752             extEncoderResetIndex(axis);
02753             extEncoderReadLatch(axis, &latchFlag[axis]);
02754 #endif
02755             if (latchFlag[axis]) {
02756               /* get latched position in RAW UNITS */
02757               saveLatch[axis] = rawInput[axis];
02758               waitingForLatch[axis] = 0;
02759               /* call for an abort-- when it's finished, code
02760                  above sets inputOffset[] to saveLatch[] */
02761               finalHome[axis] = 0;
02762               tpAbort(&freeAxis[axis]);
02763             }
02764           } /* end of: if (GET_AXIS_HOME_SWITCH_FLAG(axis)) */
02765         }/* end of: if (waitingForLatch[axis]) */
02766       } /* end of: for (axis = 0; ...) */
02767     } /* end of: if (GET_MOTION_ENABLE_FLAG()) */
02768     else {
02769       /* we're not enabled, so no motion planning or interpolation has
02770          been done. jointPos[] is set to emcmotStatus->input[], and
02771          likewise with coarseJointPos[], which is normally updated at
02772          the traj rate but it's convenient to do them here at the same
02773          time at the servo rate. emcmotStatus->pos, ->actualPos need to
02774          be run through forward kinematics.
02775          Note that we are running at the servo rate, so we may need
02776          to slow down by the interpolation factor to avoid soaking
02777          the CPU. If we were enabled, ->pos was set by calcs (coord mode)
02778          or forward kins (free mode), and ->actualPos was set by forward
02779          kins on ->input[], all at the trajectory rate. */
02780       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02781         coarseJointPos[axis] = emcmotStatus->input[axis];
02782         jointPos[axis] = coarseJointPos[axis];
02783       }
02784       /* synthesize the trajectory interpolation, via a counter that
02785          decrements from the interpolation rate. This causes the statements
02786          to execute at the trajectory rate instead of the servo rate at which
02787          this enclosing code is called. */
02788       if (--interpolationCounter <= 0) {
02789         if (! EMCMOT_NO_FORWARD_KINEMATICS) {
02790           /* call the forward kinematics, at the effective trajectory rate */
02791           if (allHomed ||
02792               kinType == KINEMATICS_IDENTITY) {
02793             kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);
02794           }
02795           emcmotStatus->pos = emcmotStatus->actualPos;
02796         }
02797         /* reload the interpolation counter that simulates the interpolation
02798            done when enabled */
02799         interpolationCounter = emcmotConfig->interpolationRate;
02800       }
02801     }
02802 
02803 #ifdef ENABLE_PROBING
02804     extDioRead(emcmotConfig->probeIndex, &emcmotStatus->probeval);
02805     if (emcmotStatus->probing && emcmotStatus->probeTripped) {
02806       tpClear(&queue);
02807       emcmotStatus->probing = 0;
02808     }
02809     else if (emcmotStatus->probing && GET_MOTION_INPOS_FLAG() &&
02810              tpQueueDepth(&queue) == 0) {
02811       emcmotStatus->probing = 0;
02812     }
02813     else if (emcmotStatus->probing) {
02814       if (emcmotStatus->probeval == emcmotConfig->probePolarity) {
02815         emcmotStatus->probeTripped = 1;
02816         emcmotStatus->probedPos = emcmotStatus->actualPos;
02817         if (GET_MOTION_COORD_FLAG()) {
02818           tpAbort(&queue);
02819           SET_MOTION_ERROR_FLAG(0);
02820         }
02821         else {
02822           /* check axis range */
02823           for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02824             tpAbort(&freeAxis[axis]);
02825             SET_AXIS_HOMING_FLAG(axis, 0);
02826             SET_AXIS_ERROR_FLAG(axis, 0);
02827           }
02828         }
02829       }
02830     }
02831 #endif /* ENABLE_PROBING */
02832 
02833 #ifndef STEPPER_MOTORS
02834 
02835     /* SCALE OUTPUTS: */
02836 
02837     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02838       rawOutput[axis] =
02839         (emcmotStatus->output[axis] - emcmotStatus->outputOffset[axis]) *
02840         inverseOutputScale[axis];
02841     }
02842 
02843     /* WRITE OUTPUTS: */
02844 
02845     /* write DACs-- note that this is done even when not
02846        enabled, although in this case the pidOutputs are
02847        all zero unless manually overridden. They will not
02848        be set by any calculations if we're not enabled. */
02849     extDacWriteAll(EMCMOT_MAX_AXIS, rawOutput);
02850 #endif
02851 
02852     /* UPDATE THE REST OF THE DYNAMIC STATUS: */
02853 
02854     /* copy computed axis positions to status. Note that if motion is
02855        disabled, this is the same as ->input[]. */
02856     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02857       emcmotStatus->axisPos[axis] = jointPos[axis];
02858     }
02859 
02860     /* health heartbeat */
02861     emcmotStatus->heartbeat++;
02862 
02863     /* motion queue status */
02864     emcmotStatus->depth = tpQueueDepth(&queue);
02865     emcmotStatus->activeDepth = tpActiveDepth(&queue);
02866     emcmotStatus->id = tpGetExecId(&queue);
02867     emcmotStatus->queueFull = tcqFull(&queue.queue);
02868     SET_MOTION_INPOS_FLAG(0);
02869     if (tpIsDone(&queue)) {
02870       SET_MOTION_INPOS_FLAG(1);
02871     }
02872 
02873     /* axis status */
02874     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02875       SET_AXIS_INPOS_FLAG(axis, 0);
02876       if (tpIsDone(&freeAxis[axis])) {
02877         SET_AXIS_INPOS_FLAG(axis, 1);
02878       }
02879       else {
02880         /* this axis, at least, is moving, so set overriding flag */
02881         if (emcmotStatus->overrideLimits) {
02882           overriding = 1;
02883         }
02884       }
02885     }
02886 
02887     /* reset overrideLimits flag if we have started a move and now
02888        are in position */
02889     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02890       if (! GET_AXIS_INPOS_FLAG(axis)) {
02891         break;
02892       }
02893     }
02894     if (axis == EMCMOT_MAX_AXIS) {
02895       /* ran through all axes, and all are in position */
02896       if (overriding) {
02897         overriding = 0;
02898         emcmotStatus->overrideLimits = 0;
02899       }
02900     }
02901 
02902     /* check to see if we should pause in order to implement
02903        single stepping */
02904     if (stepping && idForStep != emcmotStatus->id) {
02905       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02906         tpPause(&freeAxis[axis]);
02907       }
02908       tpPause(&queue);
02909       stepping = 0;
02910       emcmotStatus->paused = 1;
02911     }
02912 
02913     /* calculate elapsed time and min/max/avg */
02914     end = etime();
02915     delta = end - start;
02916     emcmotStatus->computeTime = delta;
02917     if (2 == whichCycle) {
02918       /* traj calcs done this cycle-- use tMin,Max,Avg */
02919       mmxavgAdd(&emcmotDebug->tMmxavg, delta);
02920       emcmotDebug->tMin = mmxavgMin(&emcmotDebug->tMmxavg);
02921       emcmotDebug->tMax = mmxavgMax(&emcmotDebug->tMmxavg);
02922       emcmotDebug->tAvg = mmxavgAvg(&emcmotDebug->tMmxavg);
02923     }
02924     else if (1 == whichCycle) {
02925       /* servo calcs only this cycle-- use sMin,Max,Avg */
02926       mmxavgAdd(&emcmotDebug->sMmxavg, delta);
02927       emcmotDebug->sMin = mmxavgMin(&emcmotDebug->sMmxavg);
02928       emcmotDebug->sMax = mmxavgMax(&emcmotDebug->sMmxavg);
02929       emcmotDebug->sAvg = mmxavgAvg(&emcmotDebug->sMmxavg);
02930     }
02931     else {
02932       /* calcs disabled this cycle-- use nMin,Max,Avg */
02933       mmxavgAdd(&emcmotDebug->nMmxavg, delta);
02934       emcmotDebug->nMin = mmxavgMin(&emcmotDebug->nMmxavg);
02935       emcmotDebug->nMax = mmxavgMax(&emcmotDebug->nMmxavg);
02936       emcmotDebug->nAvg = mmxavgAvg(&emcmotDebug->nMmxavg);
02937     }
02938 
02939     /* log per-servo-cycle data if logging active */
02940     logIt = 0;
02941     if (emcmotStatus->logStarted &&
02942         emcmotStatus->logSkip >= 0) {
02943 
02944       /* record type here, since all will set this */
02945       ls.type = emcmotStatus->logType;
02946 
02947       /* now log type-specific data */
02948       switch (ls.type) {
02949 
02950       case EMCMOT_LOG_TYPE_AXIS_POS:
02951         if (logSkip-- <= 0) {
02952           ls.item.axisPos.time = end;
02953           ls.item.axisPos.input = emcmotStatus->input[loggingAxis];
02954           ls.item.axisPos.output = jointPos[loggingAxis];
02955           logSkip = emcmotStatus->logSkip;
02956           logIt = 1;
02957         }
02958         break;
02959 
02960       case EMCMOT_LOG_TYPE_ALL_INPOS:
02961         if (logSkip-- <= 0) {
02962           ls.item.allInpos.time = end;
02963           for (axis = 0;
02964                axis < EMCMOT_LOG_NUM_AXES &&
02965                  axis < EMCMOT_MAX_AXIS;
02966                axis++) {
02967             ls.item.allInpos.input[axis] = emcmotStatus->input[axis];
02968           }
02969           logSkip = emcmotStatus->logSkip;
02970           logIt = 1;
02971         }
02972         break;
02973 
02974       case EMCMOT_LOG_TYPE_ALL_OUTPOS:
02975         if (logSkip-- <= 0) {
02976           ls.item.allOutpos.time = end;
02977           for (axis = 0;
02978                axis < EMCMOT_LOG_NUM_AXES &&
02979                  axis < EMCMOT_MAX_AXIS;
02980                axis++) {
02981             ls.item.allOutpos.output[axis] = jointPos[axis];
02982           }
02983           logSkip = emcmotStatus->logSkip;
02984           logIt = 1;
02985         }
02986         break;
02987 
02988       case EMCMOT_LOG_TYPE_AXIS_VEL:
02989         if (logSkip-- <= 0) {
02990           ls.item.axisVel.time = end;
02991           ls.item.axisVel.cmdVel = jointPos[loggingAxis] -
02992             oldJointPos[loggingAxis];
02993           ls.item.axisVel.actVel = emcmotStatus->input[loggingAxis] -
02994             oldInput[loggingAxis];
02995           logSkip = emcmotStatus->logSkip;
02996           logIt = 1;
02997         }
02998         break;
02999 
03000       case EMCMOT_LOG_TYPE_ALL_FERROR:
03001         if (logSkip-- <= 0) {
03002           ls.item.allFerror.time = end;
03003           for (axis = 0;
03004                axis < EMCMOT_LOG_NUM_AXES &&
03005                  axis < EMCMOT_MAX_AXIS;
03006                axis++) {
03007             ls.item.allFerror.ferror[axis] = thisFerror[axis];
03008           }
03009           logSkip = emcmotStatus->logSkip;
03010           logIt = 1;
03011         }
03012         break;
03013 
03014       case EMCMOT_LOG_TYPE_POS_VOLTAGE:
03015         /* don't do a circular log-- suppress if full */
03016         if (emcmotLog->howmany >= emcmotStatus->logSize) {
03017           emcmotStatus->output[loggingAxis] = 0.0; /* drop the DAC to zero */
03018           emcmotStatus->logStarted = 0; /* stop logging */
03019           logIt = 0;            /* should still be zero, reset anyway */
03020           break;
03021         }
03022         if (logSkip-- <= 0) {
03023           ls.item.posVoltage.time = end;
03024           for (axis = 0;
03025                axis < EMCMOT_LOG_NUM_AXES &&
03026                  axis < EMCMOT_MAX_AXIS;
03027                axis++) {
03028             ls.item.posVoltage.pos = emcmotStatus->input[loggingAxis];
03029             ls.item.posVoltage.voltage = rawOutput[loggingAxis];
03030           }
03031           logSkip = emcmotStatus->logSkip;
03032           logIt = 1;
03033         }
03034         break;
03035 
03036       default:
03037         break;
03038       } /* end of: switch on log type */
03039 
03040       /* now log it */
03041 #if defined (rtlinux) && defined (RT_FIFO)
03042       if (logIt) {
03043         rtf_put(EMCMOT_LOG_RTF, &ls, sizeof(EMCMOT_LOG_STRUCT));
03044         /* FIXME-- update emcmotStatus->logPoints */
03045         logIt = 0;
03046       }
03047 #else
03048       if (logIt) {
03049         emcmotLogAdd(emcmotLog, ls);
03050         emcmotStatus->logPoints = emcmotLog->howmany;
03051         logIt = 0;
03052       }
03053 #endif
03054     } /* end of: if logging */
03055 
03056     /* set tail to head, which has already been incremented */
03057     emcmotStatus->tail = emcmotStatus->head;
03058     emcmotDebug->tail = emcmotDebug->head;
03059     emcmotConfig->tail = emcmotConfig->head;
03060 
03061     /* WRITE STATUS DATA */
03062 
03063 #if defined (rtlinux) && defined (RT_FIFO)
03064     /* write to rtlinux FIFO */
03065     rtf_put(EMCMOT_STATUS_RTF, &emcmotStatusBuffer, sizeof(EMCMOT_STATUS));
03066 
03067 #endif /* rtlinux and RT_FIFO */
03068 
03069 #ifdef rtlinux
03070     /* wait for next cycle */
03071 #ifdef USE_RTL2
03072     pthread_wait_np();
03073 #else
03074     rt_task_wait();
03075 #endif
03076   } /* end of: forever loop for RT task */
03077 #endif /* rtlinux */
03078 #ifdef USE_RTL2
03079   return(NULL);
03080 #endif
03081 } /* end of: emcmotController() function */

Defines
#define	LIMIT_SWITCH_DEBOUNCE 10
#define	AMP_FAULT_DEBOUNCE 100
#define	diagnostics printf
#define	GET_MOTION_ERROR_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ERROR_BIT ? 1 : 0)
#define	SET_MOTION_ERROR_FLAG(fl) if (fl) emcmotStatus->motionFlag \|= EMCMOT_MOTION_ERROR_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ERROR_BIT;
#define	GET_MOTION_COORD_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_COORD_BIT ? 1 : 0)
#define	SET_MOTION_COORD_FLAG(fl) if (fl) emcmotStatus->motionFlag \|= EMCMOT_MOTION_COORD_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_COORD_BIT;
#define	GET_MOTION_INPOS_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_INPOS_BIT ? 1 : 0)
#define	SET_MOTION_INPOS_FLAG(fl) if (fl) emcmotStatus->motionFlag \|= EMCMOT_MOTION_INPOS_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_INPOS_BIT;
#define	GET_MOTION_ENABLE_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ENABLE_BIT ? 1 : 0)
#define	SET_MOTION_ENABLE_FLAG(fl) if (fl) emcmotStatus->motionFlag \|= EMCMOT_MOTION_ENABLE_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ENABLE_BIT;
#define	GET_AXIS_ENABLE_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_ENABLE_BIT ? 1 : 0)
#define	SET_AXIS_ENABLE_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_ENABLE_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_ENABLE_BIT;
#define	GET_AXIS_ACTIVE_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_ACTIVE_BIT ? 1 : 0)
#define	SET_AXIS_ACTIVE_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_ACTIVE_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_ACTIVE_BIT;
#define	GET_AXIS_INPOS_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_INPOS_BIT ? 1 : 0)
#define	SET_AXIS_INPOS_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_INPOS_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_INPOS_BIT;
#define	GET_AXIS_ERROR_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_ERROR_BIT ? 1 : 0)
#define	SET_AXIS_ERROR_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_ERROR_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_ERROR_BIT;
#define	GET_AXIS_PSL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT ? 1 : 0)
#define	SET_AXIS_PSL_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MAX_SOFT_LIMIT_BIT;
#define	GET_AXIS_NSL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT ? 1 : 0)
#define	SET_AXIS_NSL_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MIN_SOFT_LIMIT_BIT;
#define	GET_AXIS_PHL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MAX_HARD_LIMIT_BIT ? 1 : 0)
#define	SET_AXIS_PHL_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_MAX_HARD_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MAX_HARD_LIMIT_BIT;
#define	GET_AXIS_NHL_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_MIN_HARD_LIMIT_BIT ? 1 : 0)
#define	SET_AXIS_NHL_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_MIN_HARD_LIMIT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_MIN_HARD_LIMIT_BIT;
#define	GET_AXIS_HOME_SWITCH_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_HOME_SWITCH_BIT ? 1 : 0)
#define	SET_AXIS_HOME_SWITCH_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_HOME_SWITCH_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_HOME_SWITCH_BIT;
#define	GET_AXIS_HOMING_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_HOMING_BIT ? 1 : 0)
#define	SET_AXIS_HOMING_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_HOMING_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_HOMING_BIT;
#define	GET_AXIS_HOMED_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_HOMED_BIT ? 1 : 0)
#define	SET_AXIS_HOMED_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_HOMED_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_HOMED_BIT;
#define	GET_AXIS_FERROR_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_FERROR_BIT ? 1 : 0)
#define	SET_AXIS_FERROR_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_FERROR_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_FERROR_BIT;
#define	GET_AXIS_FAULT_FLAG(ax) (emcmotStatus->axisFlag[ax] & EMCMOT_AXIS_FAULT_BIT ? 1 : 0)
#define	SET_AXIS_FAULT_FLAG(ax, fl) if (fl) emcmotStatus->axisFlag[ax] \|= EMCMOT_AXIS_FAULT_BIT; else emcmotStatus->axisFlag[ax] &= ~EMCMOT_AXIS_FAULT_BIT;
#define	GET_AXIS_ENABLE_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_ENABLE_BIT ? 1 : 0)
#define	SET_AXIS_ENABLE_POLARITY(ax, fl) if (fl) emcmotConfig->axisPolarity[ax] \|= EMCMOT_AXIS_ENABLE_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_ENABLE_BIT;
#define	GET_AXIS_PHL_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_MAX_HARD_LIMIT_BIT ? 1 : 0)
#define	SET_AXIS_PHL_POLARITY(ax, fl) if (fl) emcmotConfig->axisPolarity[ax] \|= EMCMOT_AXIS_MAX_HARD_LIMIT_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_MAX_HARD_LIMIT_BIT;
#define	GET_AXIS_NHL_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_MIN_HARD_LIMIT_BIT ? 1 : 0)
#define	SET_AXIS_NHL_POLARITY(ax, fl) if (fl) emcmotConfig->axisPolarity[ax] \|= EMCMOT_AXIS_MIN_HARD_LIMIT_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_MIN_HARD_LIMIT_BIT;
#define	GET_AXIS_HOME_SWITCH_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_HOME_SWITCH_BIT ? 1 : 0)
#define	SET_AXIS_HOME_SWITCH_POLARITY(ax, fl) if (fl) emcmotConfig->axisPolarity[ax] \|= EMCMOT_AXIS_HOME_SWITCH_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_HOME_SWITCH_BIT;
#define	GET_AXIS_HOMING_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_HOMING_BIT ? 1 : 0)
#define	SET_AXIS_HOMING_POLARITY(ax, fl) if (fl) emcmotConfig->axisPolarity[ax] \|= EMCMOT_AXIS_HOMING_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_HOMING_BIT;
#define	GET_AXIS_FAULT_POLARITY(ax) (emcmotConfig->axisPolarity[ax] & EMCMOT_AXIS_FAULT_BIT ? 1 : 0)
#define	SET_AXIS_FAULT_POLARITY(ax, fl) if (fl) emcmotConfig->axisPolarity[ax] \|= EMCMOT_AXIS_FAULT_BIT; else emcmotConfig->axisPolarity[ax] &= ~EMCMOT_AXIS_FAULT_BIT;
#define	FREE_AXIS_QUEUE_SIZE 4
#define	AXRANGE(axis) ((emcmotConfig->maxLimit[axis] - emcmotConfig->minLimit[axis]))
Functions
char	__attribute__ ((unused)) ident[]="$Id
void	MARK_EMCMOT_CONFIG_CHANGE (void)
void	reportError (const char *fmt,...)
int	isHoming (void)
int	inRange (EmcPose pos)
int	checkLimits (void)
int	checkJog (int axis, double vel)
void	setTrajCycleTime (double secs)
void	setServoCycleTime (double secs)
int	emcmotCommandHandler (void)
void	emcmotController (int arg)
int	init_module (void)
void	cleanup_module (void)
void	emcmot_quit (int sig)
int	getArgs (int argc, char *argv[])
int	main (int argc, char *argv[])
Variables
EMCMOT_STRUCT *	emcmotStruct
shm_t *	shmem = NULL
EMCMOT_COMMAND *	emcmotCommand
EMCMOT_STATUS *	emcmotStatus
EMCMOT_CONFIG *	emcmotConfig
EMCMOT_DEBUG *	emcmotDebug
EMCMOT_ERROR *	emcmotError
EMCMOT_LOG *	emcmotLog
EMCMOT_COMP *	emcmotComp [EMCMOT_MAX_AXIS]
EMCMOT_LOG_STRUCT	ls
int	logSkip = 0
int	loggingAxis = 0
int	logIt = 0
int	wdEnabling = 0
int	wdEnabled = 0
int	wdWait = 0
int	wdCount = 0
unsigned char	wdToggle = 0
unsigned char	allHomed = 0
double	jointHome [EMCMOT_MAX_AXIS]
EmcPose	worldHome
KINEMATICS_FORWARD_FLAGS	fflags = 0
KINEMATICS_INVERSE_FLAGS	iflags = 0
int	kinType = 0
int	maxLimitSwitchCount [EMCMOT_MAX_AXIS]
int	minLimitSwitchCount [EMCMOT_MAX_AXIS]
int	ampFaultCount [EMCMOT_MAX_AXIS]
TP_STRUCT	queue
TP_STRUCT	freeAxis [EMCMOT_MAX_AXIS]
EmcPose	freePose = {{0.0, 0.0, 0.0}, 0.0, 0.0, 0.0}
CUBIC_STRUCT	cubic [EMCMOT_MAX_AXIS]
TC_STRUCT	queueTcSpace [DEFAULT_TC_QUEUE_SIZE+10]
TC_STRUCT	freeAxisTcSpace [EMCMOT_MAX_AXIS][FREE_AXIS_QUEUE_SIZE]
double	rawInput [EMCMOT_MAX_AXIS]
double	rawOutput [EMCMOT_MAX_AXIS]
double	coarseJointPos [EMCMOT_MAX_AXIS]
double	jointPos [EMCMOT_MAX_AXIS]
double	jointVel [EMCMOT_MAX_AXIS]
double	oldJointPos [EMCMOT_MAX_AXIS]
double	oldInput [EMCMOT_MAX_AXIS]
double	inverseInputScale [EMCMOT_MAX_AXIS]
double	inverseOutputScale [EMCMOT_MAX_AXIS]
EmcPose	oldPos
EmcPose	oldVel
EmcPose	newVel
EmcPose	newAcc
double	bigVel = 1.0
int	waitingForLatch [EMCMOT_MAX_AXIS]
int	latchFlag [EMCMOT_MAX_AXIS]
double	saveLatch [EMCMOT_MAX_AXIS]
int	enabling = 0
int	coordinating = 0
int	wasOnLimit = 0
int	onLimit = 0
int	overriding = 0
int	stepping = 0
int	idForStep = 0
int	interpolationCounter = 0
int	EMCMOT_NO_FORWARD_KINEMATICS = 0
int	emcmot_done = 0

emcstepmot.c File Reference

Defines

Functions

Variables

Define Documentation

Function Documentation

Variable Documentation