emcmot.c File Reference

00645 {
00646   if(emcmotConfig->head == emcmotConfig->tail) { 
00647     emcmotConfig->config_num++;
00648     emcmotStatus->config_num = emcmotConfig->config_num;
00649     emcmotConfig->head++;
00650   }
00651 }

00437                                                   : emcmot.c,v 1.56 2001/11/08 23:55:47 paul_c Exp $";
00438 
00439 #ifdef rtai
00440 static RTIME rtaiTickPeriod,now;
00441 static long rtaiTickPeriod_long;
00442 #endif
00443 
00444 #ifdef STEPPER_MOTORS
00445 
00446 /* FIXME-- shouldn't hard code parallel port, use ext intf instead */
00447 #include "parport.h"
00448 
00449 /* value of compensator output below which it's effectively off, so that
00450    the frequency generator can be turned off */
00451 #define OUTPUT_DELTA 1.0e-6
00452 
00453 #endif /* STEPPER_MOTORS */
00454 
00455 #if ((defined(rtlinux2) || defined(RTLINUX_V2)) && !defined(CONFIG_RTL_USE_V1_API)) || defined(RTLINUX_V3)
00456 #define USE_RTL2
00457 #endif
00458 
00459 #ifdef USE_RTL2
00460 #include <rtl.h>
00461 #include <time.h>
00462 #include "mbuff.h"
00463 #endif
00464 
00465 #ifdef SIMULATED_MOTORS
00466 #include "sim.h"                /* simMotInit() */
00467 #endif
00468 
00469 /* lpg changed period from 20000 to avoid flattening a P133                */
00470 /* this is programmable in the .ini file, so faster boxes can use 16us     */
00471 static int PERIOD = 48000;      /* fundamental period for timer interrupts */
00472 
00473 #ifndef STEPPER_MOTORS
00474 extern unsigned short STG_BASE_ADDRESS;
00475 extern int FIND_STG_BASE_ADDRESS;
00476 #else
00477 extern unsigned long int PARPORT_IO_ADDRESS;
00478 
00479 #define DEFAULT_FREQ_TASK_PRIORITY 1
00480 #define DEFAULT_FREQ_TASK_STACK_SIZE 4096
00481 
00482 static int FREQ_TASK_PRIORITY = DEFAULT_FREQ_TASK_PRIORITY;
00483 static int FREQ_TASK_STACK_SIZE = DEFAULT_FREQ_TASK_STACK_SIZE;
00484 #endif
00485 
00486 #ifdef rtlinux
00487 #ifdef USE_RTL2
00488 static hrtime_t rtperiod;
00489 #else
00490 
00491 static RTIME rtstart;
00492 static RTIME rtperiod;
00493 
00494 /* useful conversion from secs to RTIME */
00495 /* RT_TICKS_PER_SEC is 1193180, BTW */
00496 #define SECS_TO_RTIME(secs) ((RTIME) (RT_TICKS_PER_SEC * secs))
00497 
00498 #endif
00499 #endif
00500 
00501 #if defined(rtlinux) || defined(rtai)
00502 static int iperiod;
00503 #endif
00504 
00505 #ifdef rtai
00506 static RTIME iperiod_rtime;
00507 #endif
00508 
00509 /* task struct */
00510 #if defined(rtlinux)|| defined(rtai)
00511 #ifdef rtai
00512 static RT_TASK emcmotTask;
00513 #else
00514 #ifdef USE_RTL2
00515 static pthread_t emcmotTask;
00516 #else
00517 static RT_TASK emcmotTask;
00518 #endif
00519 #endif
00520 
00521 #ifdef STEPPER_MOTORS
00522 #ifdef rtai
00523 static RT_TASK freqTask;
00524 #else
00525 #ifdef USE_RTL2
00526 static pthread_t freqTask;
00527 #else
00528 static RT_TASK freqTask;
00529 #endif
00530 #endif
00531 #endif
00532 #endif
00533 
00534 static int emcmotTask_created = 0;
00535 static int freqTask_created = 0;
00536 
00537 #define DEFAULT_EMCMOT_TASK_PRIORITY 2      /* the highest is 1, the lowest is
00538                                    RT_LOWEST_PRIORITY */
00539 #define DEFAULT_EMCMOT_TASK_STACK_SIZE 8192
00540 static int EMCMOT_TASK_PRIORITY=DEFAULT_EMCMOT_TASK_PRIORITY;
00541 static int EMCMOT_TASK_STACK_SIZE=DEFAULT_EMCMOT_TASK_STACK_SIZE;
00542 
00543 /* for RT-Linux, can have watchdog sound port, parallel port bit toggling */
00544 #define SOUND_PORT 0x61
00545 #define SOUND_MASK 0x02
00546 #if defined(rtlinux) || defined(rtai)
00547 static unsigned char soundByte;
00548 #endif
00549 
00550 /*
00551   Principles of communication:
00552 
00553   Data is copied in or out from the various types of comm mechanisms:
00554   mbuff mapped memory for Linux/RT-Linux, or OS shared memory for Unixes.
00555 
00556   emcmotStruct is ptr to this memory.
00557 
00558   emcmotCommand points to emcmotStruct->command,
00559   emcmotStatus points to emcmotStruct->status,
00560   emcmotError points to emcmotStruct->error, and
00561   emcmotLog points to emcmotStruct->log.
00562   emcmotComp[] points to emcmotStruct->comp[].
00563  */
00564 
00565 /* need to point to command, status, error, and log */
00566 EMCMOT_STRUCT *emcmotStruct;
00567 
00568 #ifdef rtlinux
00569 #ifndef realtimeonly
00570 #define realtimeonly
00571 #endif
00572 #endif
00573 
00574 #ifdef rtai
00575 #ifndef realtimeonly
00576 #define realtimeonly
00577 #endif
00578 #endif
00579 
00580 #ifndef realtimeonly
00581 /* also need to get shared memory id */
00582 static shm_t *shmem = NULL;
00583 #endif
00584 
00585 /* ptrs to either buffered copies or direct memory for
00586    command and status */
00587 static EMCMOT_COMMAND *emcmotCommand;
00588 static EMCMOT_STATUS *emcmotStatus;
00589 static EMCMOT_STATUS *emcmotStatus;
00590 static EMCMOT_CONFIG *emcmotConfig;
00591 static EMCMOT_DEBUG *emcmotDebug;
00592 static EMCMOT_ERROR *emcmotError; /* unused for RT_FIFO */
00593 static EMCMOT_LOG *emcmotLog;   /* unused for RT_FIFO */
00594 static EMCMOT_COMP *emcmotComp[EMCMOT_MAX_AXIS]; /* unused for RT_FIFO */
00595 
00596 static EMCMOT_LOG_STRUCT ls;
00597 static int logSkip = 0;         /* how many to skip, for per-cycle logging */
00598 static int loggingAxis = 0;     /* record of which axis to log */
00599 static int logIt = 0;
00600 static int logStartTime;        /* set when logging is started, and subtracted
00601                                    off each log time for better resolution */
00602 
00603 /* value for world home position */
00604 static EmcPose worldHome = {{0.0, 0.0, 0.0},
00605                            0.0, 0.0, 0.0};

02171 {
02172   int index;
02173   double avgint;
02174   double compin;
02175   int lower, upper;
02176   int total;
02177   double *nominal;
02178   double *ptr;
02179   double denom;
02180 
02181   /* first adjust nominput by the alter value, before looking it up */
02182   nominput += emcmotComp[axis]->alter;
02183 
02184   total = emcmotComp[axis]->total;
02185   if (total < 2) {
02186     return nominput;
02187   }
02188   avgint = emcmotComp[axis]->avgint;
02189   index = (int) (nominput / avgint);
02190   nominal = emcmotComp[axis]->nominal;
02191   if (dir < 0) {
02192     ptr = emcmotComp[axis]->reverse;
02193   }
02194   else {
02195     ptr = emcmotComp[axis]->forward;
02196   }
02197 
02198   /* set the comp input to the nominput, by default */
02199   compin = nominput;
02200   lower = upper = 0;
02201   while (index >= 0 && index < total) {
02202     if (nominput > nominal[index]) {
02203       if (index == total - 1) {
02204         /* off the top */
02205         break;
02206       }
02207       else if (nominput <= nominal[index + 1]) {
02208         /* in range */
02209         lower = index;
02210         upper = index + 1;
02211         denom = nominal[upper] - nominal[lower];
02212         if (denom < DBL_MIN) {
02213           compin = nominput;
02214           /* error */
02215         }
02216         else {
02217           compin = ptr[lower] + (nominput - nominal[lower]) * (ptr[upper] - ptr[lower]) / denom;
02218         }
02219         break;
02220       }
02221       else {
02222         /* index too low */
02223         index++;
02224         continue;
02225       }
02226     }
02227     else if (nominput < nominal[index]) {
02228       if (index == 0) {
02229         /* off the bottom */
02230         break;
02231       }
02232       else if (nominput >= nominal[index - 1]) {
02233         /* in range */
02234         lower = index - 1;
02235         upper = index;
02236         denom = nominal[upper] - nominal[lower];
02237         if (denom < DBL_MIN) {
02238           compin = nominput;
02239           /* error */
02240         }
02241         else {
02242           compin = ptr[lower] + (nominput - nominal[lower]) * (ptr[upper] - ptr[lower]) / denom;
02243         }
02244         break;
02245       }
02246       else {
02247         /* index too high */
02248         index--;
02249         continue;
02250       }
02251     }
02252     else {
02253       /* nominput == nominal[index], so return ptr[index] */
02254       compin = ptr[index];
02255       lower = index;
02256       upper = index;
02257       break;
02258     }
02259   }
02260 
02261   return compin;
02262 }

00956 {
00957   if (emcmotStatus->overrideLimits) {
00958     return 1;                   /* okay to jog when limits overridden */
00959   }
00960 
00961   if (axis < 0 ||
00962       axis >= EMCMOT_MAX_AXIS) {
00963     reportError("Can't jog out of range axis %d.",axis);
00964     return 0;                   /* can't jog out-of-range axis */
00965   }
00966 
00967   if (vel > 0.0 &&
00968       GET_AXIS_PSL_FLAG(axis) )
00969   { 
00970     reportError("Can't jog axis %d further past max soft limit.",axis);
00971     return 0;
00972   }
00973 
00974   if (vel > 0.0 &&
00975       GET_AXIS_PHL_FLAG(axis) )
00976   {
00977     reportError("Can't jog axis %d further past max hard limit.",axis);
00978     return 0;
00979   }
00980 
00981   if (vel < 0.0 &&
00982       GET_AXIS_NSL_FLAG(axis) )
00983   {
00984     reportError("Can't jog axis %d further past min soft limit.",axis);
00985     return 0;
00986   }
00987 
00988   if (vel < 0.0 &&
00989       GET_AXIS_NHL_FLAG(axis) )
00990   { 
00991     reportError("Can't jog axis %d further past min hard limit.",axis);
00992     return 0;
00993   }
00994 
00995   /* okay to jog */
00996   return 1;
00997 }

00931 {
00932   int axis;
00933 
00934   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
00935     if (! GET_AXIS_ACTIVE_FLAG(axis)) {
00936       /* if axis is not active, don't even look at its limits */
00937       continue;
00938     }
00939 
00940     if (GET_AXIS_PSL_FLAG(axis) ||
00941         GET_AXIS_NSL_FLAG(axis) ||
00942         GET_AXIS_PHL_FLAG(axis) ||
00943         GET_AXIS_NHL_FLAG(axis)) {
00944       return 0;
00945     }
00946   }
00947 
00948   return 1;
00949 }

04997 {
04998   int axis;
04999 
05000   diagnostics("emcmot: cleanup started.\n");
05001 
05002 #if 0
05003   /* WPS - There is no mutex mechanism to allow us to modify 
05004      this memory without possibly interfering with the running 
05005      tasks.
05006      After the tasks are deleted but before the the memory is freed we 
05007      will disable amps and run the quits.*/
05008   /* release traj planner space */
05009   if( 0 != emcmotStruct)
05010     {
05011       if(0 != emcmotDebug ) 
05012         {
05013           tpDelete(&emcmotDebug->queue);
05014         }
05015       
05016       /* disable amps */
05017       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
05018         extAmpEnable(axis,
05019                      ! GET_AXIS_ENABLE_POLARITY(axis));
05020       }
05021       
05022       /* quit board */
05023       extAioQuit();
05024       extDioQuit();
05025       extMotQuit();
05026     }
05027 #endif
05028   
05029 #ifdef rtai
05030   stop_rt_timer();
05031   if(emcmotTask_created)
05032     {
05033       diagnostics("emcmot: deleting emcmotTask.\n");
05034       rt_task_delete(&emcmotTask);
05035       emcmotTask_created=0;
05036     }
05037 #ifdef STEPPER_MOTORS
05038   if(freqTask_created)
05039     {
05040       diagnostics("emcmot: deleting freqTask.\n");
05041       rt_task_delete(&freqTask);
05042       freqTask_created=0;
05043     }
05044 #endif
05045 #endif
05046 
05047   /* delete control task */
05048 #ifdef rtlinux
05049 #ifdef USE_RTL2
05050   pthread_delete_np(emcmotTask);
05051 #else
05052   rt_task_delete(&emcmotTask);
05053 #endif
05054 
05055 #ifdef STEPPER_MOTORS
05056   /* delete stepper motor task */
05057 #ifdef USE_RTL2
05058   pthread_delete_np(freqTask);
05059 #else
05060   rt_task_delete(&freqTask);
05061 #endif
05062 #endif 
05063 #endif
05064 
05065   /* WPS these were moved from above to avoid a possible mutex problem.*/
05066   /* There is no point in clearing the trajectory queue since the
05067      planner should be dead by now anyway. */
05068   if( emcmotStruct != 0 && emcmotDebug != 0 && emcmotConfig != 0)
05069     {
05070       diagnostics("emcmot: disabling amps\n");
05071       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
05072         extAmpEnable(axis,
05073                  ! GET_AXIS_ENABLE_POLARITY(axis));
05074       }
05075       
05076       /* quit board */
05077       diagnostics("emcmot: quitting analog, digital and motor interfaces\n");
05078       extAioQuit();
05079       extDioQuit();
05080       extMotQuit();
05081     }
05082 
05083   /* free shared memory */
05084 #ifdef rtai
05085   if(emcmotStruct != 0)
05086     {
05087       diagnostics("emcmot: releasing rtai shared memory.\n");
05088       rtai_kfree(SHMEM_KEY);
05089       emcmotStruct = 0;
05090     }
05091 #endif
05092 
05093 #ifdef rtlinux
05094 #ifdef USE_RTL2
05095   mbuff_free("emcmotStruct", emcmotStruct);
05096 #else
05097 #if defined(rtlinux2)
05098   iounmap(emcmotStruct);
05099 #else
05100   vfree(emcmotStruct);
05101 #endif
05102 #endif
05103 
05104   /* restore oneshot mode */
05105 #ifdef USE_RTL2
05106   rtl_setclockmode(local_clock, RTL_CLOCK_MODE_ONESHOT,0);
05107 #else
05108   rtl_set_oneshot_mode();
05109 #endif
05110 #endif
05111 
05112 #if !defined(rtlinux) && !defined(rtai)
05113   /* detach from shmem */
05114   if (NULL != shmem) {
05115     rcs_shm_close(shmem);
05116     shmem = NULL;
05117   }
05118 
05119 #endif
05120 
05121 #ifdef rtai
05122   rt_umount_rtai();
05123 #endif
05124 
05125   /* FIXME-- testing */
05126   diagnostics("emcmot: debounce counts: ");
05127   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
05128     diagnostics("%d ", debouncecount[axis]);
05129   }
05130   diagnostics("\n");
05131 
05132   diagnostics("emcmot: cleanup finished.\n");
05133 }

00876 {
00877   int t;
00878 
00879   if (kinType == KINEMATICS_INVERSE_ONLY) {
00880     if (rehomeAll) {
00881       for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
00882         SET_AXIS_HOMED_FLAG(t, 0);
00883       }
00884     }
00885     else {
00886       SET_AXIS_HOMED_FLAG(axis, 0);
00887     }
00888   }
00889   if (NULL != emcmotDebug) {
00890     emcmotDebug->allHomed = 0;
00891   }
00892 }

01079 {
01080   int axis;
01081   int valid;
01082 
01083   /* check for split read */
01084   if (emcmotCommand->head != emcmotCommand->tail) {
01085     emcmotDebug->split++;
01086     return 0;                 /* not really an error */
01087   }
01088 
01089   if (emcmotCommand->commandNum != emcmotStatus->commandNumEcho) {
01090     /* increment head count-- we'll be modifying emcmotStatus */
01091     emcmotStatus->head++;
01092     emcmotDebug->head++;
01093 
01094     /* got a new command-- echo command and number... */
01095     emcmotStatus->commandEcho = emcmotCommand->command;
01096     emcmotStatus->commandNumEcho = emcmotCommand->commandNum;
01097 
01098     /* clear status value by default */
01099     emcmotStatus->commandStatus = EMCMOT_COMMAND_OK;
01100 
01101     /* log it, if appropriate */
01102     if (emcmotStatus->logStarted &&
01103         emcmotStatus->logType == EMCMOT_LOG_TYPE_CMD) {
01104       ls.item.cmd.time = etime(); /* don't subtract off logStartTime,
01105                                      since we want an absolute time value */
01106       ls.item.cmd.command = emcmotCommand->command;
01107       ls.item.cmd.commandNum = emcmotCommand->commandNum;
01108       emcmotLogAdd(emcmotLog, ls);
01109       emcmotStatus->logPoints = emcmotLog->howmany;
01110     }
01111 
01112     /* ...and process command */
01113     switch (emcmotCommand->command) {
01114     case EMCMOT_FREE:
01115       /* change the mode to free axis motion */
01116       /* can be done at any time */
01117       /* reset the emcmotDebug->coordinating flag to defer transition to
01118          controller cycle */
01119       emcmotDebug->coordinating = 0;
01120       emcmotDebug->teleoperating = 0;
01121       break;
01122 
01123     case EMCMOT_COORD:
01124       /* change the mode to coordinated axis motion */
01125       /* can be done at any time */
01126       /* set the emcmotDebug->coordinating flag to defer transition to
01127          controller cycle */
01128       emcmotDebug->coordinating = 1;
01129       emcmotDebug->teleoperating = 0;
01130       if (kinType != KINEMATICS_IDENTITY) {
01131         if (!emcmotDebug->allHomed) {
01132           reportError("all axes must be homed before going into coordinated mode");
01133           emcmotDebug->coordinating = 0;
01134           break;
01135         }
01136       }
01137       break;
01138 
01139     case EMCMOT_TELEOP:
01140       /* change the mode to teleop motion */
01141       /* can be done at any time */
01142       /* set the emcmotDebug->teleoperating flag to defer transition to
01143          controller cycle */
01144       emcmotDebug->teleoperating = 1;
01145       if (kinType != KINEMATICS_IDENTITY) {
01146         if (! emcmotDebug->allHomed) {
01147           reportError("all axes must be homed before going into teleop mode");
01148           emcmotDebug->teleoperating = 0;
01149           break;
01150         }
01151 
01152       }
01153       break;
01154 
01155     case EMCMOT_SET_NUM_AXES:
01156       /* set the global NUM_AXES, which must be between 1 and
01157          EMCMOT_MAX_AXIS, inclusive */
01158       axis = emcmotCommand->axis;
01159       /* note that this comparison differs from the check on the
01160          range of 'axis' in most other places, since those checks
01161          are for a value to be used as an index and here it's a value
01162          to be used as a counting number. The indenting is different
01163          here so as not to match macro editing on that other bunch. */
01164       if (axis <= 0 ||
01165           axis > EMCMOT_MAX_AXIS) {
01166         break;
01167       }
01168       NUM_AXES = axis;
01169       break;
01170 
01171     case EMCMOT_SET_WORLD_HOME:
01172       worldHome = emcmotCommand->pos;
01173       break;
01174 
01175     case EMCMOT_SET_JOINT_HOME:
01176       axis = emcmotCommand->axis;
01177       if (axis < 0 ||
01178           axis >= EMCMOT_MAX_AXIS) {
01179         break;
01180       }
01181       emcmotDebug->jointHome[axis] = emcmotCommand->offset; /* FIXME-- use 'home' instead */
01182       break;
01183 
01184     case EMCMOT_SET_HOME_OFFSET:
01185       MARK_EMCMOT_CONFIG_CHANGE();
01186       axis = emcmotCommand->axis;
01187       if (axis < 0 ||
01188           axis >= EMCMOT_MAX_AXIS) {
01189         break;
01190       }
01191       emcmotConfig->homeOffset[axis] = emcmotCommand->offset;
01192       break;
01193 
01194     case EMCMOT_OVERRIDE_LIMITS:
01195       if (emcmotCommand->axis < 0) {
01196         /* don't override limits */
01197         emcmotStatus->overrideLimits = 0;
01198       }
01199       else {
01200         emcmotStatus->overrideLimits = 1;
01201       }
01202       emcmotDebug->overriding = 0;
01203       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01204         SET_AXIS_ERROR_FLAG(axis, 0);
01205       }
01206       break;
01207 
01208     case EMCMOT_SET_TRAJ_CYCLE_TIME:
01209       /* set the cycle time for trajectory calculations */
01210       /* really should be done only at startup before
01211          controller is run, but at least it requires
01212          no active motions and the interpolators need
01213          to be cleared */
01214       setTrajCycleTime(emcmotCommand->cycleTime);
01215       break;
01216 
01217     case EMCMOT_SET_SERVO_CYCLE_TIME:
01218       /* set the cycle time for servo calculations, which is the
01219          period for emcmotController execution */
01220       /* really should be done only at startup before
01221          controller is run, but at least it requires
01222          no active motions and the interpolators need
01223          to be cleared */
01224 #ifdef rtai
01225       if(rtaiTickPeriod > 0)
01226         {
01227           rtaiTickPeriod_long= (long) rtaiTickPeriod;
01228           iperiod = nano2count((int) (1e9* emcmotCommand->cycleTime));
01229           
01230           if ( iperiod < rtaiTickPeriod_long) 
01231             {
01232               iperiod = rtaiTickPeriod_long;
01233             }
01234           iperiod /= rtaiTickPeriod_long;
01235           iperiod *= rtaiTickPeriod_long;
01236           iperiod_rtime = (RTIME) iperiod;
01237           now = rt_get_time();
01238           rt_task_make_periodic(&emcmotTask,now+rtaiTickPeriod,iperiod_rtime);
01239           setServoCycleTime(count2nano(iperiod)*1e-9);
01240         }
01241 #else
01242 
01243 #ifdef rtlinux
01244 #ifdef USE_RTL2
01245       iperiod = (HRTICKS_PER_SEC * emcmotCommand->cycleTime);
01246 #else
01247       iperiod = (RT_TICKS_PER_SEC * emcmotCommand->cycleTime);
01248 #endif
01249 
01250       /* make it an even multiple of fundamental period */
01251       iperiod = iperiod / PERIOD;
01252       iperiod = iperiod * PERIOD;
01253       if (iperiod < PERIOD) {
01254         iperiod = PERIOD;
01255       }
01256 #ifdef USE_RTL2
01257       rtperiod = (hrtime_t) iperiod;
01258 #else
01259       rtperiod = (RTIME) iperiod;
01260 #endif
01261 
01262 #ifdef USE_RTL2
01263       pthread_make_periodic_np(emcmotTask,
01264                                gethrtime() + rtperiod,
01265                                rtperiod);
01266 #else
01267       rt_task_make_periodic(&emcmotTask,
01268                             rt_get_time() + rtperiod,
01269                             rtperiod);
01270 #endif
01271 
01272 #ifdef USE_RTL2
01273       setServoCycleTime(((double) iperiod) / ((double) HRTICKS_PER_SEC));
01274 #else
01275       setServoCycleTime(((double) iperiod) / ((double) RT_TICKS_PER_SEC));
01276 #endif
01277 #else
01278       setServoCycleTime(emcmotCommand->cycleTime);
01279 #endif /* rtlinux */
01280 #endif /* rtai*/
01281       break;
01282 
01283     case EMCMOT_SET_POSITION_LIMITS:
01284       MARK_EMCMOT_CONFIG_CHANGE();
01285       /* set the position limits for the axis */
01286       /* can be done at any time */
01287       axis = emcmotCommand->axis;
01288       if (axis < 0 ||
01289           axis >= EMCMOT_MAX_AXIS) {
01290         break;
01291       }
01292       emcmotConfig->minLimit[axis] = emcmotCommand->minLimit;
01293       emcmotConfig->maxLimit[axis] = emcmotCommand->maxLimit;
01294       break;
01295 
01296     case EMCMOT_SET_OUTPUT_LIMITS:
01297       MARK_EMCMOT_CONFIG_CHANGE();
01298       /* set the output limits for the axis */
01299       /* can be done at any time */
01300       axis = emcmotCommand->axis;
01301       if (axis < 0 ||
01302           axis >= EMCMOT_MAX_AXIS) {
01303         break;
01304       }
01305 
01306       emcmotConfig->minOutput[axis] = emcmotCommand->minLimit;
01307       emcmotConfig->maxOutput[axis] = emcmotCommand->maxLimit;
01308       break;
01309 
01310     case EMCMOT_SET_OUTPUT_SCALE:
01311       axis = emcmotCommand->axis;
01312       if (axis < 0 ||
01313           axis >= EMCMOT_MAX_AXIS ||
01314           emcmotCommand->scale == 0) {
01315         break;
01316       }
01317       emcmotStatus->outputScale[axis] = emcmotCommand->scale;
01318       emcmotStatus->outputOffset[axis] = emcmotCommand->offset;
01319       emcmotDebug->inverseOutputScale[axis] = 1.0 / emcmotStatus->outputScale[axis];
01320       break;
01321 
01322     case EMCMOT_SET_INPUT_SCALE:
01323       /*
01324         change the scale factor for the position input, e.g.,
01325         encoder counts per unit. Note that this is not a good idea
01326         once things have gotten underway, since the axis will
01327         jump servo to the "new" position, the gains will no longer
01328         be appropriate, etc.
01329       */
01330       axis = emcmotCommand->axis;
01331       if (axis < 0 ||
01332           axis >= EMCMOT_MAX_AXIS ||
01333           emcmotCommand->scale == 0.0) {
01334         break;
01335       }
01336 #if 0
01337       /* adjust last saved input value to match this one, so we
01338          don't get a spurious following error */
01339       emcmotDebug->oldInput[axis] = emcmotDebug->oldInput[axis] * emcmotStatus->inputScale[axis] +
01340         emcmotStatus->inputOffset[axis]; /* temp calc */
01341       emcmotDebug->oldInput[axis] = (emcmotDebug->oldInput[axis] - emcmotCommand->offset) /
01342         emcmotCommand->scale;
01343 #endif
01344       emcmotDebug->oldInputValid[axis] = 0;
01345 
01346       /* now make them active */
01347       emcmotStatus->inputScale[axis] = emcmotCommand->scale;
01348       emcmotStatus->inputOffset[axis] = emcmotCommand->offset;
01349       emcmotDebug->inverseInputScale[axis] = 1.0 / emcmotStatus->inputScale[axis];
01350 
01351       break;
01352 
01353       /*
01354         Max and min ferror work like this:
01355         limiting ferror is determined by slope of ferror line,
01356         = maxFerror/limitVel
01357         -> limiting ferror = maxFerror/limitVel * vel.
01358         If ferror < minFerror then OK else
01359         if ferror < limiting ferror then OK else ERROR
01360       */
01361 
01362     case EMCMOT_SET_MAX_FERROR:
01363       MARK_EMCMOT_CONFIG_CHANGE();
01364       axis = emcmotCommand->axis;
01365       if (axis < 0 ||
01366           axis >= EMCMOT_MAX_AXIS ||
01367           emcmotCommand->maxFerror < 0.0) {
01368         break;
01369       }
01370       emcmotConfig->maxFerror[axis] = emcmotCommand->maxFerror;
01371       break;
01372 
01373     case EMCMOT_SET_MIN_FERROR:
01374       MARK_EMCMOT_CONFIG_CHANGE();
01375       axis = emcmotCommand->axis;
01376       if (axis < 0 ||
01377           axis >= EMCMOT_MAX_AXIS ||
01378           emcmotCommand->minFerror < 0.0) {
01379         break;
01380       }
01381       emcmotConfig->minFerror[axis] = emcmotCommand->minFerror;
01382       break;
01383 
01384     case EMCMOT_JOG_CONT:
01385       /* do a continuous jog, implemented as an incremental
01386          jog to the software limit, or the full range of travel
01387          if software limits don't yet apply because we're not homed */
01388 
01389       /* check axis range */
01390       axis = emcmotCommand->axis;
01391       if (axis < 0 ||
01392           axis >= EMCMOT_MAX_AXIS) {
01393         break;
01394       }
01395 
01396       /* requires no motion, in free mode, enable on */
01397       if (GET_MOTION_COORD_FLAG() )
01398         {
01399           reportError("Can't jog axis in coordinated mode.");
01400           SET_AXIS_ERROR_FLAG(axis, 1);
01401           break;
01402         }
01403 
01404       if( ! GET_MOTION_INPOS_FLAG() )
01405         {
01406           reportError("Can't jog axis when not in position.");
01407           SET_AXIS_ERROR_FLAG(axis, 1);
01408           break;
01409         }
01410 
01411       if( ! GET_MOTION_ENABLE_FLAG()) {
01412         reportError("Can't jog axis when not enabled.");
01413         SET_AXIS_ERROR_FLAG(axis, 1);
01414         break;
01415       }
01416 
01417       /* don't jog further onto limits */
01418       if (! checkJog(axis, emcmotCommand->vel)) {
01419         SET_AXIS_ERROR_FLAG(axis, 1);
01420         break;
01421       }
01422 
01423       if (emcmotCommand->vel > 0.0) {
01424         if (GET_AXIS_HOMED_FLAG(axis)) {
01425           emcmotDebug->freePose.tran.x = emcmotConfig->maxLimit[axis];
01426         }
01427         else {
01428           emcmotDebug->freePose.tran.x = AXRANGE(axis);
01429         }
01430       }
01431       else {
01432         if (GET_AXIS_HOMED_FLAG(axis)) {
01433           emcmotDebug->freePose.tran.x = emcmotConfig->minLimit[axis];
01434         }
01435         else {
01436           emcmotDebug->freePose.tran.x = - AXRANGE(axis);
01437         }
01438       }
01439 
01440       tpSetVmax(&emcmotDebug->freeAxis[axis], fabs(emcmotCommand->vel));
01441       tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
01442       SET_AXIS_ERROR_FLAG(axis, 0);
01443       /* clear axis homed flag(s) if we don't have forward kins.
01444          Otherwise, a transition into coordinated mode will incorrectly
01445          assume the homed position. Do all if they've all been moved
01446          since homing, otherwise just do this one */
01447       clearHomes(axis);
01448       break;
01449 
01450     case EMCMOT_JOG_INCR:
01451       /* do an incremental jog */
01452 
01453       /* check axis range */
01454       axis = emcmotCommand->axis;
01455       if (axis < 0 ||
01456           axis >= EMCMOT_MAX_AXIS) {
01457         break;
01458       }
01459 
01460       /* requires no motion, in free mode, enable on */
01461       if (GET_MOTION_COORD_FLAG() ||
01462           ! GET_MOTION_INPOS_FLAG() ||
01463           ! GET_MOTION_ENABLE_FLAG()) {
01464         SET_AXIS_ERROR_FLAG(axis, 1);
01465         break;
01466       }
01467 
01468       /* don't jog further onto limits */
01469       if (! checkJog(axis, emcmotCommand->vel)) {
01470         SET_AXIS_ERROR_FLAG(axis, 1);
01471         break;
01472       }
01473 
01474       if (emcmotCommand->vel > 0.0) {
01475         emcmotDebug->freePose.tran.x = emcmotDebug->jointPos[axis] + emcmotCommand->offset; /* FIXME-- use 'goal' instead */
01476         if (GET_AXIS_HOMED_FLAG(axis)) {
01477           if (emcmotDebug->freePose.tran.x > emcmotConfig->maxLimit[axis]) {
01478             emcmotDebug->freePose.tran.x = emcmotConfig->maxLimit[axis];
01479           }
01480         }
01481       }
01482       else {
01483         emcmotDebug->freePose.tran.x = emcmotDebug->jointPos[axis] - emcmotCommand->offset; /* FIXME-- use 'goal' instead */
01484         if (GET_AXIS_HOMED_FLAG(axis)) {
01485           if (emcmotDebug->freePose.tran.x < emcmotConfig->minLimit[axis]) {
01486             emcmotDebug->freePose.tran.x = emcmotConfig->minLimit[axis];
01487           }
01488         }
01489       }
01490 
01491       tpSetVmax(&emcmotDebug->freeAxis[axis], fabs(emcmotCommand->vel));
01492       tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
01493       SET_AXIS_ERROR_FLAG(axis, 0);
01494       /* clear axis homed flag(s) if we don't have forward kins.
01495          Otherwise, a transition into coordinated mode will incorrectly
01496          assume the homed position. Do all if they've all been moved
01497          since homing, otherwise just do this one */
01498       clearHomes(axis);
01499 
01500       break;
01501 
01502     case EMCMOT_JOG_ABS:
01503       /* do an absolute jog */
01504 
01505       /* check axis range */
01506       axis = emcmotCommand->axis;
01507       if (axis < 0 ||
01508           axis >= EMCMOT_MAX_AXIS) {
01509         break;
01510       }
01511 
01512       /* requires no motion, in free mode, enable on */
01513       if (GET_MOTION_COORD_FLAG() ||
01514           ! GET_MOTION_INPOS_FLAG() ||
01515           ! GET_MOTION_ENABLE_FLAG()) {
01516         SET_AXIS_ERROR_FLAG(axis, 1);
01517         break;
01518       }
01519 
01520       /* don't jog further onto limits */
01521       if (! checkJog(axis, emcmotCommand->vel)) {
01522         SET_AXIS_ERROR_FLAG(axis, 1);
01523         break;
01524       }
01525 
01526       emcmotDebug->freePose.tran.x = emcmotCommand->offset; /* FIXME-- use 'goal' instead */
01527       if (GET_AXIS_HOMED_FLAG(axis)) {
01528         if (emcmotDebug->freePose.tran.x > emcmotConfig->maxLimit[axis]) {
01529           emcmotDebug->freePose.tran.x = emcmotConfig->maxLimit[axis];
01530         }
01531         else if (emcmotDebug->freePose.tran.x < emcmotConfig->minLimit[axis]) {
01532           emcmotDebug->freePose.tran.x = emcmotConfig->minLimit[axis];
01533         }
01534       }
01535 
01536       tpSetVmax(&emcmotDebug->freeAxis[axis], fabs(emcmotCommand->vel));
01537       tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
01538       SET_AXIS_ERROR_FLAG(axis, 0);
01539       /* clear axis homed flag(s) if we don't have forward kins.
01540          Otherwise, a transition into coordinated mode will incorrectly
01541          assume the homed position. Do all if they've all been moved
01542          since homing, otherwise just do this one */
01543       clearHomes(axis);
01544 
01545       break;
01546 
01547     case EMCMOT_SET_TERM_COND:
01548       /* sets termination condition for motion emcmotDebug->queue */
01549       tpSetTermCond(&emcmotDebug->queue, emcmotCommand->termCond);
01550       break;
01551 
01552     case EMCMOT_SET_LINE:
01553       /* emcmotDebug->queue up a linear move */
01554       /* requires coordinated mode, enable off, not on limits */
01555       if (! GET_MOTION_COORD_FLAG() ||
01556           ! GET_MOTION_ENABLE_FLAG()) {
01557         reportError("need to be enabled, in coord mode for linear move");
01558         emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
01559         SET_MOTION_ERROR_FLAG(1);
01560         break;
01561       }
01562       else if (! inRange(emcmotCommand->pos)) {
01563         reportError("linear move %d out of range",
01564                     emcmotCommand->id);
01565         emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01566         tpAbort(&emcmotDebug->queue);
01567         SET_MOTION_ERROR_FLAG(1);
01568         break;
01569       }
01570       else if (! checkLimits()) {
01571         reportError("can't do linear move with limits exceeded");
01572         emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01573         tpAbort(&emcmotDebug->queue);
01574         SET_MOTION_ERROR_FLAG(1);
01575         break;
01576       }
01577 
01578       /* append it to the emcmotDebug->queue */
01579       tpSetId(&emcmotDebug->queue, emcmotCommand->id);
01580       if (-1 == tpAddLine(&emcmotDebug->queue, emcmotCommand->pos)) {
01581         reportError("can't add linear move");
01582         emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
01583         tpAbort(&emcmotDebug->queue);
01584         SET_MOTION_ERROR_FLAG(1);
01585         break;
01586       }
01587       else {
01588         SET_MOTION_ERROR_FLAG(0);
01589         /* set flag that indicates all axes need rehoming, if any axis
01590            is moved in joint mode, for machines with no forward kins */
01591         rehomeAll = 1;
01592       }
01593       break;
01594 
01595     case EMCMOT_SET_CIRCLE:
01596       /* emcmotDebug->queue up a circular move */
01597       /* requires coordinated mode, enable on, not on limits */
01598       if (! GET_MOTION_COORD_FLAG() ||
01599           ! GET_MOTION_ENABLE_FLAG()) {
01600         reportError("need to be enabled, in coord mode for circular move");
01601         emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
01602         SET_MOTION_ERROR_FLAG(1);
01603         break;
01604       }
01605       else if (! inRange(emcmotCommand->pos)) {
01606         reportError("circular move %d out of range",
01607                     emcmotCommand->id);
01608         emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01609         tpAbort(&emcmotDebug->queue);
01610         SET_MOTION_ERROR_FLAG(1);
01611         break;
01612       }
01613       else if (! checkLimits()) {
01614         reportError("can't do circular move with limits exceeded");
01615         emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
01616         tpAbort(&emcmotDebug->queue);
01617         SET_MOTION_ERROR_FLAG(1);
01618         break;
01619       }
01620 
01621       /* append it to the emcmotDebug->queue */
01622       tpSetId(&emcmotDebug->queue, emcmotCommand->id);
01623       if (-1 ==
01624           tpAddCircle(&emcmotDebug->queue, emcmotCommand->pos,
01625                       emcmotCommand->center, emcmotCommand->normal,
01626                       emcmotCommand->turn)) {
01627         reportError("can't add circular move");
01628         emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
01629         tpAbort(&emcmotDebug->queue);
01630         SET_MOTION_ERROR_FLAG(1);
01631         break;
01632       }
01633       else {
01634         SET_MOTION_ERROR_FLAG(0);
01635         /* set flag that indicates all axes need rehoming, if any axis
01636            is moved in joint mode, for machines with no forward kins */
01637         rehomeAll = 1;
01638       }
01639       break;
01640 
01641     case EMCMOT_SET_VEL:
01642       /* set the velocity for subsequent moves */
01643       /* can do it at any time */
01644       emcmotStatus->vel = emcmotCommand->vel;
01645       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01646         tpSetVmax(&emcmotDebug->freeAxis[axis], emcmotStatus->vel);
01647       }
01648       tpSetVmax(&emcmotDebug->queue, emcmotStatus->vel);
01649       break;
01650 
01651     case EMCMOT_SET_VEL_LIMIT:
01652       MARK_EMCMOT_CONFIG_CHANGE();
01653       /* set the absolute max velocity for all subsequent moves */
01654       /* can do it at any time */
01655       emcmotConfig->limitVel = emcmotCommand->vel;
01656       tpSetVlimit(&emcmotDebug->queue, emcmotConfig->limitVel);
01657       break;
01658 
01659     case EMCMOT_SET_AXIS_VEL_LIMIT:
01660       MARK_EMCMOT_CONFIG_CHANGE();
01661       /* check axis range */
01662       axis = emcmotCommand->axis;
01663       if (axis < 0 ||
01664           axis >= EMCMOT_MAX_AXIS) {
01665         break;
01666       }
01667       tpSetVlimit(&emcmotDebug->freeAxis[axis], emcmotCommand->vel);
01668       emcmotConfig->axisLimitVel[axis] = emcmotCommand->vel;
01669       emcmotDebug->bigVel[axis] = 10 * emcmotCommand->vel;
01670       break;
01671 
01672     case EMCMOT_SET_HOMING_VEL:
01673       MARK_EMCMOT_CONFIG_CHANGE();
01674       /* set the homing velocity */
01675       /* can do it at any time */
01676       /* sign of vel should set polarity, and mag-sign are recorded */
01677 
01678       /* check axis range */
01679       axis = emcmotCommand->axis;
01680       if (axis < 0 ||
01681           axis >= EMCMOT_MAX_AXIS) {
01682         break;
01683       }
01684 
01685       if (emcmotCommand->vel < 0.0) {
01686         emcmotConfig->homingVel[axis] = - emcmotCommand->vel;
01687         SET_AXIS_HOMING_POLARITY(axis, 0);
01688       }
01689       else {
01690         emcmotConfig->homingVel[axis] = emcmotCommand->vel;
01691         SET_AXIS_HOMING_POLARITY(axis, 1);
01692       }
01693       break;
01694 
01695     case EMCMOT_SET_ACC:
01696       /* set the max acceleration */
01697       /* can do it at any time */
01698       emcmotStatus->acc = emcmotCommand->acc;
01699       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01700         tpSetAmax(&emcmotDebug->freeAxis[axis], emcmotStatus->acc);
01701       }
01702       tpSetAmax(&emcmotDebug->queue, emcmotStatus->acc);
01703       break;
01704 
01705     case EMCMOT_PAUSE:
01706       /* pause the motion */
01707       /* can happen at any time */
01708       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01709         tpPause(&emcmotDebug->freeAxis[axis]);
01710       }
01711       tpPause(&emcmotDebug->queue);
01712       emcmotStatus->paused = 1;
01713       break;
01714 
01715     case EMCMOT_RESUME:
01716       /* resume paused motion */
01717       /* can happen at any time */
01718       emcmotDebug->stepping = 0;
01719       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01720         tpResume(&emcmotDebug->freeAxis[axis]);
01721       }
01722       tpResume(&emcmotDebug->queue);
01723       emcmotStatus->paused = 0;
01724       break;
01725 
01726     case EMCMOT_STEP:
01727       /* resume paused motion until id changes */
01728       /* can happen at any time */
01729       emcmotDebug->idForStep = emcmotStatus->id;
01730       emcmotDebug->stepping = 1;
01731       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01732         tpResume(&emcmotDebug->freeAxis[axis]);
01733       }
01734       tpResume(&emcmotDebug->queue);
01735       emcmotStatus->paused = 0;
01736       break;
01737 
01738     case EMCMOT_SCALE:
01739       /* override speed */
01740       /* can happen at any time */
01741       if (emcmotCommand->scale < 0.0) {
01742         emcmotCommand->scale = 0.0; /* clamp it */
01743       }
01744       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
01745         tpSetVscale(&emcmotDebug->freeAxis[axis], emcmotCommand->scale);
01746         emcmotStatus->axVscale[axis] = emcmotCommand->scale;
01747       }
01748       tpSetVscale(&emcmotDebug->queue, emcmotCommand->scale);
01749       emcmotStatus->qVscale = emcmotCommand->scale;
01750       break;
01751 
01752     case EMCMOT_ABORT:
01753       /* abort motion */
01754       /* can happen at any time */
01755       /* check for coord or free space motion active */
01756       if (GET_MOTION_TELEOP_FLAG()) {
01757         emcmotDebug->teleop_data.desiredVel.tran.x = 0.0;
01758         emcmotDebug->teleop_data.desiredVel.tran.y = 0.0;
01759         emcmotDebug->teleop_data.desiredVel.tran.z = 0.0;
01760         emcmotDebug->teleop_data.desiredVel.a = 0.0;
01761         emcmotDebug->teleop_data.desiredVel.b = 0.0;
01762         emcmotDebug->teleop_data.desiredVel.c = 0.0;
01763       }
01764       else if (GET_MOTION_COORD_FLAG()) {
01765         tpAbort(&emcmotDebug->queue);
01766         SET_MOTION_ERROR_FLAG(0);
01767       }
01768       else {
01769         /* check axis range */
01770         axis = emcmotCommand->axis;
01771         if (axis < 0 ||
01772             axis >= EMCMOT_MAX_AXIS) {
01773           break;
01774         }
01775         tpAbort(&emcmotDebug->freeAxis[axis]);
01776         SET_AXIS_HOMING_FLAG(axis, 0);
01777         SET_AXIS_ERROR_FLAG(axis, 0);
01778       }
01779       /* FIXME-- testing dout */
01780       extMotDout(MOT_DOUT_ABORT_VAL);
01781       break;
01782 
01783     case EMCMOT_DISABLE:
01784       /* go into disable */
01785       /* can happen at any time */
01786       /* reset the emcmotDebug->enabling flag to defer disable until
01787          controller cycle (it *will* be honored) */
01788       emcmotDebug->enabling = 0;
01789       if (kinType == KINEMATICS_INVERSE_ONLY) {
01790         emcmotDebug->teleoperating = 0;
01791         emcmotDebug->coordinating = 0;
01792       }
01793       break;
01794 
01795     case EMCMOT_ENABLE:
01796       /* come out of disable */
01797       /* can happen at any time */
01798       /* set the emcmotDebug->enabling flag to defer enable
01799          until controller cycle */
01800       emcmotDebug->enabling = 1;
01801       if (kinType == KINEMATICS_INVERSE_ONLY) {
01802         emcmotDebug->teleoperating = 0;
01803         emcmotDebug->coordinating = 0;
01804       }
01805       break;
01806 
01807     case EMCMOT_SET_PID:
01808       MARK_EMCMOT_CONFIG_CHANGE();
01809       /* configure the PID gains */
01810       axis = emcmotCommand->axis;
01811       if (axis < 0 ||
01812           axis >= EMCMOT_MAX_AXIS) {
01813         break;
01814       }
01815       pidSetGains(&emcmotConfig->pid[axis],
01816                   emcmotCommand->pid);
01817       break;
01818 
01819     case EMCMOT_ACTIVATE_AXIS:
01820       /* make axis active, so that amps will be enabled when
01821          system is enabled or disabled */
01822       /* can be done at any time */
01823       axis = emcmotCommand->axis;
01824       if (axis < 0 ||
01825           axis >= EMCMOT_MAX_AXIS) {
01826         break;
01827       }
01828       SET_AXIS_ACTIVE_FLAG(axis, 1);
01829       break;
01830 
01831     case EMCMOT_DEACTIVATE_AXIS:
01832       /* make axis inactive, so that amps won't be affected when
01833          system is enabled or disabled */
01834       /* can be done at any time */
01835       axis = emcmotCommand->axis;
01836       if (axis < 0 ||
01837           axis >= EMCMOT_MAX_AXIS) {
01838         break;
01839       }
01840       SET_AXIS_ACTIVE_FLAG(axis, 0);
01841       break;
01842 
01843     case EMCMOT_ENABLE_AMPLIFIER:
01844       /* enable the amplifier directly, but don't enable calculations */
01845       /* can be done at any time */
01846       axis = emcmotCommand->axis;
01847       if (axis < 0 ||
01848           axis >= EMCMOT_MAX_AXIS) {
01849         break;
01850       }
01851       extAmpEnable(axis, GET_AXIS_ENABLE_POLARITY(axis));
01852       break;
01853 
01854     case EMCMOT_DISABLE_AMPLIFIER:
01855       /* disable the axis calculations and amplifier, but don't
01856          disable calculations */
01857       /* can be done at any time */
01858       axis = emcmotCommand->axis;
01859       if (axis < 0 ||
01860           axis >= EMCMOT_MAX_AXIS) {
01861         break;
01862       }
01863       extAmpEnable(axis, ! GET_AXIS_ENABLE_POLARITY(axis));
01864       break;
01865 
01866     case EMCMOT_OPEN_LOG:
01867       /* open a data log */
01868       axis = emcmotCommand->axis;
01869       valid = 0;
01870       if (emcmotCommand->logSize > 0 &&
01871           emcmotCommand->logSize <= EMCMOT_LOG_MAX) {
01872         /* handle log-specific data */
01873         switch (emcmotCommand->logType) {
01874         case EMCMOT_LOG_TYPE_AXIS_POS:
01875         case EMCMOT_LOG_TYPE_AXIS_VEL:
01876         case EMCMOT_LOG_TYPE_POS_VOLTAGE:
01877           if (axis >= 0 &&
01878               axis < EMCMOT_MAX_AXIS) {
01879             valid = 1;
01880           }
01881           break;
01882 
01883         default:
01884           valid = 1;
01885           break;
01886         }
01887       }
01888 
01889       if (valid) {
01890         /* success */
01891         loggingAxis = axis;
01892         emcmotLogInit(emcmotLog,
01893                       emcmotCommand->logType,
01894                       emcmotCommand->logSize);
01895         emcmotStatus->logOpen = 1;
01896         emcmotStatus->logStarted = 0;
01897         emcmotStatus->logSize = emcmotCommand->logSize;
01898         emcmotStatus->logSkip = emcmotCommand->logSkip;
01899         emcmotStatus->logType = emcmotCommand->logType;
01900         emcmotStatus->logTriggerType = emcmotCommand->logTriggerType;
01901         emcmotStatus->logTriggerVariable = emcmotCommand->logTriggerVariable;
01902         emcmotStatus->logTriggerThreshold = emcmotCommand->logTriggerThreshold;
01903         if (axis >= 0 &&
01904             axis < EMCMOT_MAX_AXIS &&
01905             emcmotStatus->logTriggerType == EMCLOG_DELTA_TRIGGER) 
01906           {
01907             switch(emcmotStatus->logTriggerVariable)
01908               {
01909               case EMCLOG_TRIGGER_ON_FERROR:
01910                 emcmotStatus->logStartVal = emcmotDebug->ferrorCurrent[loggingAxis];
01911                 break;
01912                 
01913               case EMCLOG_TRIGGER_ON_VOLT:
01914                 emcmotStatus->logStartVal = emcmotDebug->rawOutput[loggingAxis];
01915                 break;
01916               case EMCLOG_TRIGGER_ON_POS:
01917                 emcmotStatus->logStartVal = emcmotDebug->jointPos[loggingAxis];
01918                 break;
01919               case EMCLOG_TRIGGER_ON_VEL:
01920                 emcmotStatus->logStartVal = emcmotDebug->jointPos[loggingAxis] - emcmotDebug->oldJointPos[loggingAxis];
01921                 break;
01922                 
01923               default:
01924                 break;
01925               } 
01926           }
01927       }
01928       break;
01929 
01930     case EMCMOT_START_LOG:
01931       /* start logging */
01932       /* first ignore triggered log types */
01933       if (emcmotStatus->logType == EMCMOT_LOG_TYPE_POS_VOLTAGE) {
01934         break;
01935       }
01936       /* set the global baseTime, to be subtracted off log times, otherwise
01937          time values are too large for the small increments to appear */
01938       if (emcmotStatus->logOpen &&
01939           emcmotStatus->logTriggerType == EMCLOG_MANUAL_TRIGGER) {
01940         logStartTime = etime();
01941         emcmotStatus->logStarted = 1;
01942         logSkip = 0;
01943       }
01944       break;
01945 
01946     case EMCMOT_STOP_LOG:
01947       /* stop logging */
01948       emcmotStatus->logStarted = 0;
01949       break;
01950 
01951     case EMCMOT_CLOSE_LOG:
01952       emcmotStatus->logOpen = 0;
01953       emcmotStatus->logStarted = 0;
01954       emcmotStatus->logSize = 0;
01955       emcmotStatus->logSkip = 0;
01956       emcmotStatus->logType = 0;
01957       break;
01958 
01959     case EMCMOT_DAC_OUT:
01960       /* write output to dacs directly */
01961       /* will only persist if amplifiers are disabled */
01962       axis = emcmotCommand->axis;
01963       if (axis < 0 ||
01964           axis >= EMCMOT_MAX_AXIS) {
01965         break;
01966       }
01967       /* trigger log, if active */
01968       if (emcmotStatus->logType == EMCMOT_LOG_TYPE_POS_VOLTAGE &&
01969           loggingAxis == axis &&
01970           emcmotStatus->logOpen != 0) {
01971         emcmotStatus->logStarted = 1;
01972         logSkip = 0;
01973       }
01974       emcmotStatus->output[axis] = emcmotCommand->dacOut;
01975       break;
01976 
01977     case EMCMOT_HOME:
01978       /* home the specified axis */
01979       /* need to be in free mode, enable on */
01980       /* homing is basically a slow incremental jog to full range */
01981       axis = emcmotCommand->axis;
01982       if (axis < 0 ||
01983           axis >= EMCMOT_MAX_AXIS) {
01984         break;
01985       }
01986       if (GET_MOTION_COORD_FLAG() ||
01987           ! GET_MOTION_ENABLE_FLAG()) {
01988         break;
01989       }
01990 
01991       if (GET_AXIS_HOMING_POLARITY(axis)) {
01992         emcmotDebug->freePose.tran.x = + 2.0 * AXRANGE(axis);
01993       }
01994       else {
01995         emcmotDebug->freePose.tran.x = - 2.0 * AXRANGE(axis);
01996       }
01997 
01998       tpSetVmax(&emcmotDebug->freeAxis[axis], emcmotConfig->homingVel[axis]);
01999       tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
02000       emcmotDebug->homingPhase[axis]=1;
02001       SET_AXIS_HOMING_FLAG(axis, 1);
02002       SET_AXIS_HOMED_FLAG(axis, 0);
02003       break;
02004 
02005     case EMCMOT_ENABLE_WATCHDOG:
02006       emcmotDebug->wdEnabling = 1;
02007       emcmotDebug->wdWait = emcmotCommand->wdWait;
02008       if (emcmotDebug->wdWait < 0) {
02009         emcmotDebug->wdWait = 0;
02010       }
02011       break;
02012 
02013     case EMCMOT_DISABLE_WATCHDOG:
02014       emcmotDebug->wdEnabling = 0;
02015       break;
02016 
02017     case EMCMOT_SET_POLARITY:
02018       MARK_EMCMOT_CONFIG_CHANGE();
02019       axis = emcmotCommand->axis;
02020       if (axis < 0 ||
02021           axis >= EMCMOT_MAX_AXIS) {
02022         break;
02023       }
02024       if (emcmotCommand->level) {
02025         /* normal */
02026         emcmotConfig->axisPolarity[axis] |= emcmotCommand->axisFlag;
02027       }
02028       else {
02029         /* inverted */
02030         emcmotConfig->axisPolarity[axis] &= ~emcmotCommand->axisFlag;
02031       }
02032       break;
02033 
02034 #ifdef ENABLE_PROBING
02035     case EMCMOT_SET_PROBE_INDEX:
02036       MARK_EMCMOT_CONFIG_CHANGE();
02037       emcmotConfig->probeIndex = emcmotCommand->probeIndex;
02038       break;
02039 
02040     case EMCMOT_SET_PROBE_POLARITY:
02041       MARK_EMCMOT_CONFIG_CHANGE();
02042       emcmotConfig->probePolarity = emcmotCommand->level;
02043       break;
02044 
02045     case EMCMOT_CLEAR_PROBE_FLAGS:
02046       emcmotStatus->probeTripped = 0;
02047       emcmotStatus->probing = 1;
02048       break;
02049 
02050     case EMCMOT_PROBE:
02051       /* most of this is taken from EMCMOT_SET_LINE */
02052       /* emcmotDebug->queue up a linear move */
02053       /* requires coordinated mode, enable off, not on limits */
02054       if (! GET_MOTION_COORD_FLAG() ||
02055           ! GET_MOTION_ENABLE_FLAG()) {
02056         reportError("need to be enabled, in coord mode for probe move");
02057         emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
02058         SET_MOTION_ERROR_FLAG(1);
02059         break;
02060       }
02061       else if (! inRange(emcmotCommand->pos)) {
02062         reportError("probe move %d out of range",
02063                     emcmotCommand->id);
02064         emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
02065         tpAbort(&emcmotDebug->queue);
02066         SET_MOTION_ERROR_FLAG(1);
02067         break;
02068       }
02069       else if (! checkLimits()) {
02070         reportError("can't do probe move with limits exceeded");
02071         emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
02072         tpAbort(&emcmotDebug->queue);
02073         SET_MOTION_ERROR_FLAG(1);
02074         break;
02075       }
02076 
02077       /* append it to the emcmotDebug->queue */
02078       tpSetId(&emcmotDebug->queue, emcmotCommand->id);
02079       if (-1 == tpAddLine(&emcmotDebug->queue, emcmotCommand->pos)) {
02080         reportError("can't add probe move");
02081         emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
02082         tpAbort(&emcmotDebug->queue);
02083         SET_MOTION_ERROR_FLAG(1);
02084         break;
02085       }
02086       else {
02087         emcmotStatus->probeTripped = 0;
02088         emcmotStatus->probing = 1;
02089         SET_MOTION_ERROR_FLAG(0);
02090         /* set flag that indicates all axes need rehoming, if any axis
02091            is moved in joint mode, for machines with no forward kins */
02092         rehomeAll = 1;
02093       }
02094       break;
02095 #endif /* ENABLE_PROBING */
02096 
02097     case EMCMOT_SET_TELEOP_VECTOR:
02098       if (! GET_MOTION_TELEOP_FLAG() ||
02099           ! GET_MOTION_ENABLE_FLAG()) {
02100         reportError("need to be enabled, in teleop mode for teleop move");
02101       }
02102       else {
02103         double velmag;
02104         emcmotDebug->teleop_data.desiredVel = emcmotCommand->pos;
02105         pmCartMag(emcmotDebug->teleop_data.desiredVel.tran, &velmag);
02106         if (emcmotDebug->teleop_data.desiredVel.a > velmag) {
02107           velmag = emcmotDebug->teleop_data.desiredVel.a;
02108         }
02109         if (emcmotDebug->teleop_data.desiredVel.b > velmag) {
02110           velmag = emcmotDebug->teleop_data.desiredVel.b;
02111         }
02112         if (emcmotDebug->teleop_data.desiredVel.c > velmag) {
02113           velmag = emcmotDebug->teleop_data.desiredVel.c;
02114         }
02115         if (velmag > emcmotConfig->limitVel) {
02116           pmCartScalMult(emcmotDebug->teleop_data.desiredVel.tran, 
02117                          emcmotConfig->limitVel/velmag,
02118                          &emcmotDebug->teleop_data.desiredVel.tran);
02119           emcmotDebug->teleop_data.desiredVel.a *= 
02120             emcmotConfig->limitVel/velmag;
02121           emcmotDebug->teleop_data.desiredVel.b *= 
02122             emcmotConfig->limitVel/velmag;
02123           emcmotDebug->teleop_data.desiredVel.c *= 
02124             emcmotConfig->limitVel/velmag;
02125         }
02126         /* flag that all joints need to be homed, if any joint
02127            is jogged individually later */
02128         rehomeAll = 1;
02129       }
02130       break;
02131       
02132     case EMCMOT_SET_DEBUG:
02133       emcmotConfig->debug = emcmotCommand->debug;
02134       MARK_EMCMOT_CONFIG_CHANGE();
02135       break;
02136 
02137     case EMCMOT_SET_AOUT:
02138       tpSetAout(&emcmotDebug->queue, emcmotCommand->out, emcmotCommand->minLimit, emcmotCommand->maxLimit);
02139       break;
02140 
02141     case EMCMOT_SET_DOUT:
02142       tpSetDout(&emcmotDebug->queue, emcmotCommand->out, emcmotCommand->start, emcmotCommand->end);
02143       break;
02144 
02145     default:
02146       reportError("unrecognized command %d", emcmotCommand->command);
02147       emcmotStatus->commandStatus = EMCMOT_COMMAND_UNKNOWN_COMMAND;
02148       break;
02149     } /* end of: command switch */
02150 
02151     /* synch tail count */
02152     emcmotStatus->tail = emcmotStatus->head;
02153     emcmotConfig->tail = emcmotConfig->head;
02154     emcmotDebug->tail = emcmotDebug->head;
02155 
02156   } /* end of: if-new-command */
02157 
02158   return 0;
02159 }

02279 {
02280   int first = 1;                /* true the first time thru, for initing */
02281   double start, end, delta;     /* time stamping */
02282   int homeFlag;                 /* result of ext read to home switch */
02283   int axis;                     /* axis loop counter */
02284   int t;                        /* loop counter if we're in axis loop */
02285   int isLimit;                  /* result of ext read to limit sw */
02286   int whichCycle;               /* 0=none, 1=servo, 2=traj */
02287   int fault;
02288 #ifndef NO_ROUNDING
02289   double numres;
02290 #endif
02291   double thisFerror[EMCMOT_MAX_AXIS] = {0.0};
02292   double limitFerror;           /* current scaled ferror */
02293   double magFerror;
02294   double oldbcomp;
02295   int retval;
02296   /* end of backlash stuff */
02297 #ifdef STEPPER_MOTORS
02298   double pdmultfloat;
02299 #endif
02300 #ifdef COMPING
02301   int dir[EMCMOT_MAX_AXIS] = {1}; /* flag for direction, used for axis comp */
02302 #endif /* COMPING */
02303 
02304 #if defined(rtai) || defined(rtlinux)
02305   for (;;) {
02306 #endif /* rtai || rtlinux */
02307 
02308 #ifdef SIMULATED_MOTORS
02309 #if defined(rtlinux) || defined(rtai)
02310     simMotInit(NULL);
02311 #endif
02312 #endif
02313 
02314 #if defined(rtlinux) || defined(rtai)
02315     /* toggle sound port watchdog */
02316 
02317     if (emcmotDebug->wdEnabling && ! emcmotDebug->wdEnabled) {
02318       /* just turned WD on */
02319       emcmotDebug->wdCount = emcmotDebug->wdWait;
02320       emcmotDebug->wdToggle = 0;
02321       emcmotDebug->wdEnabled = 1;
02322     }
02323     if (! emcmotDebug->wdEnabling && emcmotDebug->wdEnabled) {
02324       /* just turned WD off */
02325       /* clear the bit */
02326       soundByte = inb(SOUND_PORT);
02327       soundByte &= ~SOUND_MASK;
02328       outb(soundByte, SOUND_PORT);
02329       emcmotDebug->wdEnabled = 0;
02330     }
02331 
02332     if (emcmotDebug->wdEnabled && emcmotDebug->wdCount-- <= 0) {
02333       soundByte = inb(SOUND_PORT);
02334       emcmotDebug->wdToggle = ! emcmotDebug->wdToggle;
02335       if (emcmotDebug->wdToggle) {
02336         soundByte |= SOUND_MASK;
02337       }
02338       else {
02339         soundByte &= ~SOUND_MASK;
02340       }
02341       outb(soundByte, SOUND_PORT);
02342       emcmotDebug->wdCount = emcmotDebug->wdWait;
02343     }
02344 #endif /* rtlinux || rtai */
02345  
02346     /* record start time */
02347     start = etime();
02348 
02349     /* READ COMMAND: */
02350     emcmotCommandHandler();
02351 
02352     /* increment head count */
02353     emcmotStatus->head++;
02354 
02355     /* READ INPUTS: */
02356 
02357     /* latch all encoder feedback into raw input array, done outside
02358        of for-loop on joints below, since it's a single call for all
02359        joints */
02360     extEncoderReadAll(EMCMOT_MAX_AXIS, emcmotDebug->rawInput);
02361 
02362     /* process input and read limit switches */
02363     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02364       /* save old cycle's values */
02365       if (! first && emcmotDebug->oldInputValid[axis] ) {
02366         emcmotDebug->oldInput[axis] = emcmotStatus->input[axis];
02367       }
02368       /* set input, scaled according to input = (raw - offset) / scale */
02369       emcmotStatus->input[axis] =
02370         (emcmotDebug->rawInput[axis] - emcmotStatus->inputOffset[axis]) *
02371         emcmotDebug->inverseInputScale[axis] - emcmotDebug->bcompincr[axis];
02372 
02373 #ifdef COMPING
02374       /* adjust feedback using compensation tables */
02375       if (GET_AXIS_HOMED_FLAG(axis)) {
02376         emcmotStatus->input[axis] = axisComp(axis, dir[axis], emcmotStatus->input[axis]);
02377       }
02378 #endif
02379       if (first || !emcmotDebug->oldInputValid[axis] ) {
02380         emcmotDebug->oldInput[axis] = emcmotStatus->input[axis];
02381         emcmotDebug->oldInputValid[axis] = 1;
02382         first = 0;
02383       }
02384 
02385       /* debounce bad feedback */
02386 #ifndef SIMULATED_MOTORS
02387       if (fabs(emcmotStatus->input[axis] - emcmotDebug->oldInput[axis]) /
02388           emcmotConfig->servoCycleTime > emcmotDebug->bigVel[axis]) {
02389         /* bad input value-- interpolate last value, up to max debounces,
02390            then hold it */
02391         if (++positionInputDebounce[axis] > POSITION_INPUT_DEBOUNCE) {
02392           /* we haven't exceeded max number of debounces allowed, so
02393              interpolate off the velocity estimate */
02394           emcmotStatus->input[axis] = emcmotDebug->oldInput[axis] +
02395             emcmotDebug->jointVel[axis] * emcmotConfig->servoCycleTime;
02396         }
02397         else {
02398           /* we've exceeded the max number of debounces allowed, so
02399              hold position. We should flag an error here, abort the move,
02400              disable motion, etc. but for now we'll rely on following error
02401              to do this */
02402           emcmotStatus->input[axis] = emcmotDebug->oldInput[axis];
02403         }         
02404         /* FIXME-- testing */
02405         debouncecount[axis]++;
02406       }
02407       else {
02408         /* no debounce needed, so reset debounce counter */
02409         positionInputDebounce[axis] = 0;
02410       }
02411 #endif
02412 
02413       /* read limit switches and amp fault from external
02414          interface, and set 'emcmotDebug->enabling' to zero if tripped
02415          to cause immediate stop */
02416 
02417       /* handle limits */
02418       if (GET_AXIS_ACTIVE_FLAG(axis)) {
02419         extMaxLimitSwitchRead(axis, &isLimit);
02420         if (isLimit == GET_AXIS_PHL_POLARITY(axis)) {
02421           if (++emcmotDebug->maxLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE) {
02422             SET_AXIS_PHL_FLAG(axis, 1);
02423             emcmotDebug->maxLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE;
02424             if (emcmotStatus->overrideLimits ||
02425                 isHoming()) {
02426             }
02427             else {
02428               SET_AXIS_ERROR_FLAG(axis, 1);
02429               emcmotDebug->enabling = 0;
02430             }
02431           }
02432         }
02433         else {
02434           SET_AXIS_PHL_FLAG(axis, 0);
02435           emcmotDebug->maxLimitSwitchCount[axis] = 0;
02436         }
02437       }
02438       if (GET_AXIS_ACTIVE_FLAG(axis)) {
02439         extMinLimitSwitchRead(axis, &isLimit);
02440         if (isLimit == GET_AXIS_NHL_POLARITY(axis)) {
02441           if (++emcmotDebug->minLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE) {
02442             SET_AXIS_NHL_FLAG(axis, 1);
02443             emcmotDebug->minLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE;
02444             if (emcmotStatus->overrideLimits ||
02445                 isHoming()) {
02446             }
02447             else {
02448               SET_AXIS_ERROR_FLAG(axis, 1);
02449               emcmotDebug->enabling = 0;
02450             }
02451           }
02452         }
02453         else {
02454           SET_AXIS_NHL_FLAG(axis, 0);
02455           emcmotDebug->minLimitSwitchCount[axis] = 0;
02456         }
02457       }
02458 
02459       if (GET_AXIS_ACTIVE_FLAG(axis) &&
02460           GET_AXIS_ENABLE_FLAG(axis)) {
02461         extAmpFault(axis, &fault);
02462         if (fault == GET_AXIS_FAULT_POLARITY(axis)) {
02463           if (++emcmotDebug->ampFaultCount[axis] > AMP_FAULT_DEBOUNCE) {
02464             SET_AXIS_ERROR_FLAG(axis, 1);
02465             SET_AXIS_FAULT_FLAG(axis, 1);
02466             emcmotDebug->ampFaultCount[axis] = AMP_FAULT_DEBOUNCE;
02467             emcmotDebug->enabling = 0;
02468           }
02469         }
02470         else {
02471           SET_AXIS_FAULT_FLAG(axis, 0);
02472           emcmotDebug->ampFaultCount[axis] = 0;
02473         }
02474       }
02475 
02476       /* read home switch and set flag if tripped. Handling of home
02477          sequence is done later. */
02478       if (GET_AXIS_ACTIVE_FLAG(axis)) {
02479         extHomeSwitchRead(axis, &homeFlag);
02480         if (homeFlag == GET_AXIS_HOME_SWITCH_POLARITY(axis)) {
02481           SET_AXIS_HOME_SWITCH_FLAG(axis, 1);
02482         }
02483         else {
02484           SET_AXIS_HOME_SWITCH_FLAG(axis, 0);
02485         }
02486       }
02487 
02488     } /* end of: loop on axes, for reading inputs, setting limit and
02489          home switch flags */
02490 
02491     /* check to see if logging should be triggered */
02492     if (emcmotStatus->logOpen &&
02493         ! emcmotStatus->logStarted &&
02494         loggingAxis >= 0 &&
02495         loggingAxis < EMCMOT_MAX_AXIS) {
02496       double val = 0.0;
02497 
02498       switch (emcmotStatus->logTriggerVariable) {
02499       case EMCLOG_TRIGGER_ON_FERROR:
02500         val = emcmotDebug->ferrorCurrent[loggingAxis];
02501         break;
02502             
02503       case EMCLOG_TRIGGER_ON_VOLT:
02504         val = emcmotDebug->rawOutput[loggingAxis];
02505         break;
02506 
02507       case EMCLOG_TRIGGER_ON_POS:
02508         val = emcmotDebug->jointPos[loggingAxis];
02509         break;
02510 
02511       case EMCLOG_TRIGGER_ON_VEL:
02512         val = emcmotDebug->jointPos[loggingAxis] - emcmotDebug->oldJointPos[loggingAxis];
02513         break;
02514             
02515       default:
02516         break;
02517       } 
02518 
02519       switch (emcmotStatus->logTriggerType) {
02520       case EMCLOG_MANUAL_TRIGGER:
02521         break;
02522               
02523       case EMCLOG_DELTA_TRIGGER:
02524         if (emcmotStatus->logStartVal - val < -emcmotStatus->logTriggerThreshold ||
02525             emcmotStatus->logStartVal - val > emcmotStatus->logTriggerThreshold) {
02526           emcmotStatus->logStarted = 1;
02527         }
02528         break;
02529 
02530       case EMCLOG_OVER_TRIGGER:
02531         if (val > emcmotStatus->logTriggerThreshold) {
02532           emcmotStatus->logStarted = 1;
02533         }
02534         break;
02535 
02536       case EMCLOG_UNDER_TRIGGER:
02537         if (val < emcmotStatus->logTriggerThreshold) {
02538           emcmotStatus->logStarted = 1;
02539         }
02540       }
02541     }
02542 
02543     /* now we're outside the axis loop, having just read input, scaled it,
02544        read the limit and home switches and amp faults. We need to abort
02545        all motion if we're on limits, handle homing sequence, and handle
02546        mode and state transitions. */
02547 
02548     /* RUN STATE LOGIC: */
02549 
02550     /* check for disabling */
02551     if (! emcmotDebug->enabling && GET_MOTION_ENABLE_FLAG()) {
02552       /* clear out the motion emcmotDebug->queue and interpolators */
02553       tpClear(&emcmotDebug->queue);
02554       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02555         tpClear(&emcmotDebug->freeAxis[axis]);
02556         cubicDrain(&emcmotDebug->cubic[axis]);
02557         if (GET_AXIS_ACTIVE_FLAG(axis)) {
02558           extAmpEnable(axis, ! GET_AXIS_ENABLE_POLARITY(axis));
02559           SET_AXIS_ENABLE_FLAG(axis, 0);
02560           SET_AXIS_HOMING_FLAG(axis, 0);
02561           emcmotStatus->output[axis] = 0.0;
02562         }
02563         /* don't clear the axis error flag, since that may signify why
02564            we just went into disabled state */
02565       }
02566       /* reset the trajectory interpolation counter, so that the kinematics
02567          functions called during the disabled state run at the nominal
02568          trajectory rate rather than the servo rate. It's loaded with
02569          emcmotConfig->interpolationRate when it goes to zero. */
02570       interpolationCounter = 0;
02571       SET_MOTION_ENABLE_FLAG(0);
02572       /* don't clear the motion error flag, since that may signify why
02573          we just went into disabled state */
02574     }
02575 
02576     /* check for emcmotDebug->enabling */
02577     if (emcmotDebug->enabling && ! GET_MOTION_ENABLE_FLAG()) {
02578       tpSetPos(&emcmotDebug->queue, emcmotStatus->pos);
02579       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02580         emcmotDebug->freePose.tran.x = emcmotDebug->jointPos[axis];
02581         tpSetPos(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
02582         pidReset(&emcmotConfig->pid[axis]);
02583         if (GET_AXIS_ACTIVE_FLAG(axis)) {
02584           extAmpEnable(axis, GET_AXIS_ENABLE_POLARITY(axis));
02585           SET_AXIS_ENABLE_FLAG(axis, 1);
02586           SET_AXIS_HOMING_FLAG(axis, 0);
02587         }
02588         /* clear any outstanding axis errors when going into enabled state */
02589         SET_AXIS_ERROR_FLAG(axis, 0);
02590       }
02591       SET_MOTION_ENABLE_FLAG(1);
02592       /* clear any outstanding motion errors when going into enabled state */
02593       SET_MOTION_ERROR_FLAG(0);
02594     }
02595 
02596     /* check for entering teleop mode */
02597     if (emcmotDebug->teleoperating && ! GET_MOTION_TELEOP_FLAG()) {
02598       if (GET_MOTION_INPOS_FLAG()) {
02599 
02600         /* update coordinated emcmotDebug->queue position */
02601         tpSetPos(&emcmotDebug->queue, emcmotStatus->pos);
02602         /* drain the cubics so they'll synch up */
02603         for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02604           cubicDrain(&emcmotDebug->cubic[axis]);
02605         }
02606         /* Initialize things to do when starting teleop mode.*/
02607         emcmotDebug->teleop_data.currentVel.tran.x = 0.0;
02608         emcmotDebug->teleop_data.currentVel.tran.y = 0.0;
02609         emcmotDebug->teleop_data.currentVel.tran.z = 0.0;
02610         emcmotDebug->teleop_data.currentVel.a = 0.0;
02611         emcmotDebug->teleop_data.currentVel.b = 0.0;
02612         emcmotDebug->teleop_data.currentVel.c = 0.0;
02613         emcmotDebug->teleop_data.desiredVel.tran.x = 0.0;
02614         emcmotDebug->teleop_data.desiredVel.tran.y = 0.0;
02615         emcmotDebug->teleop_data.desiredVel.tran.z = 0.0;
02616         emcmotDebug->teleop_data.desiredVel.a = 0.0;
02617         emcmotDebug->teleop_data.desiredVel.b = 0.0;
02618         emcmotDebug->teleop_data.desiredVel.c = 0.0;
02619         emcmotDebug->teleop_data.currentAccell.tran.x = 0.0;
02620         emcmotDebug->teleop_data.currentAccell.tran.y = 0.0;
02621         emcmotDebug->teleop_data.currentAccell.tran.z = 0.0;
02622         emcmotDebug->teleop_data.currentAccell.a = 0.0;
02623         emcmotDebug->teleop_data.currentAccell.b = 0.0;
02624         emcmotDebug->teleop_data.currentAccell.c = 0.0;
02625         emcmotDebug->teleop_data.desiredAccell.tran.x = 0.0;
02626         emcmotDebug->teleop_data.desiredAccell.tran.y = 0.0;
02627         emcmotDebug->teleop_data.desiredAccell.tran.z = 0.0;
02628         emcmotDebug->teleop_data.desiredAccell.a = 0.0;
02629         emcmotDebug->teleop_data.desiredAccell.b = 0.0;
02630         emcmotDebug->teleop_data.desiredAccell.c = 0.0;
02631         SET_MOTION_TELEOP_FLAG(1);
02632         SET_MOTION_ERROR_FLAG(0);
02633       }
02634       else {
02635         /* not in position-- don't honor mode change */
02636         emcmotDebug->teleoperating = 0;
02637       }
02638     }
02639     else {
02640       if (GET_MOTION_INPOS_FLAG()) {
02641         if (!emcmotDebug->teleoperating  && GET_MOTION_TELEOP_FLAG()) {
02642           SET_MOTION_TELEOP_FLAG(0);
02643           if (! emcmotDebug->coordinating) {
02644             for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02645               /* update free planner positions */
02646               emcmotDebug->freePose.tran.x = emcmotDebug->jointPos[axis];
02647               tpSetPos(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
02648               /* drain the cubics so they'll synch up */
02649               cubicDrain(&emcmotDebug->cubic[axis]);
02650             }
02651           }
02652         }
02653       }
02654 
02655       /* check for entering coordinated mode */
02656       if (emcmotDebug->coordinating && ! GET_MOTION_COORD_FLAG()) {
02657         if (GET_MOTION_INPOS_FLAG()) {
02658           /* update coordinated emcmotDebug->queue position */
02659           tpSetPos(&emcmotDebug->queue, emcmotStatus->pos);
02660           /* drain the cubics so they'll synch up */
02661           for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02662             cubicDrain(&emcmotDebug->cubic[axis]);
02663           }
02664           /* clear the override limits flags */
02665           emcmotDebug->overriding = 0;
02666           emcmotStatus->overrideLimits = 0;
02667           SET_MOTION_COORD_FLAG(1);
02668           SET_MOTION_TELEOP_FLAG(0);
02669           SET_MOTION_ERROR_FLAG(0);
02670         }
02671         else {
02672           /* not in position-- don't honor mode change */
02673           emcmotDebug->coordinating = 0;
02674         }
02675       }
02676 
02677       /* check entering free space mode */
02678       if (! emcmotDebug->coordinating && GET_MOTION_COORD_FLAG()) {
02679         if (GET_MOTION_INPOS_FLAG()) {
02680           for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02681             /* update free planner positions */
02682             emcmotDebug->freePose.tran.x = emcmotDebug->jointPos[axis];
02683             tpSetPos(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
02684             /* drain the cubics so they'll synch up */
02685             cubicDrain(&emcmotDebug->cubic[axis]);
02686           }
02687           SET_MOTION_COORD_FLAG(0);
02688           SET_MOTION_TELEOP_FLAG(0);
02689           SET_MOTION_ERROR_FLAG(0);
02690         }
02691         else {
02692           /* not in position-- don't honor mode change */
02693           emcmotDebug->coordinating = 1;
02694         }
02695       }
02696 
02697     }
02698 
02699     /* check for homing sequences */
02700     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02701       if (GET_AXIS_HOMING_FLAG(axis)) {
02702         if (tpIsDone(&emcmotDebug->freeAxis[axis])) {
02703           /* check for decel or final move */
02704           if (emcmotDebug->homingPhase[axis] == 5) {
02705             /* reset flag-- we're back at home */
02706             emcmotDebug->homingPhase[axis] = 0;
02707 
02708             /* rework the home kinematics values-- this could be done
02709                after each message to set world or joint kinematics, but
02710                we'll defer it to here so it happens just prior to when
02711                it's needed. Note that the nominal value of these may be
02712                changed, if the kinematics need to. */
02713             kinematicsHome(&worldHome, emcmotDebug->jointHome, &fflags, &iflags);
02714 
02715             /* clear flag that indicates all joints need rehoming if any
02716                joint is moved, for machines with no forward kins */
02717             rehomeAll = 0;
02718 
02719             /* set input offset to value such that resulting input
02720                is the emcmotDebug->jointHome[] value, with:
02721                input = (raw - offset) / scale ->
02722                offset = raw - input * scale ->
02723                offset = raw - emcmotDebug->jointHome * scale */
02724 
02725             emcmotStatus->inputOffset[axis] = emcmotDebug->rawInput[axis] -
02726               emcmotDebug->jointHome[axis] * emcmotStatus->inputScale[axis];
02727 
02728             /* recompute inputs to match so we don't have a
02729                momentary jump */
02730             emcmotStatus->input[axis] = emcmotDebug->jointHome[axis];
02731             emcmotDebug->oldInput[axis] = emcmotStatus->input[axis];
02732             
02733             /* reset interpolator so that it doesn't jump */
02734             cubicOffset(&emcmotDebug->cubic[axis], emcmotDebug->jointHome[axis] - emcmotDebug->coarseJointPos[axis]);
02735 
02736             /* set axis position to emcmotDebug->jointHome upon homing */
02737             emcmotDebug->jointPos[axis] = emcmotDebug->jointHome[axis];
02738             emcmotDebug->freePose.tran.x = emcmotDebug->jointHome[axis];
02739             tpSetPos(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
02740 
02741             SET_AXIS_HOMING_FLAG(axis, 0);
02742             SET_AXIS_HOMED_FLAG(axis, 1);
02743 
02744             /* set emcmotDebug->allHomed flag if all active axes are homed; this
02745                will signify that kinematics functions can be called */
02746             emcmotDebug->allHomed = 1;
02747             for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
02748               if (GET_AXIS_ACTIVE_FLAG(t) &&
02749                   ! GET_AXIS_HOMED_FLAG(t)) {
02750                 /* one active axis is not homed */
02751                 emcmotDebug->allHomed = 0;
02752                 break;
02753               }
02754             }
02755             /* FIXME-- this only works with no forward kins */
02756             if (emcmotDebug->allHomed) {
02757               emcmotStatus->pos = worldHome;
02758               emcmotStatus->actualPos = worldHome;
02759             }
02760           }
02761 
02762           if (emcmotDebug->homingPhase[axis] == 4) {
02763             /* just finished decel, now we'll do final home */
02764             /* add move back to latched position + backoff */
02765             emcmotDebug->freePose.tran.x =
02766               (emcmotDebug->saveLatch[axis] - 
02767                emcmotStatus->inputOffset[axis]) *
02768               emcmotDebug->inverseInputScale[axis] +
02769               emcmotConfig->homeOffset[axis];
02770             /* Note that I put a multiplication factor of 2 in front of 
02771                the homing velocity below. The reason is that, I think,
02772                if you found the index pulse you know your exact position
02773                it is save to travel with higher speeds. In addition to
02774                that, you actually see that the machine has found its
02775                index pulse for the specified axis */
02776             tpSetVmax(&emcmotDebug->freeAxis[axis], 
02777                       2 * (emcmotConfig->homingVel[axis]));
02778             if ((retval = tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose)) == 0) {
02779               /* Advance homing sequence only if motion is added to the tp */
02780               emcmotDebug->homingPhase[axis] = 5;
02781             }
02782           }
02783         } /* end of: if axis is done either decel or final home */
02784       } /* end of: if axis is homing */
02785     } /* end of: axis loop that checks for homing */
02786 
02787     /* RUN MOTION CALCULATIONS: */
02788 
02789     /* run axis interpolations and outputs, but only if we're
02790        enabled. This section is "suppressed" if we're not
02791        enabled, although the read/write of encoders/dacs is still
02792        done. */
02793     whichCycle = 0;
02794     if (GET_MOTION_ENABLE_FLAG()) {
02795       /* set whichCycle to be at least a servo cycle, for calc time logging */
02796       whichCycle = 1;
02797 
02798       /* check to see if the interpolators are empty */
02799       while (cubicNeedNextPoint(&emcmotDebug->cubic[0])) {
02800         /* they're empty, so pull next point(s) off Cartesian or
02801            joint planner, depending upon coord or free mode. */
02802 
02803         /* check to see whether we're in teleop, coordinated or free mode, to
02804            decide which motion planner to call */
02805         if (GET_MOTION_TELEOP_FLAG()) {
02806           double accell_mag;
02807 
02808           emcmotDebug->teleop_data.desiredAccell.tran.x = (emcmotDebug->teleop_data.desiredVel.tran.x - emcmotDebug->teleop_data.currentVel.tran.x)/emcmotConfig->trajCycleTime;
02809           emcmotDebug->teleop_data.desiredAccell.tran.y = (emcmotDebug->teleop_data.desiredVel.tran.y - emcmotDebug->teleop_data.currentVel.tran.y)/emcmotConfig->trajCycleTime;
02810           emcmotDebug->teleop_data.desiredAccell.tran.z = (emcmotDebug->teleop_data.desiredVel.tran.z - emcmotDebug->teleop_data.currentVel.tran.z)/emcmotConfig->trajCycleTime;
02811           pmCartMag(emcmotDebug->teleop_data.desiredAccell.tran, &accell_mag);
02812           emcmotDebug->teleop_data.desiredAccell.a = (emcmotDebug->teleop_data.desiredVel.a - emcmotDebug->teleop_data.currentVel.a)/emcmotConfig->trajCycleTime;
02813           emcmotDebug->teleop_data.desiredAccell.b = (emcmotDebug->teleop_data.desiredVel.b - emcmotDebug->teleop_data.currentVel.b)/emcmotConfig->trajCycleTime;
02814           emcmotDebug->teleop_data.desiredAccell.c = (emcmotDebug->teleop_data.desiredVel.c - emcmotDebug->teleop_data.currentVel.c)/emcmotConfig->trajCycleTime;
02815           if (emcmotDebug->teleop_data.desiredAccell.a > accell_mag) {
02816             accell_mag = emcmotDebug->teleop_data.desiredAccell.a;
02817           }
02818           if (emcmotDebug->teleop_data.desiredAccell.b > accell_mag) {
02819             accell_mag = emcmotDebug->teleop_data.desiredAccell.b;
02820           }
02821           if (emcmotDebug->teleop_data.desiredAccell.c > accell_mag) {
02822             accell_mag = emcmotDebug->teleop_data.desiredAccell.c;
02823           }
02824           if (accell_mag > emcmotStatus->acc) {
02825             pmCartScalMult(emcmotDebug->teleop_data.desiredAccell.tran, emcmotStatus->acc/accell_mag,
02826                            &emcmotDebug->teleop_data.currentAccell.tran);
02827             emcmotDebug->teleop_data.currentAccell.a =
02828               emcmotDebug->teleop_data.desiredAccell.a * emcmotStatus->acc/accell_mag;
02829             emcmotDebug->teleop_data.currentAccell.b =
02830               emcmotDebug->teleop_data.desiredAccell.b * emcmotStatus->acc/accell_mag;
02831             emcmotDebug->teleop_data.currentAccell.c =
02832               emcmotDebug->teleop_data.desiredAccell.c * emcmotStatus->acc/accell_mag;
02833             emcmotDebug->teleop_data.currentVel.tran.x += emcmotDebug->teleop_data.currentAccell.tran.x*emcmotConfig->trajCycleTime;
02834             emcmotDebug->teleop_data.currentVel.tran.y += emcmotDebug->teleop_data.currentAccell.tran.y*emcmotConfig->trajCycleTime;
02835             emcmotDebug->teleop_data.currentVel.tran.z += emcmotDebug->teleop_data.currentAccell.tran.z*emcmotConfig->trajCycleTime;
02836             emcmotDebug->teleop_data.currentVel.a += emcmotDebug->teleop_data.currentAccell.a*emcmotConfig->trajCycleTime;
02837             emcmotDebug->teleop_data.currentVel.b += emcmotDebug->teleop_data.currentAccell.b*emcmotConfig->trajCycleTime;
02838             emcmotDebug->teleop_data.currentVel.c += emcmotDebug->teleop_data.currentAccell.c*emcmotConfig->trajCycleTime;
02839           }
02840           else {
02841             emcmotDebug->teleop_data.currentAccell = emcmotDebug->teleop_data.desiredAccell;
02842             emcmotDebug->teleop_data.currentVel = emcmotDebug->teleop_data.desiredVel;
02843           }
02844 
02845           emcmotStatus->pos.tran.x += emcmotDebug->teleop_data.currentVel.tran.x*emcmotConfig->trajCycleTime;
02846           emcmotStatus->pos.tran.y += emcmotDebug->teleop_data.currentVel.tran.y*emcmotConfig->trajCycleTime;
02847           emcmotStatus->pos.tran.z += emcmotDebug->teleop_data.currentVel.tran.z*emcmotConfig->trajCycleTime;         
02848           emcmotStatus->pos.a += emcmotDebug->teleop_data.currentVel.a*emcmotConfig->trajCycleTime;
02849           emcmotStatus->pos.b += emcmotDebug->teleop_data.currentVel.b*emcmotConfig->trajCycleTime;
02850           emcmotStatus->pos.c += emcmotDebug->teleop_data.currentVel.c*emcmotConfig->trajCycleTime;           
02851 
02852           /* convert to joints */
02853           kinematicsInverse(&emcmotStatus->pos, emcmotDebug->coarseJointPos, &iflags, &fflags);
02854 
02855           /* spline joints up-- note that we may be adding points that
02856              fail soft limits, but we'll abort at the end of this cycle
02857              so it doesn't really matter */
02858           for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02859             cubicAddPoint(&emcmotDebug->cubic[axis], emcmotDebug->coarseJointPos[axis]);
02860           }
02861 
02862           if (kinType == KINEMATICS_IDENTITY) {
02863             /* call forward kinematics on input points for actual pos,
02864                at trajectory rate to save bandwidth */
02865             kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);
02866           }
02867           else {
02868             /* fake it by setting actual pos to commanded pos */
02869             emcmotStatus->actualPos = emcmotStatus->pos;
02870           }
02871         }
02872         else {
02873           if (GET_MOTION_COORD_FLAG()) {
02874             /* we're in coordinated mode-- pull a pose off the Cartesian
02875                trajectory planner, run it through the inverse kinematics,
02876                and spline up the joint points for interpolation in
02877                servo cycles. */
02878 
02879             /* set whichCycle to be a Cartesian trajectory cycle,
02880                for calc time logging */
02881             whichCycle = 2;
02882 
02883             /* run coordinated trajectory planning cycle */
02884             tpRunCycle(&emcmotDebug->queue);
02885 
02886             /* set new commanded traj pos */
02887             emcmotStatus->pos = tpGetPos(&emcmotDebug->queue);
02888 
02889             /* convert to joints */
02890             kinematicsInverse(&emcmotStatus->pos, emcmotDebug->coarseJointPos, &iflags, &fflags);
02891 
02892             /* spline joints up-- note that we may be adding points that
02893                fail soft limits, but we'll abort at the end of this cycle
02894                so it doesn't really matter */
02895             for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02896               cubicAddPoint(&emcmotDebug->cubic[axis], emcmotDebug->coarseJointPos[axis]);
02897             }
02898 
02899             if (kinType == KINEMATICS_IDENTITY) {
02900               /* call forward kinematics on input points for actual pos,
02901                  at trajectory rate to save bandwidth */
02902               kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);
02903             }
02904             else {
02905               /* fake it by setting actual pos to commanded pos */
02906               emcmotStatus->actualPos = emcmotStatus->pos;
02907             }
02908 
02909             /* now emcmotStatus->actualPos, emcmotStatus->pos, and
02910                emcmotDebug->coarseJointPos[] are set */
02911 
02912           } /* end of: coord mode */
02913           else {
02914             /* we're in free mode-- run joint planning cycles */
02915             for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02916               /* set whichCycle to be a joint trajectory cycle,
02917                  for calc time logging */
02918               /* note that this may include one or more joint trajectory
02919                  cycles, so calc time may be inherently variable */
02920               whichCycle = 2;
02921 
02922               /* run joint trajectory planning cycle */
02923               tpRunCycle(&emcmotDebug->freeAxis[axis]);
02924 
02925               /* set new coarse joint position. FIXME-- this uses only
02926                  the tran.x field of the TP_STRUCT, which is overkill.
02927                  We need a TP_STRUCT with a single scalar element. */
02928               emcmotDebug->coarseJointPos[axis] = tpGetPos(&emcmotDebug->freeAxis[axis]).tran.x;
02929 
02930               /* spline joint up-- note that we may be adding a point that
02931                  fails soft limit, but we'll abort at the end of this cycle
02932                  so it doesn't really matter */
02933               cubicAddPoint(&emcmotDebug->cubic[axis], emcmotDebug->coarseJointPos[axis]);
02934             } /* end of: axis for loop for joint planning cycle */
02935             
02936             if (kinType == KINEMATICS_IDENTITY) {
02937               /* set actualPos from actual inputs */
02938               kinematicsForward(emcmotStatus->input, &emcmotStatus->actualPos, &fflags, &iflags);
02939               /* set pos from nominal joints, since we're in joint mode */
02940               kinematicsForward(emcmotDebug->coarseJointPos, &emcmotStatus->pos, &fflags, &iflags);
02941             }
02942             else if (kinType != KINEMATICS_INVERSE_ONLY) {
02943               /* here is where we call the forward kinematics repeatedly,
02944                  when we're in free mode, so that the world coordinates are
02945                  kept up to date when joints are moving. This is only done
02946                  if we have the kinematics. emcmotStatus->pos needs to be set
02947                  with an estimate for the kinematics to converge, which is
02948                  true when we enter free mode from coordinated mode or after
02949                  the machine is homed. */
02950               EmcPose temp = emcmotStatus->pos;
02951               if (0 == kinematicsForward(emcmotStatus->input, &temp, &fflags, &iflags)) {
02952                 emcmotStatus->pos = temp;
02953                 emcmotStatus->actualPos = temp;
02954               }
02955               /* else leave them alone */
02956             }
02957             else {
02958               /* no foward kins, and we're in joint mode, so we have no
02959                  estimate of world coords, and we have to leave them alone */
02960             }
02961 
02962             /* now emcmotStatus->actualPos, emcmotStatus->pos, and
02963                emcmotDebug->coarseJointPos[] are set */
02964 
02965           } /* end of: free mode trajectory planning */
02966         } /* end of: not teleop mode  */
02967       } /* end of: while (cubicNeedNextPoint(0)) */
02968 
02969       /* we're still in motion enabled section. For coordinated mode,
02970          the Cartesian trajectory cycle has been computed, if necessary,
02971          run through the inverse kinematics, and the joints have been
02972          splined up for interpolation. For free mode, the joint trajectory
02973          cycles have been computed, if necessary, and the joints have
02974          been splined up for interpolation. We still need to push the
02975          actual input through the forward kinematics, for actual pos.
02976 
02977          Effects:
02978 
02979          For coord mode, emcmotStatus->pos contains the commanded
02980          Cartesian pose, emcmotDebug->coarseJointPos[] contains the results of the
02981          inverse kinematics at the coarse (trajectory) rate, and the
02982          interpolators are not empty.
02983 
02984          For free mode, emcmotStatus->pos is unchanged, and needs to
02985          be updated via the forward kinematics. FIXME-- make sure this
02986          happens, and note where in this comment. emcmotDebug->coarseJointPos[]
02987          contains the results of the joint trajectory calculations
02988          at the coarse (trajectory) rate, and the interpolators are
02989          not empty.
02990       */
02991 
02992       /* check for soft joint limits. If so, abort all motion. The
02993          interpolators will pick this up further down and begin
02994          planning abort and stop. */
02995       emcmotDebug->onLimit = 0;
02996       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
02997         SET_AXIS_PSL_FLAG(axis, 0);
02998         SET_AXIS_NSL_FLAG(axis, 0);
02999         if (GET_AXIS_HOMED_FLAG(axis)) {
03000           if (emcmotDebug->coarseJointPos[axis] > emcmotConfig->maxLimit[axis]) {
03001             SET_AXIS_ERROR_FLAG(axis, 1);
03002             SET_AXIS_PSL_FLAG(axis, 1);
03003             emcmotDebug->onLimit = 1;
03004           }
03005           else if (emcmotDebug->coarseJointPos[axis] < emcmotConfig->minLimit[axis]) {
03006             SET_AXIS_ERROR_FLAG(axis, 1);
03007             SET_AXIS_NSL_FLAG(axis, 1);
03008           }
03009         }
03010       }
03011 
03012       /* reset emcmotDebug->wasOnLimit flag iff all joints are free of
03013          soft limits, as seen in the flag bits set last cycle.
03014          No need to do this for hard limits, since emcmotDebug->wasOnLimit
03015          only prevents flurry of aborts while on a soft limit
03016          and hard limits don't abort, they disable. */
03017       if (emcmotDebug->onLimit) {
03018         if (! emcmotDebug->wasOnLimit) {
03019           /* abort everything, regardless of coord or free mode */
03020           tpAbort(&emcmotDebug->queue);
03021           for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03022             tpAbort(&emcmotDebug->freeAxis[axis]);
03023             emcmotDebug->wasOnLimit = 1;
03024           }
03025         }
03026         /* else we were on a limit, so inhibit firing of aborts */
03027       }
03028       else {
03029         /* not on a limit, so clear emcmotDebug->wasOnLimit so aborts fire next
03030            time we are on a limit */
03031         emcmotDebug->wasOnLimit = 0;
03032       }
03033 
03034       if (whichCycle == 2) {
03035         /* we're on a trajectory cycle, either Cartesian or joint planning, so
03036            log per-traj-cycle data if logging active */
03037         logIt = 0;
03038 
03039         if (emcmotStatus->logStarted &&
03040             emcmotStatus->logSkip >= 0) {
03041 
03042           /* calculate current vel, accel, for logging */
03043           emcmotDebug->newVel.tran.x = (emcmotStatus->pos.tran.x - emcmotDebug->oldPos.tran.x) /
03044             emcmotConfig->trajCycleTime;
03045           emcmotDebug->newVel.tran.y = (emcmotStatus->pos.tran.y - emcmotDebug->oldPos.tran.y) /
03046             emcmotConfig->trajCycleTime;
03047           emcmotDebug->newVel.tran.z = (emcmotStatus->pos.tran.z - emcmotDebug->oldPos.tran.z) /
03048             emcmotConfig->trajCycleTime;
03049           emcmotDebug->oldPos = emcmotStatus->pos;
03050           emcmotDebug->newAcc.tran.x = (emcmotDebug->newVel.tran.x - emcmotDebug->oldVel.tran.x) /
03051             emcmotConfig->trajCycleTime;
03052           emcmotDebug->newAcc.tran.y = (emcmotDebug->newVel.tran.y - emcmotDebug->oldVel.tran.y) /
03053             emcmotConfig->trajCycleTime;
03054           emcmotDebug->newAcc.tran.z = (emcmotDebug->newVel.tran.z - emcmotDebug->oldVel.tran.z) /
03055             emcmotConfig->trajCycleTime;
03056           emcmotDebug->oldVel = emcmotDebug->newVel;
03057 
03058           /* save the type with the log item */
03059           ls.type = emcmotStatus->logType;
03060 
03061           /* now log type-specific data */
03062           switch (emcmotStatus->logType) {
03063 
03064           case EMCMOT_LOG_TYPE_TRAJ_POS:
03065             if (logSkip-- <= 0) {
03066               ls.item.trajPos.time = start - logStartTime;
03067               ls.item.trajPos.pos = emcmotStatus->pos.tran;
03068               logSkip = emcmotStatus->logSkip;
03069               logIt = 1;
03070             }
03071             break;
03072 
03073           case EMCMOT_LOG_TYPE_TRAJ_VEL:
03074             if (logSkip-- <= 0) {
03075               ls.item.trajVel.time = start - logStartTime;
03076               ls.item.trajVel.vel = emcmotDebug->newVel.tran;
03077               pmCartMag(emcmotDebug->newVel.tran, &ls.item.trajVel.mag);
03078               logSkip = emcmotStatus->logSkip;
03079               logIt = 1;
03080             }
03081             break;
03082 
03083           case EMCMOT_LOG_TYPE_TRAJ_ACC:
03084             if (logSkip-- <= 0) {
03085               ls.item.trajAcc.time = start - logStartTime;
03086               ls.item.trajAcc.acc = emcmotDebug->newAcc.tran;
03087               pmCartMag(emcmotDebug->newAcc.tran, &ls.item.trajAcc.mag);
03088               logSkip = emcmotStatus->logSkip;
03089               logIt = 1;
03090             }
03091             break;
03092 
03093           default:
03094             break;
03095           } /* end of: switch on log type */
03096 
03097           /* now log it */
03098           if (logIt) {
03099             emcmotLogAdd(emcmotLog, ls);
03100             emcmotStatus->logPoints = emcmotLog->howmany;
03101             logIt = 0;
03102           }
03103         } /* end of: if (emcmotStatus->logStarted &&
03104              emcmotStatus->logSkip >= 0) */
03105       } /* end of: if (whichCycle == 2), for trajectory cycle logging */
03106 
03107       /* run interpolation and compensation */
03108       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03109         /* interpolate */
03110         emcmotDebug->oldJointPos[axis] = emcmotDebug->jointPos[axis];
03111         emcmotDebug->jointPos[axis] = cubicInterpolate(&emcmotDebug->cubic[axis], 0, 0, 0, 0);
03112         emcmotDebug->jointVel[axis] = (emcmotDebug->jointPos[axis] - emcmotDebug->oldJointPos[axis]) / emcmotConfig->servoCycleTime;
03113 #ifdef COMPING
03114         /* set direction flag iff there's a direction change, otherwise
03115            leave it alone so that stops won't change dir */
03116         if (emcmotDebug->jointVel[axis] > 0.0 && dir[axis] < 0) {
03117           dir[axis] = 1;
03118         }
03119         else if (emcmotDebug->jointVel[axis] < 0.0 && dir[axis] > 0) {
03120           dir[axis] = -1;
03121         }
03122         /* else leave it alone */
03123 #endif /* COMPING */
03124 
03125         /* run output calculations */
03126         if (GET_AXIS_ACTIVE_FLAG(axis)) {
03127           /* BACKLASH COMPENSATION */
03128           /* FIXME-- make backlash part of the EMC status proper, not
03129              the PID structure */
03130           if (emcmotDebug->jointPos[axis] - emcmotDebug->oldJointPos[axis] > 0.0) {
03131             oldbcomp = emcmotDebug->bcomp[axis];
03132             emcmotDebug->bcomp[axis] = + emcmotConfig->pid[axis].backlash / 2.0;
03133             if (emcmotDebug->bcompdir[axis] != + 1) {
03134               emcmotDebug->bac_done[axis] = 0;
03135               emcmotDebug->bac_di[axis] = emcmotDebug->bcompincr[axis];
03136               emcmotDebug->bac_d[axis] = 0;
03137               emcmotDebug->bac_D[axis] = emcmotDebug->bcomp[axis] - oldbcomp;
03138               emcmotDebug->bac_halfD[axis] = 0.5 * emcmotDebug->bac_D[axis];
03139               emcmotDebug->bac_incrincr[axis] = emcmotStatus->acc *
03140                 emcmotConfig->servoCycleTime * emcmotConfig->servoCycleTime;
03141               emcmotDebug->bac_incr[axis] = - 0.5 * emcmotDebug->bac_incrincr[axis];
03142               emcmotDebug->bcompdir[axis] = + 1;
03143             }
03144           }
03145           else if (emcmotDebug->jointPos[axis] - emcmotDebug->oldJointPos[axis] < 0.0) {
03146             oldbcomp = emcmotDebug->bcomp[axis];
03147             emcmotDebug->bcomp[axis] = - emcmotConfig->pid[axis].backlash / 2.0;
03148             if (emcmotDebug->bcompdir[axis] != - 1) {
03149               emcmotDebug->bac_done[axis] = 0;
03150               emcmotDebug->bac_di[axis] = emcmotDebug->bcompincr[axis];
03151               emcmotDebug->bac_d[axis] = 0;
03152               emcmotDebug->bac_D[axis] = emcmotDebug->bcomp[axis] - oldbcomp;
03153               emcmotDebug->bac_halfD[axis] = 0.5 * emcmotDebug->bac_D[axis];
03154               emcmotDebug->bac_incrincr[axis] = emcmotStatus->acc *
03155                 emcmotConfig->servoCycleTime * emcmotConfig->servoCycleTime;
03156               emcmotDebug->bac_incr[axis] = - 0.5 * emcmotDebug->bac_incrincr[axis];
03157               emcmotDebug->bcompdir[axis] = - 1;
03158             }
03159           }
03160           /* else no motion, so leave emcmotDebug->bcomp what it was */
03161 
03162           /* mete out the backlash according to an acc/dec profile */
03163           if (! emcmotDebug->bac_done[axis]) {
03164             if (emcmotDebug->bac_D[axis] > 0.0) {
03165               emcmotDebug->bcompincr[axis] = emcmotDebug->bac_di[axis] + emcmotDebug->bac_d[axis];
03166               if (emcmotDebug->bac_d[axis] < emcmotDebug->bac_halfD[axis]) {
03167                 emcmotDebug->bac_incr[axis] += emcmotDebug->bac_incrincr[axis];
03168                 emcmotDebug->bac_d[axis] += emcmotDebug->bac_incr[axis];
03169               }
03170               else if (emcmotDebug->bac_d[axis] < emcmotDebug->bac_D[axis]) {
03171                 emcmotDebug->bac_incr[axis] -= emcmotDebug->bac_incrincr[axis];
03172                 emcmotDebug->bac_d[axis] += emcmotDebug->bac_incr[axis];
03173               }
03174               if (emcmotDebug->bac_d[axis] >= emcmotDebug->bac_D[axis] || emcmotDebug->bac_incr[axis] <= 0.0) {
03175                 emcmotDebug->bac_done[axis] = 1;
03176               }
03177             }
03178             else {
03179               emcmotDebug->bcompincr[axis] = emcmotDebug->bac_di[axis] + emcmotDebug->bac_d[axis];
03180               if (emcmotDebug->bac_d[axis] > emcmotDebug->bac_halfD[axis]) {
03181                 emcmotDebug->bac_incr[axis] += emcmotDebug->bac_incrincr[axis];
03182                 emcmotDebug->bac_d[axis] -= emcmotDebug->bac_incr[axis];
03183               }
03184               else if (emcmotDebug->bac_d[axis] > emcmotDebug->bac_D[axis]) {
03185                 emcmotDebug->bac_incr[axis] -= emcmotDebug->bac_incrincr[axis];
03186                 emcmotDebug->bac_d[axis] -= emcmotDebug->bac_incr[axis];
03187               }
03188               if (emcmotDebug->bac_d[axis] <= emcmotDebug->bac_D[axis] || emcmotDebug->bac_incr[axis] <= 0.0) {
03189                 emcmotDebug->bac_done[axis] = 1;
03190               }
03191             }
03192           } /* end of: if not emcmotDebug->bac_done[axis] */
03193 
03194           /* ADJUST OUTPUT: */
03195 
03196           /*
03197             this computes outJointPos[] from emcmotDebug->jointPos[], by adding backlash
03198             compensation and rounding result to nearest input unit
03199           */
03200           emcmotDebug->outJointPos[axis] = emcmotDebug->jointPos[axis] + emcmotDebug->bcompincr[axis];
03201 #ifndef NO_ROUNDING
03202           numres = outJointPos[axis] * emcmotStatus->inputScale[axis];
03203           if (numres < 0.0) {
03204             outJointPos[axis] = emcmotDebug->inverseInputScale[axis] * ((int) (numres - 0.5));
03205           }
03206           else {
03207             outJointPos[axis] = emcmotDebug->inverseInputScale[axis] * ((int) (numres + 0.5));
03208           }
03209 #endif
03210 
03211           /* COMPENSATE: */
03212 
03213           /*
03214             compensation means compute output
03215             'emcmotStatus->output[]' based on desired position
03216             'outJointPos[]' and input 'emcmotStatus->input[]'.
03217 
03218             Currently the source calls for PID compensation.
03219             FIXME-- add wrapper for compensator, with ptr to
03220             emcmotStatus struct, with semantics that ->output[]
03221             needs to be filled.
03222           */
03223 
03224           /* here is PID compensation */
03225           /* note that we have to compare adjusted output 'outJointPos'
03226              with the input, but the input has already had backlash comp
03227              taken out, while the output has just had it added in. So,
03228              we need to add it to the input for this calculation */
03229           emcmotStatus->output[axis] =
03230             pidRunCycle(&emcmotConfig->pid[axis],
03231                         emcmotStatus->input[axis] + emcmotDebug->bcompincr[axis],
03232                         emcmotDebug->outJointPos[axis]);
03233 
03234           /* COMPUTE FOLLOWING ERROR: */
03235 
03236           /* compute signed following error and magnitude */
03237           thisFerror[axis] = emcmotDebug->jointPos[axis] - emcmotStatus->input[axis];
03238           magFerror = fabs(thisFerror[axis]);
03239           emcmotDebug->ferrorCurrent[axis] = thisFerror[axis];
03240 
03241           /* record the max ferror for this axis */
03242           if (emcmotDebug->ferrorHighMark[axis] < magFerror) {
03243             emcmotDebug->ferrorHighMark[axis] = magFerror;
03244           }
03245 
03246           /* compute the scaled ferror for a move of this speed */
03247           limitFerror = emcmotConfig->maxFerror[axis] /
03248             emcmotConfig->limitVel *
03249             emcmotDebug->jointVel[axis];
03250           if (limitFerror < emcmotConfig->minFerror[axis]) {
03251             limitFerror = emcmotConfig->minFerror[axis];
03252           }
03253 
03254           /* abort if this ferror is greater than the scaled ferror */
03255           if (magFerror > limitFerror) {
03256             /* abort! abort! following error exceeded */
03257             SET_AXIS_ERROR_FLAG(axis, 1);
03258             SET_AXIS_FERROR_FLAG(axis, 1);
03259             if (emcmotDebug->enabling) {
03260               /* report the following error just this once */
03261               reportError("axis %d following error", axis);
03262             }
03263             emcmotDebug->enabling = 0;
03264           }
03265           else {
03266             SET_AXIS_FERROR_FLAG(axis, 0);
03267           }
03268         } /* end of: if (GET_AXIS_ACTIVE_FLAG(axis)) */
03269         else {
03270           /* axis is not active-- leave the pid output where it is--
03271              if axis is not active one can still write to the dac */
03272         }
03273 
03274         /* CLAMP OUTPUT: */
03275 
03276         /*
03277           clamp output means take 'emcmotStatus->output[]' and
03278           limit to range
03279           'emcmotConfig->minOutput[] .. emcmotConfig->maxOutput[]'
03280         */
03281         if (emcmotStatus->output[axis] < emcmotConfig->minOutput[axis]) {
03282           emcmotStatus->output[axis] = emcmotConfig->minOutput[axis];
03283         }
03284         else if (emcmotStatus->output[axis] > emcmotConfig->maxOutput[axis]) {
03285           emcmotStatus->output[axis] = emcmotConfig->maxOutput[axis];
03286         }
03287 
03288         /* CHECK FOR LATCH CONDITION: */
03289         /*
03290           check for latch condition means if we're waiting for a
03291           latched index pulse, and we see the pulse switch, 
03292           we read the raw input and abort. The offset is set
03293           above in the homing section by noting that if we're homing,
03294           and emcmotDebug->homingPhase[] is 3, we latched.
03295 
03296           This presumes an encoder index pulse.
03297           FIXME-- remove explicit calls to encoder index pulse, to
03298           allow for open-loop control latching via switches only.
03299           Open-loop control can be achieved, at least for STG boards, by
03300           defining NO_INDEX_PULSE in extstgmot.c
03301         */
03302         if (emcmotDebug->homingPhase[axis] == 3) {
03303           /* read encoder index pulse */
03304           extEncoderReadLatch(axis, &emcmotDebug->latchFlag[axis]);
03305           if (emcmotDebug->latchFlag[axis]) {
03306             /* code below is excuted once the index pulse is found */
03307             /* call for an abort-- when it's finished, code
03308                above sets inputOffset[] to emcmotDebug->saveLatch[] */
03309             if (tpAbort(&emcmotDebug->freeAxis[axis]) == 0) {
03310               /* Only advance the homing sequence if the motion is really aborted */
03311               emcmotDebug->homingPhase[axis] = 4;
03312               /* get latched position in RAW UNITS */
03313               emcmotDebug->saveLatch[axis] = emcmotDebug->rawInput[axis];
03314             }
03315           } /* end of: if (emcmotDebug->latchFlag[axis]) */
03316         } /* end of: if (emcmotDebug->homingPhase[axis] == 3 */
03317 
03318         /* CHECK FOR HOMING PHASE 2, COMMAND THE MOVE TO THE INDEX PULSE */
03319         if (emcmotDebug->homingPhase[axis] == 2) {
03320           if (GET_AXIS_HOMING_POLARITY(axis)) {
03321             emcmotDebug->freePose.tran.x = - 2.0 * AXRANGE(axis);
03322           } else {
03323             emcmotDebug->freePose.tran.x = + 2.0 * AXRANGE(axis);
03324           }
03325           if ((retval = tpAddLine(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose)) == 0) {
03326             /* Only advance homing sequence if the motion is
03327                actually put in the traj. planner */
03328             extEncoderResetIndex(axis);
03329             emcmotDebug->homingPhase[axis] = 3;
03330           }
03331         } /* END OF: PHASE 2 */
03332 
03333         /* CHECK FOR HOME SWITCH CONDITION AND then tpAbort: */
03334         /* check if any of the home switch, phl, nhl are tripped */
03335         if (emcmotDebug->homingPhase[axis] == 1) {
03336           if (GET_AXIS_HOME_SWITCH_FLAG(axis) ||
03337               GET_AXIS_PHL_FLAG(axis) ||
03338               GET_AXIS_NHL_FLAG(axis)) {
03339             if (tpAbort(&emcmotDebug->freeAxis[axis]) == 0) {
03340               /* Advance homing sequence if motion is aborted */
03341               emcmotDebug->homingPhase[axis] = 2;
03342             }
03343           }
03344         } /* end of: if(emcmotDebug->homingPhase[axis]==1){ */
03345       } /* end of: for (axis = 0; ...) */
03346     } /* end of: if (GET_MOTION_ENABLE_FLAG()) */
03347     else {
03348       /*
03349         we're not enabled, so no motion planning or interpolation has
03350         been done. emcmotDebug->jointPos[] is set to emcmotStatus->input[],
03351         and likewise with emcmotDebug->coarseJointPos[], which is normally
03352         updated at the traj rate but it's convenient to do them here at the
03353         same time at the servo rate. emcmotStatus->pos, ->actualPos need to be
03354         run through forward kinematics.  Note that we are running at the servo
03355         rate, so we need to slow down by the interpolation factor to avoid
03356         soaking the CPU. If we were enabled, ->pos was set by calcs (coord
03357         mode) or forward kins (free mode), and ->actualPos was set by forward
03358         kins on ->input[], all at the trajectory rate.
03359       */
03360       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03361         emcmotDebug->coarseJointPos[axis] = emcmotStatus->input[axis];
03362         emcmotDebug->oldJointPos[axis] = emcmotDebug->jointPos[axis];
03363         emcmotDebug->jointPos[axis] = emcmotDebug->coarseJointPos[axis];
03364         emcmotDebug->jointVel[axis] = (emcmotDebug->jointPos[axis] - emcmotDebug->oldJointPos[axis]) / emcmotConfig->servoCycleTime;
03365       }
03366       /* synthesize the trajectory interpolation, via a counter that
03367          decrements from the interpolation rate. This causes the statements
03368          to execute at the trajectory rate instead of the servo rate at which
03369          this enclosing code is called. */
03370       if (--interpolationCounter <= 0) {
03371         if (kinType != KINEMATICS_INVERSE_ONLY) {
03372           /* call the forward kinematics, at the effective trajectory rate */
03373           EmcPose temp = emcmotStatus->pos;
03374           if (0 == kinematicsForward(emcmotStatus->input, &temp, &fflags, &iflags)) {
03375             emcmotStatus->pos = temp;
03376             emcmotStatus->actualPos = temp;
03377           }
03378         }
03379         /* else can't generate Cartesian position, so leave it alone */
03380 
03381         /* reload the interpolation counter that simulates the interpolation
03382            done when enabled */
03383         interpolationCounter = emcmotConfig->interpolationRate;
03384       }
03385     }
03386 
03387 #ifdef ENABLE_PROBING
03388     extDioRead(emcmotConfig->probeIndex, &emcmotStatus->probeval);
03389     if (emcmotStatus->probing && emcmotStatus->probeTripped) {
03390       tpClear(&emcmotDebug->queue);
03391       emcmotStatus->probing = 0;
03392     }
03393     else if (emcmotStatus->probing && GET_MOTION_INPOS_FLAG() &&
03394              tpQueueDepth(&emcmotDebug->queue) == 0) {
03395       emcmotStatus->probing = 0;
03396     }
03397     else if (emcmotStatus->probing) {
03398       if (emcmotStatus->probeval == emcmotConfig->probePolarity) {
03399         emcmotStatus->probeTripped = 1;
03400         emcmotStatus->probedPos = emcmotStatus->actualPos;
03401         if (GET_MOTION_COORD_FLAG()) {
03402           tpAbort(&emcmotDebug->queue);
03403           SET_MOTION_ERROR_FLAG(0);
03404         }
03405         else {
03406           /* check axis range */
03407           for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03408             tpAbort(&emcmotDebug->freeAxis[axis]);
03409             SET_AXIS_HOMING_FLAG(axis, 0);
03410             SET_AXIS_ERROR_FLAG(axis, 0);
03411           }
03412         }
03413       }
03414     }
03415 #endif /* ENABLE_PROBING */
03416 
03417     /* SCALE OUTPUTS: */
03418 
03419     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03420 #ifndef STEPPER_MOTORS
03421       emcmotDebug->rawOutput[axis] =
03422         (emcmotStatus->output[axis] - emcmotStatus->outputOffset[axis]) *
03423         emcmotDebug->inverseOutputScale[axis];
03424 #else
03425       /* lpg - stepper motors, rawoutput is typically -10->0->+10 with
03426        * zero indicating no correction required. For very small corrections,
03427        * the enable gets shut off so no motion takes place and gets used below
03428        * to avoid divide by zero calc (which would give infinite stepper periods)
03429        */
03430       emcmotDebug->rawOutput[axis] = emcmotStatus->output[axis];
03431       if (emcmotDebug->rawOutput[axis] < +OUTPUT_DELTA &&
03432           emcmotDebug->rawOutput[axis] > -OUTPUT_DELTA) {
03433         emcmotDebug->enable[axis] = 0;
03434       }
03435       else {
03436 #ifdef rtai
03437         pdmultfloat = 1e9 /
03438           (emcmotDebug->rawOutput[axis] *
03439            ((double) emcmotStatus->outputScale[axis]) *
03440            ((double) PERIOD));
03441 #else
03442 #ifdef USE_RTL2
03443         pdmultfloat = ((double) HRTICKS_PER_SEC) /
03444           (emcmotDebug->rawOutput[axis] *
03445            ((double) emcmotStatus->outputScale[axis]) *
03446            ((double) PERIOD));
03447 #else
03448         pdmultfloat = ((double) RT_TICKS_PER_SEC) /
03449           (emcmotDebug->rawOutput[axis] *
03450            ((double) emcmotStatus->outputScale[axis]) *
03451            ((double) PERIOD));
03452 #endif
03453 #endif
03454         /* round it */
03455         if (pdmultfloat < 0.0) {
03456           emcmotDebug->pdmult[axis] = (int) (pdmultfloat - 0.5);
03457           emcmotDebug->enable[axis] = 1;
03458           /* lpg */
03459           /*
03460             rounding may have caused a problem for very high gain/high
03461             error calcs. If negative number close to zero get rounded to 0, 
03462             we lose the sign information on pdmult. This seems to be handled 
03463             in the 1st phase drive routine, but is harmful to the 
03464             lpg_phase_drive().
03465           */
03466           if (emcmotDebug->pdmult[axis] == 0) {
03467             emcmotDebug->pdmult[axis] = -1;
03468           }
03469         }
03470         else {
03471           emcmotDebug->pdmult[axis] = (int) (pdmultfloat + 0.5);
03472           emcmotDebug->enable[axis] = 1;
03473           /* lpg - make sure pdmult > 0 as zero means infinite step rate */
03474           if (emcmotDebug->pdmult[axis] == 0) {
03475             emcmotDebug->pdmult[axis] = 1;
03476           }
03477         }
03478       }
03479 #endif
03480     }
03481 
03482     /* WRITE OUTPUTS: */
03483 
03484     /* write DACs-- note that this is done even when not
03485        enabled, although in this case the pidOutputs are
03486        all zero unless manually overridden. They will not
03487        be set by any calculations if we're not enabled. */
03488     extDacWriteAll(EMCMOT_MAX_AXIS, emcmotDebug->rawOutput);
03489 
03490     /* UPDATE THE REST OF THE DYNAMIC STATUS: */
03491 
03492     /* copy computed axis positions to status. Note that if motion is
03493        disabled, this is the same as ->input[]. */
03494     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03495       emcmotStatus->axisPos[axis] = emcmotDebug->jointPos[axis];
03496     }
03497 
03498     /* health heartbeat */
03499     emcmotStatus->heartbeat++;
03500 
03501     /* motion emcmotDebug->queue status */
03502     emcmotStatus->depth = tpQueueDepth(&emcmotDebug->queue);
03503     emcmotStatus->activeDepth = tpActiveDepth(&emcmotDebug->queue);
03504     emcmotStatus->id = tpGetExecId(&emcmotDebug->queue);
03505     emcmotStatus->queueFull = tcqFull(&emcmotDebug->queue.queue);
03506     SET_MOTION_INPOS_FLAG(0);
03507     if (tpIsDone(&emcmotDebug->queue)) {
03508       SET_MOTION_INPOS_FLAG(1);
03509     }
03510 
03511     /* axis status */
03512     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03513       SET_AXIS_INPOS_FLAG(axis, 0);
03514       if (tpIsDone(&emcmotDebug->freeAxis[axis])) {
03515         SET_AXIS_INPOS_FLAG(axis, 1);
03516       }
03517       else {
03518         /* this axis, at least, is moving, so set emcmotDebug->overriding flag */
03519         if (emcmotStatus->overrideLimits) {
03520           emcmotDebug->overriding = 1;
03521         }
03522       }
03523     }
03524 
03525     /* reset overrideLimits flag if we have started a move and now
03526        are in position */
03527     for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03528       if (! GET_AXIS_INPOS_FLAG(axis)) {
03529         break;
03530       }
03531     }
03532     if (axis == EMCMOT_MAX_AXIS) {
03533       /* ran through all axes, and all are in position */
03534       if (emcmotDebug->overriding) {
03535         emcmotDebug->overriding = 0;
03536         emcmotStatus->overrideLimits = 0;
03537       }
03538     }
03539 
03540     /* check to see if we should pause in order to implement
03541        single emcmotDebug->stepping */
03542     if (emcmotDebug->stepping && emcmotDebug->idForStep != emcmotStatus->id) {
03543       for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
03544         tpPause(&emcmotDebug->freeAxis[axis]);
03545       }
03546       tpPause(&emcmotDebug->queue);
03547       emcmotDebug->stepping = 0;
03548       emcmotStatus->paused = 1;
03549     }
03550 
03551     /* calculate elapsed time and min/max/avg */
03552     end = etime();
03553     delta = end - start;
03554 
03555     emcmotDebug->last_time = emcmotDebug->cur_time;
03556     emcmotDebug->cur_time = end;
03557     
03558     if (emcmotDebug->last_time != 0.0) {
03559       mmxavgAdd(&emcmotDebug->yMmxavg, (emcmotDebug->cur_time - emcmotDebug->last_time));
03560       emcmotDebug->yMin = mmxavgMin(&emcmotDebug->yMmxavg);
03561       emcmotDebug->yMax = mmxavgMax(&emcmotDebug->yMmxavg);
03562       emcmotDebug->yAvg = mmxavgAvg(&emcmotDebug->yMmxavg);
03563     }
03564 
03565     emcmotStatus->computeTime = delta;
03566     if (2 == whichCycle) {
03567       /* traj calcs done this cycle-- use tMin,Max,Avg */
03568       mmxavgAdd(&emcmotDebug->tMmxavg, delta);
03569       emcmotDebug->tMin = mmxavgMin(&emcmotDebug->tMmxavg);
03570       emcmotDebug->tMax = mmxavgMax(&emcmotDebug->tMmxavg);
03571       emcmotDebug->tAvg = mmxavgAvg(&emcmotDebug->tMmxavg);
03572     }
03573     else if (1 == whichCycle) {
03574       /* servo calcs only this cycle-- use sMin,Max,Avg */
03575       mmxavgAdd(&emcmotDebug->sMmxavg, delta);
03576       emcmotDebug->sMin = mmxavgMin(&emcmotDebug->sMmxavg);
03577       emcmotDebug->sMax = mmxavgMax(&emcmotDebug->sMmxavg);
03578       emcmotDebug->sAvg = mmxavgAvg(&emcmotDebug->sMmxavg);
03579     }
03580     else {
03581       /* calcs disabled this cycle-- use nMin,Max,Avg */
03582       mmxavgAdd(&emcmotDebug->nMmxavg, delta);
03583       emcmotDebug->nMin = mmxavgMin(&emcmotDebug->nMmxavg);
03584       emcmotDebug->nMax = mmxavgMax(&emcmotDebug->nMmxavg);
03585       emcmotDebug->nAvg = mmxavgAvg(&emcmotDebug->nMmxavg);
03586     }
03587 
03588     /* log per-servo-cycle data if logging active */
03589     logIt = 0;
03590     if (emcmotStatus->logStarted &&
03591         emcmotStatus->logSkip >= 0) {
03592 
03593       /* record type here, since all will set this */
03594       ls.type = emcmotStatus->logType;
03595 
03596       /* now log type-specific data */
03597       switch (ls.type) {
03598 
03599       case EMCMOT_LOG_TYPE_AXIS_POS:
03600         if (logSkip-- <= 0) {
03601           ls.item.axisPos.time = end - logStartTime;
03602           ls.item.axisPos.input = emcmotStatus->input[loggingAxis];
03603           ls.item.axisPos.output = emcmotDebug->jointPos[loggingAxis];
03604           logSkip = emcmotStatus->logSkip;
03605           logIt = 1;
03606         }
03607         break;
03608 
03609       case EMCMOT_LOG_TYPE_ALL_INPOS:
03610         if (logSkip-- <= 0) {
03611           ls.item.allInpos.time = end - logStartTime;
03612           for (axis = 0;
03613                axis < EMCMOT_LOG_NUM_AXES &&
03614                  axis < EMCMOT_MAX_AXIS;
03615                axis++) {
03616             ls.item.allInpos.input[axis] = emcmotStatus->input[axis];
03617           }
03618           logSkip = emcmotStatus->logSkip;
03619           logIt = 1;
03620         }
03621         break;
03622 
03623       case EMCMOT_LOG_TYPE_ALL_OUTPOS:
03624         if (logSkip-- <= 0) {
03625           ls.item.allOutpos.time = end - logStartTime;
03626           for (axis = 0;
03627                axis < EMCMOT_LOG_NUM_AXES &&
03628                  axis < EMCMOT_MAX_AXIS;
03629                axis++) {
03630             ls.item.allOutpos.output[axis] = emcmotDebug->jointPos[axis];
03631           }
03632           logSkip = emcmotStatus->logSkip;
03633           logIt = 1;
03634         }
03635         break;
03636 
03637       case EMCMOT_LOG_TYPE_AXIS_VEL:
03638         if (logSkip-- <= 0) {
03639           ls.item.axisVel.time = end - logStartTime;
03640           ls.item.axisVel.cmdVel = emcmotDebug->jointPos[loggingAxis] -
03641             emcmotDebug->oldJointPos[loggingAxis];
03642           ls.item.axisVel.actVel = emcmotStatus->input[loggingAxis] -
03643             emcmotDebug->oldInput[loggingAxis];
03644           logSkip = emcmotStatus->logSkip;
03645           logIt = 1;
03646         }
03647         break;
03648 
03649       case EMCMOT_LOG_TYPE_ALL_FERROR:
03650         if (logSkip-- <= 0) {
03651           ls.item.allFerror.time = end - logStartTime;
03652           for (axis = 0;
03653                axis < EMCMOT_LOG_NUM_AXES &&
03654                  axis < EMCMOT_MAX_AXIS;
03655                axis++) {
03656             ls.item.allFerror.ferror[axis] = emcmotDebug->ferrorCurrent[axis];
03657           }
03658           logSkip = emcmotStatus->logSkip;
03659           logIt = 1;
03660         }
03661         break;
03662 
03663       case EMCMOT_LOG_TYPE_POS_VOLTAGE:
03664         /* don't do a circular log-- suppress if full */
03665         if (emcmotLog->howmany >= emcmotStatus->logSize) {
03666           emcmotStatus->output[loggingAxis] = 0.0; /* drop the DAC to zero */
03667           emcmotStatus->logStarted = 0; /* stop logging */
03668           logIt = 0;            /* should still be zero, reset anyway */
03669           break;
03670         }
03671         if (logSkip-- <= 0) {
03672           ls.item.posVoltage.time = end - logStartTime;
03673           for (axis = 0;
03674                axis < EMCMOT_LOG_NUM_AXES &&
03675                  axis < EMCMOT_MAX_AXIS;
03676                axis++) {
03677             ls.item.posVoltage.pos = emcmotStatus->input[loggingAxis];
03678             ls.item.posVoltage.voltage = emcmotDebug->rawOutput[loggingAxis];
03679           }
03680           logSkip = emcmotStatus->logSkip;
03681           logIt = 1;
03682         }
03683         break;
03684 
03685       default:
03686         break;
03687       } /* end of: switch on log type */
03688 
03689       /* now log it */
03690       if (logIt) {
03691         emcmotLogAdd(emcmotLog, ls);
03692         emcmotStatus->logPoints = emcmotLog->howmany;
03693         logIt = 0;
03694       }
03695     } /* end of: if logging */
03696 
03697     emcmotDebug->running_time = etime() - emcmotDebug->start_time;
03698 
03699     /* set tail to head, which has already been incremented */
03700     emcmotStatus->tail = emcmotStatus->head;
03701     emcmotDebug->tail = emcmotDebug->head;
03702     emcmotConfig->tail = emcmotConfig->head;
03703     /* wait for next cycle */
03704 #ifdef rtai
03705     rt_task_wait_period();
03706     while(0 == emcmotStruct)
03707       {
03708         rt_task_wait_period();
03709       }
03710 #endif
03711 #ifdef rtlinux
03712 #ifdef USE_RTL2
03713     pthread_wait_np();
03714 #else
03715     rt_task_wait();
03716 #endif
03717 #endif 
03718 #if defined(rtai) || defined(rtlinux)
03719   } /* end of: forever loop for RT task */
03720 #endif
03721 
03722 #ifdef USE_RTL2
03723   return NULL;
03724 #endif
03725 
03726 } /* end of: emcmotController() function */

05141 {
05142   emcmot_done = 1;
05143 }

01068 {
01069 #if defined(rtlinux) || defined(rtai)
01070   outb(byte, MOT_DOUT_PORT);
01071 #endif
01072 }

05151 {
05152   int t;
05153   int len;
05154 
05155   for (t = 1; t < argc; t++) {
05156     /* try -ini */
05157     /* NOTE: motion controller cannot normally read an INI file.
05158        This is provided for main() simulation, which can read
05159        an INI file and needs it for the plant simulation parameters */
05160     if (!strcmp(argv[t], "-ini")) {
05161       if (t == argc - 1) {
05162         diagnostics("missing -ini parameter\n");
05163         return -1;
05164       }
05165       else {
05166         strcpy(EMCMOT_INIFILE, argv[t+1]);
05167         t++;            /* step over following arg */
05168         continue;
05169       }
05170     }
05171 
05172     /* try SHMEM_BASE_ADDRESS */
05173     len = strlen("SHMEM_BASE_ADDRESS");
05174     if (! strncmp(argv[t], "SHMEM_BASE_ADDRESS", len)) {
05175       if (argv[t][len] != '=' ||
05176           1 != sscanf(&argv[t][len + 1], "%li", &SHMEM_BASE_ADDRESS)) {
05177         diagnostics("syntax: %s SHMEM_BASE_ADDRESS=integer\n", argv[0]);
05178         return -1;
05179       }
05180       else {
05181         continue;
05182       }
05183     }
05184 
05185     /* try SHMEM_KEY */
05186     len = strlen("SHMEM_KEY");
05187     if (! strncmp(argv[t], "SHMEM_KEY", len)) {
05188       if (argv[t][len] != '=' ||
05189           1 != sscanf(&argv[t][len + 1], "%i", &SHMEM_KEY)) {
05190         diagnostics("syntax: %s SHMEM_KEY=integer\n", argv[0]);
05191         return -1;
05192       }
05193       else {
05194         continue;
05195       }
05196     }
05197 
05198     /* FIXME */
05199     /* try -noforward */
05200     if (!strcmp(argv[t], "-noforward")) {
05201       EMCMOT_NO_FORWARD_KINEMATICS = 1;
05202       kinType = KINEMATICS_INVERSE_ONLY;
05203       continue;
05204     }
05205 
05206     /* no match-- bad arg */
05207     diagnostics("bad argument to %s: %s\n", argv[0], argv[t]);
05208     return -1;
05209   }
05210 
05211   return 0;
05212 }

00900 {
00901   double joint[EMCMOT_MAX_AXIS];
00902   int axis;
00903 
00904   /* fill in all joints with 0 */
00905   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
00906     joint[axis] = 0.0;
00907   }
00908 
00909   /* now fill in with real values, for joints that are used */
00910   kinematicsInverse(&pos, joint, &iflags, &fflags);
00911 
00912   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
00913     if (! GET_AXIS_ACTIVE_FLAG(axis)) {
00914       /* if axis is not active, don't even look at its limits */
00915       continue;
00916     }
00917 
00918     if (joint[axis] > emcmotConfig->maxLimit[axis] ||
00919         joint[axis] < emcmotConfig->minLimit[axis]) {
00920       return 0;         /* can't move further past limit */
00921     }
00922   }
00923 
00924   /* okay to move */
00925   return 1;
00926 }

04332 {
04333   int axis;
04334   int t;
04335 #ifdef USE_RTL2
04336   hrtime_t now;
04337   pthread_attr_t attr;
04338   struct sched_param sched_param;
04339   struct timespec resolution;
04340   int resperiod;
04341   hrtime_t hr_timer_period;
04342 #endif
04343   PID_STRUCT pid;
04344 #ifdef rtai
04345   int error_code;
04346 #endif
04347 
04348   /* lpg - echo module params so we know they were accepted */  
04349 #ifdef STEPPER_MOTORS
04350   char *pstype;
04351   switch(STEPPING_TYPE){
04352     case 0: pstype = "Step/Dir"; break;
04353     case 1: pstype = "Phase"; break;
04354     case 2: pstype = "Quadrature/other"; break;
04355     default: pstype = "Invalid";
04356   }
04357   diagnostics("emcmot: STEPPING_TYPE = %s\n",pstype);
04358   diagnostics("emcmot: PARPORT_IO_ADDRESS = 0x%x\n",PARPORT_IO_ADDRESS);
04359 #endif
04360   diagnostics("emcmot: SHMEM_BASE_ADDRESS = %ld\n",SHMEM_BASE_ADDRESS);
04361   diagnostics("emcmot: SHMEM_KEY = %d\n",SHMEM_KEY);
04362   diagnostics("emcmot: PERIOD = %dns\n",PERIOD);
04363   diagnostics("emcmot: EMCMOT_TASK_PRIORITY = %d\n",EMCMOT_TASK_PRIORITY);
04364   diagnostics("emcmot: EMCMOT_TASK_STACK_SIZE = %d\n",EMCMOT_TASK_STACK_SIZE);
04365 #ifdef STEPPER_MOTORS
04366   diagnostics("emcmot: FREQ_TASK_PRIORITY = %d\n",FREQ_TASK_PRIORITY);
04367   diagnostics("emcmot: FREQ_TASK_STACK_SIZE = %d\n",FREQ_TASK_STACK_SIZE);
04368 #endif
04369 #ifdef rtai
04370   diagnostics("emcmot: sizeof(RT_TASK) = %d\n", sizeof(RT_TASK));
04371 #endif
04372 
04373 /* lpg - the next 2 params only exist for non stepper systems (servo to go)*/
04374 #if 0
04375   diagnostics("emcmot: STG_BASE_ADDRESS = 0x%hx\n",STG_BASE_ADDRESS);
04376   diagnostics("emcmot: FIND_STG_BASE_ADDRESS = %d\n",FIND_STG_BASE_ADDRESS);
04377 #endif
04378 
04379 
04380    emcmotTask_created=0;
04381    freqTask_created=0;
04382    emcmotStruct=0;
04383    emcmotDebug=0;
04384    emcmotStatus=0;
04385    emcmotCommand=0;
04386    emcmotConfig=0;
04387 
04388   /* record the kinematics type of the machine */
04389   kinType = kinematicsType();
04390   if (EMCMOT_NO_FORWARD_KINEMATICS) {
04391     kinType = KINEMATICS_INVERSE_ONLY;
04392   }
04393 
04394 #ifdef rtai
04395   rt_mount_rtai();
04396   rt_linux_use_fpu(1);
04397   emcmotStruct = rtai_kmalloc(SHMEM_KEY, sizeof(EMCMOT_STRUCT));
04398   if(0 == emcmotStruct)
04399     {
04400       diagnostics("emcmot: can't allocate shared memory with rtai_kmalloc(%d,%d)\n.", SHMEM_KEY,sizeof(EMCMOT_STRUCT));
04401       return -1;
04402     }
04403   diagnostics("emcmot: emcmotStruct allocated at 0x%X from rtai_kmalloc(%d,%d)\n", ((unsigned int) emcmotStruct), SHMEM_KEY, sizeof(EMCMOT_STRUCT));
04404   
04405   rt_set_periodic_mode();
04406   /* PERIOD is in microseconds, convert to nanoseconds then to rtai counts 
04407      and set the timer..*/
04408   rtaiTickPeriod = start_rt_timer(nano2count(PERIOD));
04409   if (rtaiTickPeriod < 1 )
04410     {
04411       diagnostics("emcmot: problem with start_rt_timer(%d)\n",
04412                   nano2count(PERIOD));
04413       return(-1);
04414     }
04415   rtaiTickPeriod_long = (long) rtaiTickPeriod;
04416   diagnostics("emcmot: rtaiTickPeriod=%d\n",rtaiTickPeriod_long);
04417 #else
04418 #ifdef rtlinux
04419   
04420   /* use pure periodic timing */
04421 #ifdef USE_RTL2
04422   local_clock = rtl_getschedclock();
04423 
04424   /* PERIOD units were established for RTL 09J which was approximately
04425      microseconds the same as the timer resolution,
04426      but in RTL 2.0 most of the funtions take nanosecond periods and we
04427      should dynamically discover the timer resolution.
04428      THE PERIOD units were changed to nanoseconds on  7-Mar-2000, so
04429      RTL2 no longer needs to convert but RTL1 does now.
04430   */
04431   clock_getres (local_clock, &resolution);
04432   resperiod = timespec_to_ns(&resolution); /* resperiod probably equals 838 */
04433 
04434   /* make period and even timer interval */
04435   PERIOD /= resperiod;
04436   PERIOD *= resperiod;
04437   if (PERIOD < resperiod) {
04438     PERIOD = resperiod;
04439   }
04440   hr_timer_period = (hrtime_t) PERIOD;
04441 
04442   if (rtl_setclockmode(local_clock, RTL_CLOCK_MODE_PERIODIC,
04443                        hr_timer_period)) {
04444     diagnostics("can't set periodic mode\n");
04445     if (rtl_setclockmode(local_clock, RTL_CLOCK_MODE_ONESHOT,0)) {
04446       diagnostics("can't set oneshot mode\n"); ;
04447     }
04448   }
04449 
04450 #else  /* RTL1 */
04451 
04452   /*
04453     THE PERIOD units were changed from timer ticks to nanoseconds on
04454     7-Mar-2000, so RTL2 no longer needs to convert but RTL1 does now
04455   */
04456   PERIOD = (int) (((double) PERIOD) / 1.0e9 * ((double) RT_TICKS_PER_SEC));
04457   rtstart = rtl_set_periodic_mode(PERIOD);
04458 
04459   if (rtstart == RT_TIME_END) {
04460     /* can't do it-- revert to oneshot mode */
04461     rtl_set_oneshot_mode();
04462     diagnostics("reverting to oneshot mode\n");
04463   }
04464   else {
04465     diagnostics("running in periodic mode\n");
04466   }
04467 #endif
04468 
04469 #ifdef USE_RTL2
04470 /* set shared memory pointer using mbuff which does not require that
04471    memory be reserved in lilo.conf */
04472   emcmotStruct = (EMCMOT_STRUCT *) mbuff_alloc("emcmotStruct", sizeof(EMCMOT_STRUCT));
04473 #else
04474 #if defined(rtlinux2)  /* set shared memory pointer using ioremap for
04475                           2.1+ kernels */
04476   emcmotStruct = (EMCMOT_STRUCT *) ioremap(SHMEM_BASE_ADDRESS, sizeof(EMCMOT_STRUCT));
04477 #else
04478   /* set shared memory pointer */
04479   emcmotStruct = (EMCMOT_STRUCT *) vremap(SHMEM_BASE_ADDRESS, sizeof(EMCMOT_STRUCT));
04480 #endif
04481 
04482 #endif
04483 
04484 #else  /* not rtlinux */
04485   /* get command shmem */
04486   shmem = rcs_shm_open(SHMEM_KEY, sizeof(EMCMOT_STRUCT), 1, 0666);
04487   if (NULL == shmem ||
04488       NULL == shmem->addr) {
04489     diagnostics("can't get shared memory\n");
04490     return -1;
04491   }
04492 
04493   memset(shmem->addr, 0, sizeof(EMCMOT_STRUCT));
04494   /* map shmem area into local address space */
04495   emcmotStruct = (EMCMOT_STRUCT *) shmem->addr;
04496 
04497 #endif /* else not rtlinux */
04498 
04499 #endif /* not rtai */
04500 
04501   /* we'll reference emcmotStruct directly */
04502   emcmotCommand = (EMCMOT_COMMAND *) &emcmotStruct->command;
04503   emcmotStatus = (EMCMOT_STATUS *) &emcmotStruct->status;
04504   emcmotConfig = (EMCMOT_CONFIG *) &emcmotStruct->config;
04505   emcmotDebug = (EMCMOT_DEBUG *) &emcmotStruct->debug;
04506   
04507   emcmotError = (EMCMOT_ERROR *) &emcmotStruct->error;
04508   emcmotLog = (EMCMOT_LOG *) &emcmotStruct->log;
04509   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04510     emcmotComp[axis] = (EMCMOT_COMP *) &emcmotStruct->comp[axis];
04511 
04512     emcmotDebug->bcomp[axis]=0;  /* backlash comp value */
04513     emcmotDebug->bcompdir[axis]=0; /* 0=none, 1=pos, -1=neg */
04514     emcmotDebug->bcompincr[axis]=0;  /* incremental backlash comp */
04515     emcmotDebug->bac_done[axis]=0; 
04516     emcmotDebug->bac_d[axis]=0;
04517     emcmotDebug->bac_di[axis]=0;
04518     emcmotDebug->bac_D[axis]=0;
04519     emcmotDebug->bac_halfD[axis]=0;
04520     emcmotDebug->bac_incrincr[axis]=0;
04521     emcmotDebug->bac_incr[axis]=0;
04522   }
04523 
04524   /* zero shared memory */
04525   for (t = 0; t < sizeof(EMCMOT_STRUCT); t++) {
04526     ((char *) emcmotStruct)[t] = 0;
04527   }
04528 
04529   /* init locals */
04530   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04531     emcmotDebug->maxLimitSwitchCount[axis] = 0;
04532     emcmotDebug->minLimitSwitchCount[axis] = 0;
04533     emcmotDebug->ampFaultCount[axis] = 0;
04534   }
04535 
04536   /* init compensation struct */
04537   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04538     emcmotComp[axis]->total = 0;
04539     emcmotComp[axis]->alter = 0.0;
04540     /* leave out the avgint, nominal, forward, and reverse, since
04541        these can't be zero and the total flag prevents their use anyway */
04542   }
04543 
04544   /* init error struct */
04545   emcmotErrorInit(emcmotError);
04546 
04547   /* init command struct */
04548   emcmotCommand->head = 0;
04549   emcmotCommand->command = 0;
04550   emcmotCommand->commandNum = 0;
04551   emcmotCommand->tail = 0;
04552 
04553   /* init status struct */
04554 
04555   emcmotStatus->head = 0;
04556   emcmotDebug->head = 0;
04557   emcmotConfig->head = 0;
04558 
04559   emcmotStatus->motionFlag = 0;
04560   SET_MOTION_ERROR_FLAG(0);
04561   SET_MOTION_COORD_FLAG(0);
04562   SET_MOTION_TELEOP_FLAG(0);
04563   emcmotDebug->split = 0;
04564   emcmotStatus->commandEcho = 0;
04565   emcmotStatus->commandNumEcho = 0;
04566   emcmotStatus->commandStatus = 0;
04567   emcmotStatus->heartbeat = 0;
04568   emcmotStatus->computeTime = 0.0;
04569   emcmotConfig->numAxes = EMCMOT_MAX_AXIS;
04570   emcmotConfig->trajCycleTime = TRAJ_CYCLE_TIME;
04571   emcmotConfig->servoCycleTime = SERVO_CYCLE_TIME;
04572   emcmotStatus->pos.tran.x = 0.0;
04573   emcmotStatus->pos.tran.y = 0.0;
04574   emcmotStatus->pos.tran.z = 0.0;
04575   emcmotStatus->actualPos.tran.x = 0.0;
04576   emcmotStatus->actualPos.tran.y = 0.0;
04577   emcmotStatus->actualPos.tran.z = 0.0;
04578   emcmotStatus->vel = VELOCITY;
04579   emcmotConfig->limitVel = VELOCITY;
04580   emcmotStatus->acc = ACCELERATION;
04581   emcmotStatus->qVscale = 1.0;
04582   emcmotStatus->id = 0;
04583   emcmotStatus->depth = 0;
04584   emcmotStatus->activeDepth = 0;
04585   emcmotStatus->paused = 0;
04586   emcmotStatus->overrideLimits = 0;
04587   SET_MOTION_INPOS_FLAG(1);
04588   emcmotStatus->logOpen = 0;
04589   emcmotStatus->logStarted = 0;
04590   emcmotStatus->logSize = 0;
04591   emcmotStatus->logSkip = 0;
04592   emcmotStatus->logPoints = 0;
04593   SET_MOTION_ENABLE_FLAG(0);
04594   emcmotConfig->kinematics_type = kinType;
04595 
04596   emcmotDebug->oldPos = emcmotStatus->pos;
04597   emcmotDebug->oldVel.tran.x = 0.0;
04598   emcmotDebug->oldVel.tran.y = 0.0;
04599   emcmotDebug->oldVel.tran.z = 0.0;
04600 
04601   MARK_EMCMOT_CONFIG_CHANGE();
04602 
04603   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04604     emcmotConfig->homingVel[axis] = VELOCITY;
04605     emcmotConfig->homeOffset[axis] = 0.0;
04606     emcmotStatus->axisFlag[axis] = 0;
04607     emcmotConfig->maxLimit[axis] = MAX_LIMIT;
04608     emcmotConfig->minLimit[axis] = MIN_LIMIT;
04609     emcmotConfig->maxOutput[axis] = MAX_OUTPUT;
04610     emcmotConfig->minOutput[axis] = MIN_OUTPUT;
04611     emcmotConfig->minFerror[axis] = 0.0; /* gives a true linear ferror */
04612     emcmotConfig->maxFerror[axis] = MAX_FERROR;
04613     emcmotDebug->ferrorCurrent[axis] = 0.0;
04614     emcmotDebug->ferrorHighMark[axis] = 0.0;
04615     emcmotStatus->outputScale[axis] = OUTPUT_SCALE;
04616     emcmotStatus->outputOffset[axis] = OUTPUT_OFFSET;
04617     emcmotStatus->inputScale[axis] = INPUT_SCALE;
04618     emcmotDebug->inverseInputScale[axis] = 1.0 / INPUT_SCALE;
04619     emcmotStatus->inputOffset[axis] = INPUT_OFFSET;
04620     emcmotDebug->inverseOutputScale[axis] = 1.0 / OUTPUT_SCALE;
04621     emcmotStatus->axVscale[axis] = 1.0;
04622     emcmotConfig->axisLimitVel[axis] = 1.0;
04623     emcmotDebug->bigVel[axis] = 1.0;
04624     SET_AXIS_ENABLE_FLAG(axis, 0);
04625     SET_AXIS_ACTIVE_FLAG(axis, 0); /* default is not to use it; need an
04626                                       explicit activate */
04627     SET_AXIS_NSL_FLAG(axis, 0);
04628     SET_AXIS_PSL_FLAG(axis, 0);
04629     SET_AXIS_NHL_FLAG(axis, 0);
04630     SET_AXIS_PHL_FLAG(axis, 0);
04631     SET_AXIS_INPOS_FLAG(axis, 1);
04632     SET_AXIS_HOMING_FLAG(axis, 0);
04633     SET_AXIS_HOMED_FLAG(axis, 0);
04634     SET_AXIS_FERROR_FLAG(axis, 0);
04635     SET_AXIS_FAULT_FLAG(axis, 0);
04636     SET_AXIS_ERROR_FLAG(axis, 0);
04637     emcmotConfig->axisPolarity[axis] = (EMCMOT_AXIS_FLAG) 0xFFFFFFFF;
04638     /* will read encoders directly, so don't set them here */
04639   }
04640 
04641   /* FIXME-- add emcmotError */
04642 
04643   /* init min-max-avg stats */
04644   mmxavgInit(&emcmotDebug->tMmxavg, emcmotDebug->tMmxavgSpace, MMXAVG_SIZE);
04645   mmxavgInit(&emcmotDebug->sMmxavg, emcmotDebug->sMmxavgSpace, MMXAVG_SIZE);
04646   mmxavgInit(&emcmotDebug->nMmxavg, emcmotDebug->nMmxavgSpace, MMXAVG_SIZE);
04647   mmxavgInit(&emcmotDebug->yMmxavg, emcmotDebug->yMmxavgSpace, MMXAVG_SIZE);
04648   mmxavgInit(&emcmotDebug->fMmxavg, emcmotDebug->fMmxavgSpace, MMXAVG_SIZE);
04649   mmxavgInit(&emcmotDebug->fyMmxavg, emcmotDebug->fyMmxavgSpace, MMXAVG_SIZE);
04650   emcmotDebug->tMin = 0.0;
04651   emcmotDebug->tMax = 0.0;
04652   emcmotDebug->tAvg = 0.0;
04653   emcmotDebug->sMin = 0.0;
04654   emcmotDebug->sMax = 0.0;
04655   emcmotDebug->sAvg = 0.0;
04656   emcmotDebug->nMin = 0.0;
04657   emcmotDebug->nMax = 0.0;
04658   emcmotDebug->nAvg = 0.0;
04659   emcmotDebug->yMin = 0.0;
04660   emcmotDebug->yMax = 0.0;
04661   emcmotDebug->yAvg = 0.0;
04662   emcmotDebug->fyMin = 0.0;
04663   emcmotDebug->fyMax = 0.0;
04664   emcmotDebug->fyAvg = 0.0;
04665   emcmotDebug->fMin = 0.0;
04666   emcmotDebug->fMax = 0.0;
04667   emcmotDebug->fAvg = 0.0;
04668 
04669   emcmotDebug->cur_time = emcmotDebug->last_time =   0.0;
04670   emcmotDebug->start_time = etime();
04671   emcmotDebug->running_time = 0.0;
04672 
04673   /* init motion emcmotDebug->queue */
04674   if (-1 == tpCreate(&emcmotDebug->queue, DEFAULT_TC_QUEUE_SIZE, emcmotDebug->queueTcSpace)) {
04675     diagnostics("can't create motion emcmotDebug->queue\n");
04676     return -1;
04677   }
04678   tpInit(&emcmotDebug->queue);
04679   tpSetCycleTime(&emcmotDebug->queue, emcmotConfig->trajCycleTime);
04680   tpSetPos(&emcmotDebug->queue, emcmotStatus->pos);
04681   tpSetVmax(&emcmotDebug->queue, emcmotStatus->vel);
04682   tpSetAmax(&emcmotDebug->queue, emcmotStatus->acc);
04683 
04684   /* init the axis components */
04685   pid.p = P_GAIN;
04686   pid.i = I_GAIN;
04687   pid.d = D_GAIN;
04688   pid.ff0 = FF0_GAIN;
04689   pid.ff1 = FF1_GAIN;
04690   pid.ff2 = FF2_GAIN;
04691   pid.backlash = BACKLASH;
04692   pid.bias = BIAS;
04693   pid.maxError = MAX_ERROR;
04694 
04695   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04696     if (-1 == tpCreate(&emcmotDebug->freeAxis[axis], FREE_AXIS_QUEUE_SIZE, emcmotDebug->freeAxisTcSpace[axis])) {
04697       diagnostics("can't create axis emcmotDebug->queue %d\n", axis);
04698       return -1;
04699     }
04700     tpInit(&emcmotDebug->freeAxis[axis]);
04701     tpSetCycleTime(&emcmotDebug->freeAxis[axis], emcmotConfig->trajCycleTime);
04702     /* emcmotDebug->freePose is inited to 0's in decl */
04703     tpSetPos(&emcmotDebug->freeAxis[axis], emcmotDebug->freePose);
04704     tpSetVmax(&emcmotDebug->freeAxis[axis], emcmotStatus->vel);
04705     tpSetAmax(&emcmotDebug->freeAxis[axis], emcmotStatus->acc);
04706     pidInit(&emcmotConfig->pid[axis]);
04707     pidSetGains(&emcmotConfig->pid[axis], pid);
04708     cubicInit(&emcmotDebug->cubic[axis]);
04709   }
04710 
04711   /* init the time and rate using functions to affect traj, the
04712      pids, and the cubics properly, since they're coupled */
04713   setTrajCycleTime(TRAJ_CYCLE_TIME);
04714   setServoCycleTime(SERVO_CYCLE_TIME);
04715 
04716   /* init board */
04717 
04718   if (-1 == extMotInit((const char *) EMCMOT_INIFILE)) {
04719     diagnostics("can't initialize motion hardware\n");
04720     return -1;
04721   }
04722 
04723   if (-1 == extDioInit((const char *) EMCMOT_INIFILE)) {
04724     diagnostics("can't initialize DIO hardware\n");
04725     return -1;
04726   }
04727 
04728   if (-1 == extAioInit((const char *) EMCMOT_INIFILE)) {
04729     diagnostics("can't initialize AIO hardware\n");
04730     return -1;
04731   }
04732 
04733   extEncoderSetIndexModel(EXT_ENCODER_INDEX_MODEL_MANUAL);
04734   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
04735     emcmotDebug->rawInput[axis] = 0.0;
04736     emcmotDebug->rawOutput[axis] = 0.0;
04737     emcmotDebug->coarseJointPos[axis] = 0.0;
04738     emcmotDebug->jointPos[axis] = 0.0;
04739     emcmotDebug->jointVel[axis] = 0.0;
04740     emcmotStatus->axisPos[axis] = 0.0;
04741     emcmotDebug->oldJointPos[axis] = 0.0;
04742     emcmotDebug->outJointPos[axis] = 0.0;
04743 
04744     emcmotDebug->homingPhase[axis] = 0;
04745     emcmotDebug->latchFlag[axis] = 0;
04746     emcmotDebug->saveLatch[axis] = 0.0;
04747 
04748     emcmotStatus->input[axis] = 0.0;
04749     emcmotDebug->oldInput[axis] = 0.0;
04750     emcmotDebug->oldInputValid[axis]=0;
04751     emcmotStatus->output[axis] = 0.0;
04752 
04753     emcmotDebug->jointHome[axis] = 0.0;
04754 
04755     extAmpEnable(axis,
04756                  ! GET_AXIS_ENABLE_POLARITY(axis));
04757   }
04758 
04759   emcmotStatus->tail = 0;
04760 
04761 #ifdef rtai
04762   diagnostics("emcmot: initializing emcmotTask\n");
04763   if( 0 != ( error_code = 
04764              rt_task_init(&emcmotTask, /* RT_TASK */
04765                           emcmotController, /* task function */
04766                           1, /* arg */
04767                           EMCMOT_TASK_STACK_SIZE, /* stack */
04768                           EMCMOT_TASK_PRIORITY, /* priority */
04769                           1,            /* uses fpu */
04770                           emcmotsig)) /* signal function */
04771       )
04772     {
04773       diagnostics("rt_task_init failed for emcmotTask error_code=%d\n",
04774                   error_code);
04775       return -1;
04776     }
04777   now = rt_get_time();
04778   diagnostics("emcmot: rtaiTickPeriod=%d\n",rtaiTickPeriod);
04779   if(rtaiTickPeriod > 0)
04780     {
04781       rtaiTickPeriod_long = (long) rtaiTickPeriod;
04782       diagnostics("servoCycleTime=%dns\n", 
04783                   (int) (1e9* emcmotConfig->servoCycleTime));
04784       iperiod = nano2count((int) (1e9* emcmotConfig->servoCycleTime));
04785       diagnostics("servoCycleTime=%d ticks\n", 
04786                   iperiod);
04787 
04788       if ( iperiod < rtaiTickPeriod_long) 
04789         {
04790           iperiod = rtaiTickPeriod_long;
04791         }
04792       iperiod = (iperiod + rtaiTickPeriod_long/2)/ rtaiTickPeriod_long;
04793       iperiod *= rtaiTickPeriod_long;
04794       iperiod_rtime = (RTIME) iperiod;
04795       now = rt_get_time();
04796       setServoCycleTime(count2nano(iperiod)*1e-9);
04797       diagnostics("emcmot: making emcmotTask periodic at a rate of %d ticks or %d nanoseconds \n", iperiod , count2nano(iperiod));
04798       if ( 0 != (error_code = 
04799                  rt_task_make_periodic(&emcmotTask,
04800                                        now+5*rtaiTickPeriod, 
04801                                        iperiod_rtime) 
04802                  ))
04803         {
04804           diagnostics("rt_task_make_periodic failed for emcmotTask error_code=%d\n",
04805                       error_code);
04806           return -1;
04807         }
04808       emcmotTask_created=1;
04809     }
04810   diagnostics("emcmot: rtaiTickPeriod=%d\n",rtaiTickPeriod);
04811               
04812 #ifdef STEPPER_MOTORS
04813   diagnostics("emcmot: initializing freqTask\n");
04814   switch (STEPPING_TYPE)     /* lpg */
04815   {
04816   case 1:       /* phase stepping */
04817     error_code = rt_task_init(&freqTask, /* RT_TASK */
04818                  phasefunc, /* task function */
04819                  1, /* task function arg */
04820                  FREQ_TASK_STACK_SIZE, /* stack */
04821                  FREQ_TASK_PRIORITY, /* priority */
04822                  1, /* use fpu */
04823                  freqsig); /* signal func */
04824     break;
04825   case 2:       /* lpg quadrature/other phase stepping */
04826     error_code = rt_task_init(&freqTask, /* RT_TASK */
04827                  lpg_phase_drive, /* task func */
04828                  1, /* arg */
04829                  FREQ_TASK_STACK_SIZE, /* stack */
04830                  FREQ_TASK_PRIORITY,
04831                  1, /* use fpu */
04832                  freqsig); /* signal func */
04833   default:      /* default is step/direction */
04834     error_code = rt_task_init(&freqTask, /* RT_TASK */
04835                  freqfunc, /* task func */
04836                  1, /* arg */
04837                  FREQ_TASK_STACK_SIZE, /* stack */
04838                  FREQ_TASK_PRIORITY,
04839                  1, /* use fpu */
04840                  freqsig); /* signal func */
04841     break;
04842   }
04843   diagnostics("emcmot: rtaiTickPeriod=%d\n",rtaiTickPeriod);
04844   if(error_code != 0)
04845     {
04846       diagnostics("rt_task_init failed for freqTask error_code = %d\n",
04847                   error_code);
04848       return -1;
04849     }
04850   if( rtaiTickPeriod > 0 )
04851     {
04852       now = rt_get_time();
04853       diagnostics("emcmot: making freqTask periodic at a rate of %d ticks or %d nanoseconds \n", ((int) rtaiTickPeriod_long) , ((int) count2nano(rtaiTickPeriod)));
04854       error_code = rt_task_make_periodic(&freqTask,
04855                                          now+5*rtaiTickPeriod, 
04856                                          rtaiTickPeriod);
04857       if(error_code != 0)
04858         {
04859           diagnostics("rt_task_make_periodic failed for freqTask error_code = %d\n",
04860                       error_code);
04861           return -1;
04862         }
04863       freqTask_created=1;
04864     }
04865 
04866 #endif
04867 
04868 #else 
04869 
04870 #ifdef rtlinux
04871 
04872   /* init control task */
04873 
04874 #ifdef USE_RTL2
04875   pthread_attr_init (&attr);
04876   sched_param.sched_priority = EMCMOT_TASK_PRIORITY;
04877   pthread_attr_setschedparam (&attr, &sched_param);
04878   attr.use_fp = 1;
04879   attr.stack_size = EMCMOT_TASK_STACK_SIZE;
04880   pthread_create( &emcmotTask, &attr, emcmotController, (void *) 1);
04881 
04882   /* get current time as base for starting tasks */
04883   now =gethrtime();
04884 
04885   pthread_setfp_np(emcmotTask,1);
04886 #else
04887   rt_task_init(&emcmotTask,     /* RT_TASK * */
04888                emcmotController, /* task code */
04889                0,               /* startup arg to task code */
04890                EMCMOT_TASK_STACK_SIZE, /* task stack size */
04891                EMCMOT_TASK_PRIORITY); /* priority */
04892 
04893   /* make sure we save FP context */
04894 #if defined(rtlinux_09J) || defined(rtlinux_1_0) || defined(rtlinux2)
04895   rt_task_use_fp(&emcmotTask, 1);
04896 #else
04897   rt_use_fp(1);
04898 #endif
04899 #endif /* ! USE_RTL2 */
04900 
04901   /* schedule control task to run */
04902 #ifdef USE_RTL2
04903   iperiod = (int) (HRTICKS_PER_SEC * emcmotConfig->servoCycleTime);
04904 #else
04905   iperiod = (int) (RT_TICKS_PER_SEC * emcmotConfig->servoCycleTime);
04906 #endif
04907 
04908   iperiod = iperiod / PERIOD;
04909   iperiod = iperiod * PERIOD;
04910 
04911 #ifdef USE_RTL2
04912   rtperiod = (hrtime_t) iperiod;
04913 #else
04914   rtperiod = (RTIME) iperiod;
04915 #endif
04916 #ifdef USE_RTL2
04917   pthread_make_periodic_np(emcmotTask,
04918                            gethrtime() + rtperiod,
04919                            rtperiod);
04920 #else
04921   rt_task_make_periodic(&emcmotTask,
04922                         rtstart + rtperiod,
04923                         rtperiod);
04924 #endif
04925 
04926 #endif
04927 
04928 #ifdef STEPPER_MOTORS
04929 
04930   /* init stepper motor task */
04931 #ifdef USE_RTL2
04932   pthread_attr_init (&attr);
04933   sched_param.sched_priority = FREQ_TASK_PRIORITY;
04934   pthread_attr_setschedparam (&attr, &sched_param);
04935   attr.use_fp = 1;
04936   attr.stack_size = FREQ_TASK_STACK_SIZE;
04937   switch (STEPPING_TYPE)     /* lpg */
04938   {
04939   case 1:       /* phase stepping */
04940     pthread_create(&freqTask, &attr, phasefunc, (void *) 1); break;
04941   case 2:       /* lpg quadrature/other phase stepping */
04942     pthread_create(&freqTask, &attr, lpg_phase_drive, (void *) 1); break;    
04943   default:      /* default is step/direction */
04944     pthread_create(&freqTask, &attr, freqfunc, (void *) 1);
04945   }
04946 
04947   /* get current time as base for starting tasks */
04948   pthread_setfp_np(freqTask, 1);
04949   pthread_make_periodic_np(freqTask,
04950                            gethrtime() + PERIOD,
04951                            PERIOD);
04952 
04953 #else
04954   switch (STEPPING_TYPE)
04955   {
04956   case 1:       /* phase stepping */
04957     rt_task_init(&freqTask,     /* RT_TASK * */
04958                  phasefunc, /* task code */
04959                  0,             /* startup arg to task code */
04960                  FREQ_TASK_STACK_SIZE, /* task stack size */
04961                  FREQ_TASK_PRIORITY); /* priority */
04962     break;
04963 
04964   case 2:       /* lpg quadrature/other phase stepping */
04965     rt_task_init(&freqTask,     /* RT_TASK * */
04966                  lpg_phase_drive, /* task code */
04967                  0,             /* startup arg to task code */
04968                  FREQ_TASK_STACK_SIZE, /* task stack size */
04969                  FREQ_TASK_PRIORITY); /* priority */
04970     break;
04971     
04972   default:      /* default is step/direction */
04973     rt_task_init(&freqTask,     /* RT_TASK * */
04974                  freqfunc, /* task code */
04975                  0,             /* startup arg to task code */
04976                  FREQ_TASK_STACK_SIZE, /* task stack size */
04977                  FREQ_TASK_PRIORITY); /* priority */
04978   }
04979 
04980   /* make sure we save FP context */
04981   rt_task_use_fp(&freqTask, 1);
04982 
04983   rt_task_make_periodic(&freqTask,
04984                         rt_get_time() + PERIOD,
04985                         PERIOD);
04986 
04987 #endif /* ! USE_RTL2 */
04988 #endif /* STEPPER_MOTORS  */
04989 #endif /* rtlinux */
04990 
04991   diagnostics("emcmot: init_module finished\n");
04992 
04993   return 0;
04994 }

00855 {
00856   int axis;
00857 
00858   for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) {
00859     if (GET_AXIS_HOMING_FLAG(axis)) {
00860       return 1;
00861     }
00862   }
00863 
00864   /* got here, so none are homing */
00865   return 0;
00866 }

05225 {
05226   double delta;                 /* elapsed time */
05227   int cycles=0;
05228   double start_time,cur_time;
05229 #ifndef UNDER_CE
05230   /* trap ^C */
05231   signal(SIGINT, emcmot_quit);
05232 
05233   /* process command line args */
05234   if (0 != getArgs(argc, argv)) {
05235     diagnostics("can't init module-- bad arguments\n");
05236     exit(1);
05237   }
05238 #endif
05239 
05240   /* set up data structs */
05241   if (-1 == init_module()) {
05242     diagnostics("can't init module-- execution permission problems?\n");
05243 #ifndef UNDER_CE
05244     exit(1);
05245 #else
05246     return 1;
05247 #endif
05248   }
05249   
05250   start_time = etime();
05251   while (! emcmot_done) {
05252     cycles++;
05253     delta = etime();
05254     emcmotController(0);
05255     cur_time = etime();
05256     delta = cur_time - delta;
05257     delta = emcmotConfig->servoCycleTime - delta;
05258 
05259     if (delta > 0.0) {
05260       esleep(delta);
05261     }
05262   }
05263 
05264   cleanup_module();
05265 #ifndef UNDER_CE
05266   exit(0);
05267 #else
05268   return 0;
05269 #endif
05270 }

00838 {
00839   va_list args;
00840   char error[EMCMOT_ERROR_LEN];
00841 
00842   va_start(args, fmt);
00843 #ifndef UNDER_CE
00844   vsprintf(error, fmt, args);
00845 #else
00846   RCS_CE_VSPRINTF(error,fmt,args);
00847 #endif
00848   va_end(args);
00849 
00850   emcmotErrorPut(emcmotError, error);
00851 }

01039 {
01040   static int t;
01041 
01042   /* make sure it's not zero */
01043   if (secs <= 0.0) {
01044     return;
01045   }
01046 
01047   MARK_EMCMOT_CONFIG_CHANGE();
01048 
01049   /* compute the interpolation rate as nearest integer to traj/servo*/
01050   emcmotConfig->interpolationRate =
01051     (int) (emcmotConfig->trajCycleTime / secs + 0.5);
01052 
01053   /* set the cubic interpolation rate and PID cycle time */
01054   for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
01055     cubicSetInterpolationRate(&emcmotDebug->cubic[t], emcmotConfig->interpolationRate);
01056     cubicSetSegmentTime(&emcmotDebug->cubic[t], secs);
01057     pidSetCycleTime(&emcmotConfig->pid[t], secs);
01058   }
01059 
01060   /* copy into status out */
01061   emcmotConfig->servoCycleTime = secs;
01062 }

01010 {
01011   static int t;
01012 
01013   /* make sure it's not zero */
01014   if (secs <= 0.0) {
01015     return;
01016   }
01017   
01018   MARK_EMCMOT_CONFIG_CHANGE();
01019 
01020   /* compute the interpolation rate as nearest integer to traj/servo*/
01021   emcmotConfig->interpolationRate =
01022     (int) (secs / emcmotConfig->servoCycleTime + 0.5);
01023 
01024   /* set traj planner */
01025   tpSetCycleTime(&emcmotDebug->queue, secs);
01026 
01027   /* set the free planners, cubic interpolation rate and segment time */
01028   for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
01029     tpSetCycleTime(&emcmotDebug->freeAxis[t], secs);
01030     cubicSetInterpolationRate(&emcmotDebug->cubic[t], emcmotConfig->interpolationRate);
01031   }
01032 
01033   /* copy into status out */
01034   emcmotConfig->trajCycleTime = secs;
01035 }

emcmot.c File Reference

Defines

Functions

Variables

Define Documentation

Function Documentation

Variable Documentation

Defines
#define	NO_ROUNDING
#define	COMPING
#define	DEFAULT_EMCMOT_TASK_PRIORITY 2
#define	DEFAULT_EMCMOT_TASK_STACK_SIZE 8192
#define	SOUND_PORT 0x61
#define	SOUND_MASK 0x02
#define	LIMIT_SWITCH_DEBOUNCE 10
#define	AMP_FAULT_DEBOUNCE 100
#define	POSITION_INPUT_DEBOUNCE 10
#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_TELEOP_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_TELEOP_BIT ? 1 : 0)
#define	SET_MOTION_TELEOP_FLAG(fl) if (fl) emcmotStatus->motionFlag \|= EMCMOT_MOTION_TELEOP_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_TELEOP_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) MARK_EMCMOT_CONFIG_CHANGE(); 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	AXRANGE(axis) ((emcmotConfig->maxLimit[axis] - emcmotConfig->minLimit[axis]))
#define	MOT_DOUT_PORT 0x278
#define	MOT_DOUT_ABORT_VAL 0
Functions
char	__attribute__ ((unused)) ident[]="$Id
void	MARK_EMCMOT_CONFIG_CHANGE (void)
void	reportError (const char *fmt,...)
int	isHoming (void)
void	clearHomes (int axis)
int	inRange (EmcPose pos)
int	checkLimits (void)
int	checkJog (int axis, double vel)
void	setTrajCycleTime (double secs)
void	setServoCycleTime (double secs)
void	extMotDout (unsigned char byte)
int	emcmotCommandHandler (void)
double	axisComp (int axis, int dir, double nominput)
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
KINEMATICS_FORWARD_FLAGS	fflags = 0
KINEMATICS_INVERSE_FLAGS	iflags = 0
int	kinType = 0
int	rehomeAll = 0
double	positionInputDebounce [EMCMOT_MAX_AXIS] = {0.0}
int	EMCMOT_NO_FORWARD_KINEMATICS = 0
int	interpolationCounter = 0
int	debouncecount [EMCMOT_MAX_AXIS] = {0}
int	STEPPING_TYPE
int	emcmot_done = 0