#include "_timer.h"
#include "_shm.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stdarg.h>
#include <string.h>
#include "emcpos.h"
#include "emcmotcfg.h"
#include "emcmotglb.h"
#include "emcmot.h"
#include "pid.h"
#include "cubic.h"
#include "tc.h"
#include "tp.h"
#include "segmentqueue.h"
#include "extintf.h"
#include "mmxavg.h"
#include "emcmotlog.h"
#include "parport.h"
#include <signal.h>
Include dependency graph for emcsegmot.c:
Go to the source code of this file.
Defines | |
#define | LIMIT_SWITCH_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_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) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_ENABLE_BIT ? 1 : 0) |
#define | SET_AXIS_ENABLE_POLARITY(ax, fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_ENABLE_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_ENABLE_BIT; |
#define | GET_AXIS_PHL_POLARITY(ax) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_MAX_HARD_LIMIT_BIT ? 1 : 0) |
#define | SET_AXIS_PHL_POLARITY(ax, fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_MAX_HARD_LIMIT_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_MAX_HARD_LIMIT_BIT; |
#define | GET_AXIS_NHL_POLARITY(ax) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_MIN_HARD_LIMIT_BIT ? 1 : 0) |
#define | SET_AXIS_NHL_POLARITY(ax, fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_MIN_HARD_LIMIT_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_MIN_HARD_LIMIT_BIT; |
#define | GET_AXIS_HOME_SWITCH_POLARITY(ax) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_HOME_SWITCH_BIT ? 1 : 0) |
#define | SET_AXIS_HOME_SWITCH_POLARITY(ax, fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_HOME_SWITCH_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_HOME_SWITCH_BIT; |
#define | GET_AXIS_HOMING_POLARITY(ax) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_HOMING_BIT ? 1 : 0) |
#define | SET_AXIS_HOMING_POLARITY(ax, fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_HOMING_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_HOMING_BIT; |
#define | GET_AXIS_FAULT_POLARITY(ax) (emcmotStatus->axisPolarity[ax] & EMCMOT_AXIS_FAULT_BIT ? 1 : 0) |
#define | SET_AXIS_FAULT_POLARITY(ax, fl) if (fl) emcmotStatus->axisPolarity[ax] |= EMCMOT_AXIS_FAULT_BIT; else emcmotStatus->axisPolarity[ax] &= ~EMCMOT_AXIS_FAULT_BIT; |
#define | FREE_AXIS_QUEUE_SIZE 4 |
#define | XRANGE ((emcmotStatus->maxLimit[0] - emcmotStatus->minLimit[0])) |
#define | YRANGE ((emcmotStatus->maxLimit[1] - emcmotStatus->minLimit[1])) |
#define | ZRANGE ((emcmotStatus->maxLimit[2] - emcmotStatus->minLimit[2])) |
Functions | |
void | reportError (const char *fmt,...) |
int | inRange (EmcPose pos) |
int | checkLimits () |
int | checkJog (int axis, double vel) |
void | setTrajCycleTime (double secs) |
void | setServoCycleTime (double secs) |
int | emcmotCommandHandler () |
void | emcmotController (int arg) |
int | init_module (void) |
void | cleanup_module (void) |
void | quit (int sig) |
int | getArgs (int argc, char *argv[]) |
int | main (int argc, char *argv[]) |
Variables | |
char | ident [] = "$Id: emcsegmot.c,v 1.3 2000/12/21 16:22:11 wshackle Exp $" |
EMCMOT_STRUCT * | emcmotStruct |
shm_t * | shmem = NULL |
EMCMOT_COMMAND * | emcmotCommand |
EMCMOT_STATUS * | emcmotStatus |
EMCMOT_ERROR * | emcmotError |
EMCMOT_LOG * | emcmotLog |
EMCMOT_LOG_STRUCT | ls |
int | logSkip = 0 |
int | loggingAxis = 0 |
int | logIt = 0 |
int | wdEnable = 0 |
int | wdWait = 0 |
int | wdCount = 0 |
int | maxLimitSwitchCount [EMCMOT_MAX_AXIS] |
int | minLimitSwitchCount [EMCMOT_MAX_AXIS] |
SEGMENTQUEUE | queue |
SEGMENT * | sqMemPool |
TP_STRUCT | freeAxis [EMCMOT_MAX_AXIS] |
CUBIC_STRUCT | cubic [EMCMOT_MAX_AXIS] |
TC_STRUCT | queueTcSpace [DEFAULT_TC_QUEUE_SIZE+10] |
TC_STRUCT | freeAxisTcSpace [EMCMOT_MAX_AXIS][FREE_AXIS_QUEUE_SIZE] |
double | rawInput [EMCMOT_MAX_AXIS] |
double | rawOutput [EMCMOT_MAX_AXIS] |
double | trajPos [EMCMOT_MAX_AXIS] |
double | jointPos [EMCMOT_MAX_AXIS] |
double | oldJointPos [EMCMOT_MAX_AXIS] |
double | oldInput [EMCMOT_MAX_AXIS] |
EmcPose | oldPos |
EmcPose | oldVel |
EmcPose | newVel |
EmcPose | newAcc |
double | lastInput [EMCMOT_MAX_AXIS] |
double | bigVel = 1.0 |
int | waitingForLatch [EMCMOT_MAX_AXIS] |
int | latchFlag [EMCMOT_MAX_AXIS] |
double | saveLatch [EMCMOT_MAX_AXIS] |
int | enabling = 0 |
int | coordinating = 0 |
int | wasOnLimit [EMCMOT_MAX_AXIS] |
int | stepping = 0 |
int | idForStep = 0 |
double | smStepsPerUnit [EMCMOT_MAX_AXIS] |
int | smStepsAccum [EMCMOT_MAX_AXIS] |
unsigned char | smClkPhase [EMCMOT_MAX_AXIS] |
MMXAVG_STRUCT | tMmxavg |
MMXAVG_STRUCT | sMmxavg |
MMXAVG_STRUCT | nMmxavg |
int | done = 0 |
|
Definition at line 437 of file emcsegmot.c. |
|
Definition at line 353 of file emcsegmot.c. |
|
Definition at line 349 of file emcsegmot.c. |
|
Definition at line 403 of file emcsegmot.c. |
|
Definition at line 361 of file emcsegmot.c. |
|
Definition at line 397 of file emcsegmot.c. |
|
Definition at line 423 of file emcsegmot.c. |
|
Definition at line 393 of file emcsegmot.c. |
|
Definition at line 389 of file emcsegmot.c. |
|
Definition at line 381 of file emcsegmot.c. |
|
Definition at line 415 of file emcsegmot.c. |
|
Definition at line 385 of file emcsegmot.c. |
|
Definition at line 419 of file emcsegmot.c. |
|
Definition at line 357 of file emcsegmot.c. |
|
Definition at line 377 of file emcsegmot.c. |
|
Definition at line 411 of file emcsegmot.c. |
|
Definition at line 369 of file emcsegmot.c. |
|
Definition at line 373 of file emcsegmot.c. |
|
Definition at line 407 of file emcsegmot.c. |
|
Definition at line 365 of file emcsegmot.c. |
|
Definition at line 335 of file emcsegmot.c. |
|
Definition at line 343 of file emcsegmot.c. |
|
Definition at line 331 of file emcsegmot.c. |
|
Definition at line 339 of file emcsegmot.c. |
|
Definition at line 316 of file emcsegmot.c. |
|
Definition at line 355 of file emcsegmot.c. |
|
Definition at line 351 of file emcsegmot.c. |
|
Definition at line 405 of file emcsegmot.c. |
|
Definition at line 363 of file emcsegmot.c. |
|
Definition at line 399 of file emcsegmot.c. |
|
Definition at line 425 of file emcsegmot.c. |
|
Definition at line 395 of file emcsegmot.c. |
|
Definition at line 391 of file emcsegmot.c. |
|
Definition at line 383 of file emcsegmot.c. |
|
Definition at line 417 of file emcsegmot.c. |
|
Definition at line 387 of file emcsegmot.c. |
|
Definition at line 421 of file emcsegmot.c. |
|
Definition at line 359 of file emcsegmot.c. |
|
Definition at line 379 of file emcsegmot.c. |
|
Definition at line 413 of file emcsegmot.c. |
|
Definition at line 371 of file emcsegmot.c. |
|
Definition at line 375 of file emcsegmot.c. |
|
Definition at line 409 of file emcsegmot.c. |
|
Definition at line 367 of file emcsegmot.c. |
|
Definition at line 337 of file emcsegmot.c. |
|
Definition at line 345 of file emcsegmot.c. |
|
Definition at line 333 of file emcsegmot.c. |
|
Definition at line 341 of file emcsegmot.c. |
|
Definition at line 517 of file emcsegmot.c. |
|
Definition at line 518 of file emcsegmot.c. |
|
Definition at line 519 of file emcsegmot.c. |
|
Definition at line 324 of file emcsegmot.c. |
|
Definition at line 586 of file emcsegmot.c. 00587 { 00588 if (axis < 0 || 00589 axis >= EMCMOT_MAX_AXIS) 00590 { 00591 return 0; /* can't jog out-of-range axis */ 00592 } 00593 00594 if (vel > 0.0 && 00595 (GET_AXIS_PSL_FLAG(axis) || 00596 GET_AXIS_PHL_FLAG(axis))) 00597 { 00598 return 0; /* can't jog further past max limit */ 00599 } 00600 00601 if (vel < 0.0 && 00602 (GET_AXIS_NSL_FLAG(axis) || 00603 GET_AXIS_NHL_FLAG(axis))) 00604 { 00605 return 0; /* can't jog further past min limit */ 00606 } 00607 00608 /* okay to jog */ 00609 return 1; 00610 } |
|
Definition at line 558 of file emcsegmot.c. 00559 { 00560 int axis; 00561 00562 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 00563 { 00564 if (! GET_AXIS_ACTIVE_FLAG(axis)) 00565 { 00566 /* if axis is not active, don't even look at its limits */ 00567 continue; 00568 } 00569 00570 if (GET_AXIS_PSL_FLAG(axis) || 00571 GET_AXIS_NSL_FLAG(axis) || 00572 GET_AXIS_PHL_FLAG(axis) || 00573 GET_AXIS_NHL_FLAG(axis)) 00574 { 00575 return 0; 00576 } 00577 } 00578 00579 return 1; 00580 } |
|
Definition at line 2904 of file emcsegmot.c. 02905 { 02906 int axis; 02907 02908 /* release traj planner space */ 02909 #if !defined(rtlinux) 02910 free(sqMemPool); 02911 #endif 02912 sqTrashQueue(&queue); 02913 02914 /* release axis planner spaces */ 02915 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02916 { 02917 tpDelete(&freeAxis[axis]); 02918 } 02919 02920 /* disable amps */ 02921 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02922 { 02923 extAmpEnable(axis, 02924 ! GET_AXIS_ENABLE_POLARITY(axis)); 02925 } 02926 02927 /* quit board */ 02928 extAioQuit(); 02929 extDioQuit(); 02930 extMotQuit(); 02931 02932 #ifdef rtlinux 02933 02934 #ifdef RT_FIFO 02935 rtf_destroy(EMCMOT_COMMAND_RTF); 02936 rtf_destroy(EMCMOT_STATUS_RTF); 02937 rtf_destroy(EMCMOT_ERROR_RTF); 02938 #endif 02939 02940 /* delete control task */ 02941 rt_task_delete(&emcmotTask); 02942 02943 /* FIXME */ 02944 diagnostics("exited emcmot\n"); 02945 02946 #ifdef STEPPER_MOTORS 02947 02948 /* delete stepper motor task */ 02949 rt_task_delete(&smTask); 02950 02951 #endif 02952 02953 #else 02954 02955 /* detach from shmem */ 02956 if (NULL != shmem) 02957 { 02958 rcs_shm_close(shmem); 02959 shmem = NULL; 02960 } 02961 02962 #endif 02963 } |
|
Definition at line 674 of file emcsegmot.c. 00675 { 00676 int axis; /* loop counter */ 00677 EmcPose jogPos; /* where to jog */ 00678 double saveVmax; /* copy of vMax for restoring */ 00679 #ifdef rtlinux 00680 #ifdef RT_FIFO 00681 int err; 00682 #endif 00683 #endif 00684 int valid; 00685 00686 /* copy command from outside into local buffers */ 00687 00688 #if defined (rtlinux) && defined (RT_FIFO) 00689 while ((err = rtf_get(EMCMOT_COMMAND_RTF, &emcmotCommandBuffer, sizeof(EMCMOT_COMMAND))) == sizeof(EMCMOT_COMMAND)) 00690 { 00691 00692 #endif /* rtlinux and RT_FIFO */ 00693 00694 /* check for split read */ 00695 if (emcmotCommand->head != emcmotCommand->tail) 00696 { 00697 emcmotStatus->split++; 00698 return 0; /* not really an error */ 00699 } 00700 00701 if (emcmotCommand->commandNum != emcmotStatus->commandNumEcho) 00702 { 00703 /* got a new command-- echo command and number... */ 00704 emcmotStatus->commandEcho = emcmotCommand->command; 00705 emcmotStatus->commandNumEcho = emcmotCommand->commandNum; 00706 00707 /* clear status value by default */ 00708 emcmotStatus->commandStatus = EMCMOT_COMMAND_OK; 00709 00710 /* log it, if appropriate */ 00711 if (emcmotStatus->logStarted && 00712 emcmotStatus->logType == EMCMOT_LOG_TYPE_CMD) 00713 { 00714 ls.item.cmd.time = etime(); 00715 ls.item.cmd.command = emcmotCommand->command; 00716 ls.item.cmd.commandNum = emcmotCommand->commandNum; 00717 #if defined (rtlinux) && defined (RT_FIFO) 00718 rtf_put(EMCMOT_LOG_RTF, &ls, sizeof(EMCMOT_LOG_STRUCT)); 00719 #else /* not rtlinux or not RT_FIFO */ 00720 emcmotLogAdd(emcmotLog, ls); 00721 #endif /* else not rtlinux nor RT_FIFO */ 00722 } 00723 00724 /* ...and process command */ 00725 switch (emcmotCommand->command) 00726 { 00727 case EMCMOT_FREE: 00728 /* change the mode to free axis motion */ 00729 /* can be done at any time */ 00730 /* reset the coordinating flag to defer transition to 00731 controller cycle */ 00732 coordinating = 0; 00733 break; 00734 00735 case EMCMOT_COORD: 00736 /* change the mode to coordinated axis motion */ 00737 /* can be done at any time */ 00738 /* set the coordinating flag to defer transition to 00739 controller cycle */ 00740 coordinating = 1; 00741 break; 00742 00743 case EMCMOT_SET_TRAJ_CYCLE_TIME: 00744 /* set the cycle time for trajectory calculations */ 00745 /* really should be done only at startup before 00746 controller is run, but at least it requires 00747 no active motions and the interpolators need 00748 to be cleared */ 00749 setTrajCycleTime(emcmotCommand->cycleTime); 00750 break; 00751 00752 case EMCMOT_SET_SERVO_CYCLE_TIME: 00753 /* set the cycle time for servo calculations, which is the 00754 period for emcmotController execution */ 00755 /* really should be done only at startup before 00756 controller is run, but at least it requires 00757 no active motions and the interpolators need 00758 to be cleared */ 00759 setServoCycleTime(emcmotCommand->cycleTime); 00760 #ifdef rtlinux 00761 rt_task_make_periodic(&emcmotTask, 00762 rt_get_time(), 00763 (RTIME) (RT_TICKS_PER_SEC * 00764 emcmotStatus->servoCycleTime)); 00765 #ifdef STEPPER_MOTORS 00766 /* make stepper task run twice as fast as servo (axis) rate */ 00767 rt_task_make_periodic(&smTask, 00768 rt_get_time(), 00769 ((RTIME) (RT_TICKS_PER_SEC * 00770 emcmotStatus->servoCycleTime)) >> 1); 00771 #endif /* STEPPER_MOTORS */ 00772 #endif /* rtlinux */ 00773 break; 00774 00775 case EMCMOT_SET_POSITION_LIMITS: 00776 /* set the position limits for the axis */ 00777 /* can be done at any time */ 00778 axis = emcmotCommand->axis; 00779 if (axis < 0 || 00780 axis >= EMCMOT_MAX_AXIS) 00781 { 00782 break; 00783 } 00784 emcmotStatus->minLimit[axis] = emcmotCommand->minLimit; 00785 emcmotStatus->maxLimit[axis] = emcmotCommand->maxLimit; 00786 break; 00787 00788 case EMCMOT_SET_OUTPUT_LIMITS: 00789 /* set the output limits for the axis */ 00790 /* can be done at any time */ 00791 axis = emcmotCommand->axis; 00792 if (axis < 0 || 00793 axis >= EMCMOT_MAX_AXIS) 00794 { 00795 break; 00796 } 00797 00798 emcmotStatus->minOutput[axis] = emcmotCommand->minLimit; 00799 emcmotStatus->maxOutput[axis] = emcmotCommand->maxLimit; 00800 break; 00801 00802 case EMCMOT_SET_OUTPUT_SCALE: 00803 axis = emcmotCommand->axis; 00804 if (axis < 0 || 00805 axis >= EMCMOT_MAX_AXIS || 00806 emcmotCommand->scale == 0) 00807 { 00808 break; 00809 } 00810 emcmotStatus->outputScale[axis] = emcmotCommand->scale; 00811 emcmotStatus->outputOffset[axis] = emcmotCommand->offset; 00812 break; 00813 00814 case EMCMOT_SET_INPUT_SCALE: 00815 /* 00816 change the scale factor for the position input, e.g., 00817 encoder counts per unit. Note that this is not a good idea 00818 once things have gotten underway, since the axis will 00819 jump servo to the "new" position, the gains will no longer 00820 be appropriate, etc. 00821 */ 00822 axis = emcmotCommand->axis; 00823 if (axis < 0 || 00824 axis >= EMCMOT_MAX_AXIS || 00825 emcmotCommand->scale == 0) 00826 { 00827 break; 00828 } 00829 00830 /* adjust last saved input value to match this one, so we 00831 don't get a spurious following error */ 00832 lastInput[axis] = lastInput[axis] * emcmotStatus->inputScale[axis] + 00833 emcmotStatus->inputOffset[axis]; 00834 lastInput[axis] = (lastInput[axis] - emcmotCommand->offset) / 00835 emcmotCommand->scale; 00836 00837 /* now make them active */ 00838 emcmotStatus->inputScale[axis] = emcmotCommand->scale; 00839 emcmotStatus->inputOffset[axis] = emcmotCommand->offset; 00840 00841 /* STEPPER MOTORS */ 00842 smStepsPerUnit[axis] = emcmotCommand->scale; 00843 /* END STEPPER MOTORS */ 00844 break; 00845 00846 case EMCMOT_SET_MAX_FERROR: 00847 axis = emcmotCommand->axis; 00848 if (axis < 0 || 00849 axis >= EMCMOT_MAX_AXIS || 00850 emcmotCommand->maxFerror < 0.0) 00851 { 00852 break; 00853 } 00854 emcmotStatus->maxFerror[axis] = 00855 emcmotCommand->maxFerror; 00856 break; 00857 00858 case EMCMOT_JOG_CONT: 00859 /* do a continuous jog, implemented as an incremental 00860 jog to the software limit, or the full range of travel 00861 if software limits don't yet apply because we're not homed */ 00862 00863 /* check axis range */ 00864 axis = emcmotCommand->axis; 00865 if (axis < 0 || 00866 axis >= EMCMOT_MAX_AXIS) 00867 { 00868 break; 00869 } 00870 00871 /* requires no motion, in free mode, enable on */ 00872 if (GET_MOTION_COORD_FLAG() != 0 || 00873 ! GET_MOTION_INPOS_FLAG() || 00874 ! GET_MOTION_ENABLE_FLAG()) 00875 { 00876 SET_AXIS_ERROR_FLAG(axis, 1); 00877 break; 00878 } 00879 00880 /* don't jog further onto limits */ 00881 if (! checkJog(axis, emcmotCommand->vel)) 00882 { 00883 SET_AXIS_ERROR_FLAG(axis, 1); 00884 break; 00885 } 00886 00887 /* start jogPos "here", and override one value with jog end point */ 00888 jogPos = emcmotStatus->pos; 00889 00890 switch (axis) 00891 { 00892 case 0: 00893 if (emcmotCommand->vel > 0.0) 00894 { 00895 if (GET_AXIS_HOMED_FLAG(axis)) 00896 { 00897 jogPos.tran.x = emcmotStatus->maxLimit[axis]; 00898 } 00899 else 00900 { 00901 jogPos.tran.x = emcmotStatus->pos.tran.x + XRANGE; 00902 } 00903 } 00904 else 00905 { 00906 if (GET_AXIS_HOMED_FLAG(axis)) 00907 { 00908 jogPos.tran.x = emcmotStatus->minLimit[axis]; 00909 } 00910 else 00911 { 00912 jogPos.tran.x = emcmotStatus->pos.tran.x - XRANGE; 00913 } 00914 } 00915 jogPos.tran.y = 0.0; 00916 jogPos.tran.z = 0.0; 00917 break; 00918 00919 case 1: 00920 if (emcmotCommand->vel > 0.0) 00921 { 00922 if (GET_AXIS_HOMED_FLAG(axis)) 00923 { 00924 jogPos.tran.y = emcmotStatus->maxLimit[axis]; 00925 } 00926 else 00927 { 00928 jogPos.tran.y = emcmotStatus->pos.tran.y + YRANGE; 00929 } 00930 } 00931 else 00932 { 00933 if (GET_AXIS_HOMED_FLAG(axis)) 00934 { 00935 jogPos.tran.y = emcmotStatus->minLimit[axis]; 00936 } 00937 else 00938 { 00939 jogPos.tran.y = emcmotStatus->pos.tran.y - YRANGE; 00940 } 00941 } 00942 jogPos.tran.z = 0.0; 00943 jogPos.tran.x = 0.0; 00944 break; 00945 00946 case 2: 00947 if (emcmotCommand->vel > 0.0) 00948 { 00949 if (GET_AXIS_HOMED_FLAG(axis)) 00950 { 00951 jogPos.tran.z = emcmotStatus->maxLimit[axis]; 00952 } 00953 else 00954 { 00955 jogPos.tran.z = emcmotStatus->pos.tran.z + ZRANGE; 00956 } 00957 } 00958 else 00959 { 00960 if (GET_AXIS_HOMED_FLAG(axis)) 00961 { 00962 jogPos.tran.z = emcmotStatus->minLimit[axis]; 00963 } 00964 else 00965 { 00966 jogPos.tran.z = emcmotStatus->pos.tran.z - ZRANGE; 00967 } 00968 } 00969 jogPos.tran.x = 0.0; 00970 jogPos.tran.y = 0.0; 00971 break; 00972 00973 default: 00974 break; 00975 } 00976 00977 saveVmax = freeAxis[axis].vMax; 00978 tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel)); 00979 tpAddLine(&freeAxis[axis], jogPos); 00980 tpSetVmax(&freeAxis[axis], saveVmax); 00981 SET_AXIS_ERROR_FLAG(axis, 0); 00982 00983 break; 00984 00985 case EMCMOT_JOG_INCR: 00986 /* do an incremental jog */ 00987 00988 /* check axis range */ 00989 axis = emcmotCommand->axis; 00990 if (axis < 0 || 00991 axis >= EMCMOT_MAX_AXIS) 00992 { 00993 break; 00994 } 00995 00996 /* requires no motion, in free mode, enable on */ 00997 if (GET_MOTION_COORD_FLAG() != 0 || 00998 ! GET_MOTION_INPOS_FLAG() || 00999 ! GET_MOTION_ENABLE_FLAG()) 01000 { 01001 SET_AXIS_ERROR_FLAG(axis, 1); 01002 break; 01003 } 01004 01005 /* don't jog further onto limits */ 01006 if (! checkJog(axis, emcmotCommand->vel)) 01007 { 01008 SET_AXIS_ERROR_FLAG(axis, 1); 01009 break; 01010 } 01011 01012 /* start jogPos "here", and add increment */ 01013 jogPos = emcmotStatus->pos; 01014 01015 switch (axis) 01016 { 01017 case 0: 01018 if (emcmotCommand->vel > 0.0) 01019 jogPos.tran.x += emcmotCommand->pos.tran.x; 01020 else 01021 jogPos.tran.x -= emcmotCommand->pos.tran.x; 01022 jogPos.tran.y = 0.0; 01023 jogPos.tran.z = 0.0; 01024 break; 01025 01026 case 1: 01027 if (emcmotCommand->vel > 0.0) 01028 jogPos.tran.y += emcmotCommand->pos.tran.y; 01029 else 01030 jogPos.tran.y -= emcmotCommand->pos.tran.y; 01031 jogPos.tran.z = 0.0; 01032 jogPos.tran.x = 0.0; 01033 break; 01034 01035 case 2: 01036 if (emcmotCommand->vel > 0.0) 01037 jogPos.tran.z += emcmotCommand->pos.tran.z; 01038 else 01039 jogPos.tran.z -= emcmotCommand->pos.tran.z; 01040 jogPos.tran.x = 0.0; 01041 jogPos.tran.y = 0.0; 01042 break; 01043 01044 default: 01045 break; 01046 } 01047 01048 saveVmax = freeAxis[axis].vMax; 01049 tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel)); 01050 tpAddLine(&freeAxis[axis], jogPos); 01051 tpSetVmax(&freeAxis[axis], saveVmax); 01052 SET_AXIS_ERROR_FLAG(axis, 0); 01053 01054 break; 01055 01056 case EMCMOT_JOG_ABS: 01057 /* do an absolute jog */ 01058 01059 /* check axis range */ 01060 axis = emcmotCommand->axis; 01061 if (axis < 0 || 01062 axis >= EMCMOT_MAX_AXIS) 01063 { 01064 break; 01065 } 01066 01067 /* requires no motion, in free mode, enable on */ 01068 if (GET_MOTION_COORD_FLAG() != 0 || 01069 ! GET_MOTION_INPOS_FLAG() || 01070 ! GET_MOTION_ENABLE_FLAG()) 01071 { 01072 SET_AXIS_ERROR_FLAG(axis, 1); 01073 break; 01074 } 01075 01076 /* don't jog further onto limits */ 01077 if (! checkJog(axis, emcmotCommand->vel)) 01078 { 01079 SET_AXIS_ERROR_FLAG(axis, 1); 01080 break; 01081 } 01082 01083 /* start jogPos "here", and set dest to arg */ 01084 jogPos = emcmotStatus->pos; 01085 01086 switch (axis) 01087 { 01088 case 0: 01089 jogPos.tran.x = emcmotCommand->pos.tran.x; 01090 jogPos.tran.y = 0.0; 01091 jogPos.tran.z = 0.0; 01092 break; 01093 01094 case 1: 01095 jogPos.tran.y = emcmotCommand->pos.tran.y; 01096 jogPos.tran.z = 0.0; 01097 jogPos.tran.x = 0.0; 01098 break; 01099 01100 case 2: 01101 jogPos.tran.z = emcmotCommand->pos.tran.z; 01102 jogPos.tran.x = 0.0; 01103 jogPos.tran.y = 0.0; 01104 break; 01105 01106 default: 01107 break; 01108 } 01109 01110 saveVmax = freeAxis[axis].vMax; 01111 tpSetVmax(&freeAxis[axis], fabs(emcmotCommand->vel)); 01112 tpAddLine(&freeAxis[axis], jogPos); 01113 tpSetVmax(&freeAxis[axis], saveVmax); 01114 SET_AXIS_ERROR_FLAG(axis, 0); 01115 01116 break; 01117 01118 case EMCMOT_SET_TERM_COND: 01119 /* sets termination condition for motion queue */ 01120 /* tpSetTermCond(&queue, emcmotCommand->termCond); */ 01121 break; 01122 01123 case EMCMOT_SET_LINE: 01124 /* queue up a linear move */ 01125 /* requires coordinated mode, enable off, not on limits */ 01126 if (! GET_MOTION_COORD_FLAG() || 01127 ! GET_MOTION_ENABLE_FLAG()) 01128 { 01129 reportError("Need to be enabled, in coord mode for linear move"); 01130 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; 01131 SET_MOTION_ERROR_FLAG(1); 01132 break; 01133 } 01134 else if (! inRange(emcmotCommand->pos)) 01135 { 01136 reportError("Linear move %d out of range", 01137 emcmotCommand->id); 01138 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; 01139 sqAbort(&queue); 01140 01141 SET_MOTION_ERROR_FLAG(1); 01142 break; 01143 } 01144 else if (! checkLimits()) 01145 { 01146 reportError("Can't do linear move with limits exceeded"); 01147 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; 01148 sqAbort(&queue); 01149 SET_MOTION_ERROR_FLAG(1); 01150 break; 01151 } 01152 01153 /* append it to the queue */ 01154 if ( -1 == sqAddLine( &queue, emcmotCommand->pos, \ 01155 emcmotCommand->id ) ) 01156 { 01157 reportError("Can't add linear move"); 01158 emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC; 01159 sqAbort(&queue); 01160 SET_MOTION_ERROR_FLAG(1); 01161 break; 01162 } 01163 else 01164 { 01165 SET_MOTION_ERROR_FLAG(0); 01166 } 01167 break; 01168 01169 case EMCMOT_SET_CIRCLE: 01170 /* queue up a circular move */ 01171 /* requires coordinated mode, enable on, not on limits */ 01172 if (! GET_MOTION_COORD_FLAG() || 01173 ! GET_MOTION_ENABLE_FLAG()) 01174 { 01175 reportError("Need to be enabled, in coord mode for circular move"); 01176 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND; 01177 SET_MOTION_ERROR_FLAG(1); 01178 break; 01179 } 01180 else if (! inRange(emcmotCommand->pos)) 01181 { 01182 reportError("Circular move %d out of range", 01183 emcmotCommand->id); 01184 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; 01185 sqAbort(&queue); 01186 01187 SET_MOTION_ERROR_FLAG(1); 01188 break; 01189 } 01190 else if (! checkLimits()) 01191 { 01192 reportError("Can't do circular move with limits exceeded"); 01193 emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS; 01194 sqAbort(&queue); 01195 01196 SET_MOTION_ERROR_FLAG(1); 01197 break; 01198 } 01199 01200 /* append it to the queue */ 01201 if ( -1 == sqAddCircle( &queue, emcmotCommand->pos, 01202 emcmotCommand->center, emcmotCommand->normal, 01203 emcmotCommand->turn, emcmotCommand->id ) ) 01204 { 01205 reportError("Can't add circular move"); 01206 emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC; 01207 sqAbort(&queue); 01208 01209 SET_MOTION_ERROR_FLAG(1); 01210 break; 01211 } 01212 else 01213 { 01214 SET_MOTION_ERROR_FLAG(0); 01215 } 01216 break; 01217 01218 case EMCMOT_SET_VEL: 01219 /* set the max velocity */ 01220 /* can do it at any time */ 01221 emcmotStatus->vel = emcmotCommand->vel; 01222 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01223 { 01224 tpSetVmax(&freeAxis[axis], emcmotStatus->vel); 01225 } 01226 sqSetFeed(&queue, emcmotStatus->vel); 01227 break; 01228 01229 /* set the "big" vel used for debouncing input, to be the 01230 10X the max seen so far */ 01231 if (bigVel < 10.0 * emcmotCommand->vel) 01232 { 01233 bigVel = 10.0 * emcmotCommand->vel; 01234 } 01235 break; 01236 01237 case EMCMOT_SET_VEL_LIMIT: 01238 /* set the absolute max velocity for all subsequent moves */ 01239 /* can do it at any time */ 01240 emcmotStatus->limitVel = emcmotCommand->vel; 01241 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01242 { 01243 tpSetVlimit(&freeAxis[axis], emcmotStatus->limitVel); 01244 } 01245 sqSetVmax(&queue, emcmotStatus->limitVel); 01246 /* now set the debounce vel */ 01247 bigVel = 10.0 * emcmotStatus->limitVel; 01248 break; 01249 01250 case EMCMOT_SET_HOMING_VEL: 01251 /* set the homing velocity */ 01252 /* can do it at any time */ 01253 /* sign of vel should set polarity, and mag-sign are recorded */ 01254 01255 /* check axis range */ 01256 axis = emcmotCommand->axis; 01257 if (axis < 0 || 01258 axis >= EMCMOT_MAX_AXIS) 01259 { 01260 break; 01261 } 01262 01263 if (emcmotCommand->vel < 0.0) 01264 { 01265 emcmotStatus->homingVel[axis] = - emcmotCommand->vel; 01266 SET_AXIS_HOMING_POLARITY(axis, 0); 01267 } 01268 else 01269 { 01270 emcmotStatus->homingVel[axis] = emcmotCommand->vel; 01271 SET_AXIS_HOMING_POLARITY(axis, 1); 01272 } 01273 break; 01274 01275 case EMCMOT_SET_ACC: 01276 /* set the max acceleration */ 01277 /* can do it at any time */ 01278 emcmotStatus->acc = emcmotCommand->acc; 01279 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01280 { 01281 tpSetAmax(&freeAxis[axis], emcmotStatus->acc); 01282 } 01283 sqSetMaxAcc(&queue, emcmotStatus->acc); 01284 break; 01285 01286 case EMCMOT_PAUSE: 01287 /* pause the motion */ 01288 /* can happen at any time */ 01289 01290 if ( 0 == coordinating ) 01291 { 01292 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01293 { 01294 tpPause(&freeAxis[axis]); 01295 } 01296 emcmotStatus->paused = 1; 01297 } 01298 else 01299 { 01300 if (-1 ==sqPause(&queue)) 01301 { 01302 reportError("Can't pause"); 01303 sqAbort(&queue); 01304 break; 01305 } 01306 emcmotStatus->paused= sqIsPaused(&queue); 01307 } 01308 break; 01309 01310 case EMCMOT_RESUME: 01311 /* resume paused motion */ 01312 /* can happen at any time */ 01313 /* not true for segment queue!! */ 01314 if ( 0 == coordinating ) 01315 { 01316 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01317 { 01318 tpResume(&freeAxis[axis]); 01319 } 01320 emcmotStatus->paused = 0; 01321 stepping = 0; 01322 } 01323 else 01324 { 01325 if (-1 == sqResume(&queue)) 01326 { 01327 reportError("Can't resume"); 01328 sqAbort(&queue); 01329 break; 01330 } 01331 01332 emcmotStatus->paused = sqIsPaused(&queue); 01333 stepping = sqIsStepping(&queue); 01334 } 01335 01336 break; 01337 01338 case EMCMOT_STEP: 01339 /* resume paused motion until id changes */ 01340 /* can happen at any time */ 01341 if ( 0 == coordinating ) 01342 { 01343 idForStep = emcmotStatus->id; 01344 stepping = 1; 01345 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01346 { 01347 tpResume(&freeAxis[axis]); 01348 } 01349 emcmotStatus->paused = 0; 01350 } 01351 else 01352 { 01353 if (-1 == sqStep(&queue) ) 01354 { 01355 reportError("Can't step"); 01356 sqAbort(&queue); 01357 break; 01358 } 01359 } 01360 01361 break; 01362 01363 case EMCMOT_SCALE: 01364 /* override speed */ 01365 /* can happen at any time */ 01366 if (emcmotCommand->scale < 0.0) 01367 { 01368 emcmotCommand->scale = 0.0; /* clamp it */ 01369 } 01370 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01371 { 01372 tpSetVscale(&freeAxis[axis], emcmotCommand->scale); 01373 } 01374 if (-1 == sqSetFeedOverride(&queue, emcmotCommand->scale)) 01375 { 01376 reportError("Can't set new feed"); 01377 sqAbort(&queue); 01378 break; 01379 } 01380 01381 01382 break; 01383 01384 case EMCMOT_ABORT: 01385 /* abort motion */ 01386 /* can happen at any time */ 01387 /* check for coord or free space motion active */ 01388 if (GET_MOTION_COORD_FLAG()) 01389 { 01390 sqAbort(&queue); 01391 SET_MOTION_ERROR_FLAG(0); 01392 } 01393 else 01394 { 01395 /* check axis range */ 01396 axis = emcmotCommand->axis; 01397 if (axis < 0 || 01398 axis >= EMCMOT_MAX_AXIS) 01399 { 01400 break; 01401 } 01402 tpAbort(&freeAxis[axis]); 01403 SET_AXIS_HOMING_FLAG(axis, 0); 01404 SET_AXIS_ERROR_FLAG(axis, 0); 01405 } 01406 break; 01407 01408 case EMCMOT_DISABLE: 01409 /* go into disable */ 01410 /* can happen at any time */ 01411 /* reset the enabling flag to defer disable until 01412 controller cycle (it *will* be honored) */ 01413 enabling = 0; 01414 break; 01415 01416 case EMCMOT_ENABLE: 01417 /* come out of disable */ 01418 /* can happen at any time */ 01419 /* set the enabling flag to defer enable 01420 until controller cycle */ 01421 enabling = 1; 01422 break; 01423 01424 case EMCMOT_SET_PID: 01425 /* configure the PID gains */ 01426 axis = emcmotCommand->axis; 01427 if (axis < 0 || 01428 axis >= EMCMOT_MAX_AXIS) 01429 { 01430 break; 01431 } 01432 pidSetGains(&emcmotStatus->pid[axis], 01433 emcmotCommand->pid); 01434 break; 01435 01436 case EMCMOT_ACTIVATE_AXIS: 01437 /* make axis active, so that amps will be enabled when 01438 system is enabled or disabled */ 01439 /* can be done at any time */ 01440 axis = emcmotCommand->axis; 01441 if (axis < 0 || 01442 axis >= EMCMOT_MAX_AXIS) 01443 { 01444 break; 01445 } 01446 SET_AXIS_ACTIVE_FLAG(axis, 1); 01447 break; 01448 01449 case EMCMOT_DEACTIVATE_AXIS: 01450 /* make axis inactive, so that amps won't be affected when 01451 system is enabled or disabled */ 01452 /* can be done at any time */ 01453 axis = emcmotCommand->axis; 01454 if (axis < 0 || 01455 axis >= EMCMOT_MAX_AXIS) 01456 { 01457 break; 01458 } 01459 SET_AXIS_ACTIVE_FLAG(axis, 0); 01460 break; 01461 01462 case EMCMOT_ENABLE_AMPLIFIER: 01463 /* enable the amplifier directly, but don't enable calculations */ 01464 /* can be done at any time */ 01465 axis = emcmotCommand->axis; 01466 if (axis < 0 || 01467 axis >= EMCMOT_MAX_AXIS) 01468 { 01469 break; 01470 } 01471 extAmpEnable(axis, GET_AXIS_ENABLE_POLARITY(axis)); 01472 break; 01473 01474 case EMCMOT_DISABLE_AMPLIFIER: 01475 /* disable the axis calculations and amplifier, but don't 01476 disable calculations */ 01477 /* can be done at any time */ 01478 axis = emcmotCommand->axis; 01479 if (axis < 0 || 01480 axis >= EMCMOT_MAX_AXIS) 01481 { 01482 break; 01483 } 01484 extAmpEnable(axis, ! GET_AXIS_ENABLE_POLARITY(axis)); 01485 break; 01486 01487 case EMCMOT_OPEN_LOG: 01488 /* open a data log */ 01489 axis = emcmotCommand->axis; 01490 valid = 0; 01491 if (emcmotCommand->logSize > 0 && 01492 emcmotCommand->logSize <= EMCMOT_LOG_MAX) 01493 { 01494 /* handle log-specific data */ 01495 switch (emcmotCommand->logType) 01496 { 01497 case EMCMOT_LOG_TYPE_AXIS_POS: 01498 if (axis >= 0 && 01499 axis < EMCMOT_MAX_AXIS) 01500 { 01501 loggingAxis = axis; 01502 valid = 1; 01503 } 01504 break; 01505 01506 default: 01507 valid = 1; 01508 break; 01509 } 01510 } 01511 01512 if (valid) 01513 { 01514 /* success */ 01515 #if defined (rtlinux) && defined (RT_FIFO) 01516 if (-1 == rtf_create(EMCMOT_LOG_RTF, emcmotCommand->logSize * sizeof(EMCMOT_LOG_STRUCT))) 01517 { 01518 /* error-- can't open log */ 01519 break; 01520 } 01521 #else /* not rtlinux or not RT_FIFO */ 01522 emcmotLogInit(emcmotLog, 01523 emcmotCommand->logType, 01524 emcmotCommand->logSize); 01525 #endif /* else not rtlinux or not RT_FIFO */ 01526 emcmotStatus->logOpen = 1; 01527 emcmotStatus->logStarted = 0; 01528 emcmotStatus->logSize = emcmotCommand->logSize; 01529 emcmotStatus->logSkip = emcmotCommand->logSkip; 01530 emcmotStatus->logType = emcmotCommand->logType; 01531 } 01532 break; 01533 01534 case EMCMOT_START_LOG: 01535 /* start logging */ 01536 if (emcmotStatus->logOpen) 01537 { 01538 emcmotStatus->logStarted = 1; 01539 logSkip = 0; 01540 } 01541 break; 01542 01543 case EMCMOT_STOP_LOG: 01544 /* stop logging */ 01545 emcmotStatus->logStarted = 0; 01546 break; 01547 01548 case EMCMOT_CLOSE_LOG: 01549 #if defined (rtlinux) && defined (RT_FIFO) 01550 rtf_destroy(EMCMOT_LOG_RTF); 01551 #endif /* rtlinux and RT_FIFO */ 01552 emcmotStatus->logOpen = 0; 01553 emcmotStatus->logStarted = 0; 01554 emcmotStatus->logSize = 0; 01555 emcmotStatus->logSkip = 0; 01556 emcmotStatus->logType = 0; 01557 break; 01558 01559 case EMCMOT_DAC_OUT: 01560 /* write output to dacs directly */ 01561 /* will only persist if amplifiers are disabled */ 01562 axis = emcmotCommand->axis; 01563 if (axis < 0 || 01564 axis >= EMCMOT_MAX_AXIS) 01565 { 01566 break; 01567 } 01568 emcmotStatus->output[axis] = emcmotCommand->dacOut; 01569 break; 01570 01571 case EMCMOT_HOME: 01572 /* home the specified axis */ 01573 /* need to be in free mode, enable on */ 01574 /* homing is basically a slow incremental jog to full range */ 01575 axis = emcmotCommand->axis; 01576 if (GET_MOTION_COORD_FLAG() != 0 || 01577 ! GET_MOTION_ENABLE_FLAG()) 01578 { 01579 break; 01580 } 01581 if (axis < 0 || 01582 axis >= EMCMOT_MAX_AXIS) 01583 { 01584 break; 01585 } 01586 01587 jogPos = emcmotStatus->pos; 01588 01589 switch (axis) 01590 { 01591 case 0: 01592 if (GET_AXIS_HOMING_POLARITY(axis)) 01593 { 01594 jogPos.tran.x += 2.0 * XRANGE; 01595 } 01596 else 01597 { 01598 jogPos.tran.x -= 2.0 * XRANGE; 01599 } 01600 jogPos.tran.y = 0.0; 01601 jogPos.tran.z = 0.0; 01602 break; 01603 01604 case 1: 01605 if (GET_AXIS_HOMING_POLARITY(axis)) 01606 { 01607 jogPos.tran.y += 2.0 * YRANGE; 01608 } 01609 else 01610 { 01611 jogPos.tran.y -= 2.0 * YRANGE; 01612 } 01613 jogPos.tran.z = 0.0; 01614 jogPos.tran.x = 0.0; 01615 break; 01616 01617 case 2: 01618 if (GET_AXIS_HOMING_POLARITY(axis)) 01619 { 01620 jogPos.tran.z += 2.0 * ZRANGE; 01621 } 01622 else 01623 { 01624 jogPos.tran.z -= 2.0 * ZRANGE; 01625 } 01626 jogPos.tran.x = 0.0; 01627 jogPos.tran.y = 0.0; 01628 break; 01629 01630 default: 01631 break; 01632 } 01633 01634 saveVmax = freeAxis[axis].vMax; 01635 tpSetVmax(&freeAxis[axis], emcmotStatus->homingVel[axis]); 01636 tpAddLine(&freeAxis[axis], jogPos); 01637 tpSetVmax(&freeAxis[axis], saveVmax); 01638 waitingForLatch[axis] = 1; 01639 SET_AXIS_HOMING_FLAG(axis, 1); 01640 SET_AXIS_HOMED_FLAG(axis, 0); 01641 break; 01642 01643 case EMCMOT_ENABLE_WATCHDOG: 01644 wdEnable = 1; 01645 if (emcmotCommand->wdWait < 0) 01646 { 01647 wdWait = 0; 01648 } 01649 else 01650 { 01651 wdWait = emcmotCommand->wdWait; 01652 } 01653 wdCount = wdWait; 01654 break; 01655 01656 case EMCMOT_DISABLE_WATCHDOG: 01657 wdEnable = 0; 01658 break; 01659 01660 case EMCMOT_SET_POLARITY: 01661 axis = emcmotCommand->axis; 01662 if (axis < 0 || 01663 axis >= EMCMOT_MAX_AXIS) 01664 { 01665 break; 01666 } 01667 if (emcmotCommand->level) 01668 { 01669 /* normal */ 01670 emcmotStatus->axisPolarity[axis] |= emcmotCommand->axisFlag; 01671 } 01672 else 01673 { 01674 /* inverted */ 01675 emcmotStatus->axisPolarity[axis] &= ~emcmotCommand->axisFlag; 01676 } 01677 break; 01678 01679 default: 01680 reportError("Unrecognized command %d", emcmotCommand->command); 01681 emcmotStatus->commandStatus = EMCMOT_COMMAND_UNKNOWN_COMMAND; 01682 break; 01683 } /* end of command switch */ 01684 } /* end of if-new-command */ 01685 01686 #if defined (rtlinux) && defined (RT_FIFO) 01687 } /* end of while-msg loop for RT FIFO */ 01688 01689 if (err != 0) 01690 { 01691 return -1; 01692 } 01693 01694 #endif /* rtlinux and RT_FIFO */ 01695 01696 return 0; 01697 } |
|
Definition at line 1721 of file emcsegmot.c. 01722 { 01723 double start, end, delta; /* time stamping */ 01724 int homeFlag; /* result of ext read to home switch */ 01725 int axis; /* axis loop counter */ 01726 int isLimit; /* result of ext read to limit sw */ 01727 int whichCycle; /* 0=none, 1=servo, 2=traj */ 01728 int fault; 01729 double thisFerror[EMCMOT_MAX_AXIS]; 01730 01731 #ifdef rtlinux 01732 for (;;) 01733 { 01734 #endif /* rtlinux */ 01735 01736 #ifdef STEPPER_MOTORS 01737 if (fpInUse) 01738 { 01739 rt_task_wait(); 01740 continue; 01741 } 01742 #endif /* rtlinux */ 01743 01744 #ifdef rtlinux 01745 /* toggle sound, parallel port watchdogs */ 01746 if (wdEnable && 01747 wdCount-- <= 0) 01748 { 01749 soundByte = inb(SOUND_PORT); 01750 soundByte &= SOUND_MASK; 01751 toggle = !toggle; 01752 if (toggle) 01753 soundByte |= ~SOUND_MASK; 01754 outb(soundByte, SOUND_PORT); 01755 wdCount = wdWait; 01756 } 01757 #endif /* rtlinux */ 01758 01759 /* increment head count */ 01760 emcmotStatus->head++; 01761 01762 /* record start time */ 01763 start = etime(); 01764 01765 /* READ COMMAND: */ 01766 01767 #ifndef RT_FIFO 01768 emcmotCommandHandler(); 01769 01770 #endif /* not RT_FIFO */ 01771 01772 /* SAVE LAST CYCLE'S VALUES */ 01773 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01774 { 01775 oldInput[axis] = emcmotStatus->input[axis]; 01776 oldJointPos[axis] = jointPos[axis]; 01777 } 01778 01779 /* READ INPUTS: */ 01780 01781 /* latch all encoder feedback into raw input array */ 01782 #ifdef STEPPER_MOTORS 01783 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01784 { 01785 rawInput[axis] = (double) smStepsAccum[axis]; 01786 } 01787 #else 01788 extEncoderReadAll(EMCMOT_MAX_AXIS, rawInput); 01789 #endif 01790 01791 /* process input read limit switches */ 01792 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01793 { 01794 /* FIXME-- testing */ 01795 01796 /* scale according to scaled = scale (raw - offset) */ 01797 emcmotStatus->input[axis] = 01798 (rawInput[axis] - emcmotStatus->inputOffset[axis]) / 01799 emcmotStatus->inputScale[axis]; 01800 01801 #ifndef STEPPER_MOTORS 01802 /* debounce bad feedback */ 01803 if (fabs(emcmotStatus->input[axis] - lastInput[axis]) / 01804 emcmotStatus->servoCycleTime > bigVel) { 01805 /* bad input value-- debounce with the last value */ 01806 emcmotStatus->input[axis] = lastInput[axis]; 01807 } 01808 #endif 01809 lastInput[axis] = emcmotStatus->input[axis]; 01810 01811 /* reset limit flags initially */ 01812 SET_AXIS_PHL_FLAG(axis, 0); 01813 SET_AXIS_NHL_FLAG(axis, 0); 01814 01815 /* read switches from external interface */ 01816 extMaxLimitSwitchRead(axis, &isLimit); 01817 01818 /* set flags if on max limit */ 01819 if (isLimit == GET_AXIS_PHL_POLARITY(axis)) 01820 { 01821 if (++maxLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE) 01822 { 01823 /* set max limit bit */ 01824 SET_AXIS_PHL_FLAG(axis, 1); 01825 maxLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE; 01826 } 01827 } 01828 else 01829 { 01830 maxLimitSwitchCount[axis] = 0; 01831 } 01832 01833 /* read switches from external interface */ 01834 extMinLimitSwitchRead(axis, &isLimit); 01835 01836 /* set flags if on min limit */ 01837 if (isLimit == GET_AXIS_NHL_POLARITY(axis)) 01838 { 01839 if (++minLimitSwitchCount[axis] > LIMIT_SWITCH_DEBOUNCE) 01840 { 01841 /* set min limit bit */ 01842 SET_AXIS_NHL_FLAG(axis, 1); 01843 minLimitSwitchCount[axis] = LIMIT_SWITCH_DEBOUNCE; 01844 } 01845 } 01846 else 01847 { 01848 minLimitSwitchCount[axis] = 0; 01849 } 01850 01851 /* read home switch */ 01852 extHomeSwitchRead(axis, &homeFlag); 01853 SET_AXIS_HOME_SWITCH_FLAG(axis, homeFlag == GET_AXIS_HOME_SWITCH_POLARITY(axis)); 01854 01855 /* read amp fault */ 01856 extAmpFault(axis, &fault); 01857 SET_AXIS_FAULT_FLAG(axis, fault == GET_AXIS_FAULT_POLARITY(axis)); 01858 /* FIXME-- do something with amp fault, for active axes */ 01859 } 01860 01861 /* RUN STATE LOGIC: */ 01862 01863 /* check for disabling */ 01864 if (! enabling && 01865 GET_MOTION_ENABLE_FLAG()) 01866 { 01867 /* clear out the motion queue and interpolators */ 01868 sqClearQueue(&queue); 01869 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01870 { 01871 tpClear(&freeAxis[axis]); 01872 cubicDrain(&cubic[axis]); 01873 if (GET_AXIS_ACTIVE_FLAG(axis)) 01874 { 01875 extAmpEnable(axis, 01876 ! GET_AXIS_ENABLE_POLARITY(axis)); 01877 SET_AXIS_ENABLE_FLAG(axis, 0); 01878 SET_AXIS_HOMING_FLAG(axis, 0); 01879 emcmotStatus->output[axis] = 0.0; 01880 } 01881 } 01882 01883 /* reset state flags */ 01884 SET_MOTION_ENABLE_FLAG(0); 01885 } 01886 01887 /* check for enabling */ 01888 if (enabling && 01889 ! GET_MOTION_ENABLE_FLAG()) 01890 { 01891 sqSetPos(&queue, emcmotStatus->pos); 01892 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01893 { 01894 tpSetPos(&freeAxis[axis], emcmotStatus->pos); 01895 pidReset(&emcmotStatus->pid[axis]); 01896 if (GET_AXIS_ACTIVE_FLAG(axis)) 01897 { 01898 extAmpEnable(axis, 01899 GET_AXIS_ENABLE_POLARITY(axis)); 01900 SET_AXIS_ENABLE_FLAG(axis, 1); 01901 } 01902 SET_AXIS_ERROR_FLAG(axis, 0); 01903 } 01904 01905 /* reset state flags */ 01906 SET_MOTION_ENABLE_FLAG(1); 01907 SET_MOTION_ERROR_FLAG(0); 01908 } 01909 01910 /* check for entering coordinated mode */ 01911 if (coordinating && 01912 ! GET_MOTION_COORD_FLAG()) 01913 { 01914 if (GET_MOTION_INPOS_FLAG()) 01915 { 01916 /* update coordinated queue position */ 01917 sqSetPos(&queue, emcmotStatus->pos); 01918 01919 SET_MOTION_COORD_FLAG(1); 01920 SET_MOTION_ERROR_FLAG(0); 01921 } 01922 else 01923 { 01924 /* not in position-- don't honor mode change */ 01925 coordinating = 0; 01926 } 01927 } 01928 01929 /* check entering free space mode */ 01930 if (! coordinating && 01931 GET_MOTION_COORD_FLAG()) 01932 { 01933 if (GET_MOTION_INPOS_FLAG()) 01934 { 01935 /* update free planner positions */ 01936 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01937 { 01938 tpSetPos(&freeAxis[axis], emcmotStatus->pos); 01939 } 01940 SET_MOTION_COORD_FLAG(0); 01941 } 01942 else 01943 { 01944 /* not in position-- don't honor mode change */ 01945 coordinating = 1; 01946 } 01947 } 01948 01949 /* check for homing sequences */ 01950 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 01951 { 01952 if (GET_AXIS_HOMING_FLAG(axis)) 01953 { 01954 if (tpIsDone(&freeAxis[axis])) 01955 { 01956 /* set input offset to saved latch */ 01957 emcmotStatus->inputOffset[axis] = saveLatch[axis]; 01958 01959 /* recompute inputs to match so we don't have a 01960 momentary jump */ 01961 emcmotStatus->input[axis] = 01962 (rawInput[axis] - emcmotStatus->inputOffset[axis]) / 01963 emcmotStatus->inputScale[axis]; 01964 lastInput[axis] = emcmotStatus->input[axis]; 01965 01966 /* set axis position to 0 upon homing */ 01967 if (axis == 0) 01968 emcmotStatus->pos.tran.x = 0; 01969 else if (axis == 1) 01970 emcmotStatus->pos.tran.y = 0; 01971 else if (axis == 2) 01972 emcmotStatus->pos.tran.z = 0; 01973 01974 /* clear out the interpolator */ 01975 tpSetPos(&freeAxis[axis], emcmotStatus->pos); 01976 cubicDrain(&cubic[0]); 01977 cubicDrain(&cubic[1]); 01978 cubicDrain(&cubic[2]); 01979 SET_AXIS_HOMING_FLAG(axis, 0); 01980 SET_AXIS_HOMED_FLAG(axis, 1); 01981 } 01982 } 01983 } 01984 01985 /* RUN MOTION CALCULATIONS: */ 01986 01987 /* run axis interpolations and outputs, but only if we're 01988 enabled. This section is "suppressed" if we're not 01989 enabled, although the read/write of encoders/dacs is still 01990 done. */ 01991 whichCycle = 0; 01992 if (GET_MOTION_ENABLE_FLAG()) 01993 { 01994 /* set whichCycle to be at least a servo cycle */ 01995 whichCycle = 1; 01996 /* pull next point(s) from traj */ 01997 while (cubicNeedNextPoint(&cubic[0])) 01998 { 01999 /* set whichCycle to be a trajectory cycle */ 02000 whichCycle = 2; 02001 02002 /* save old values for vel, accel differences */ 02003 oldPos = emcmotStatus->pos; 02004 02005 /* get next point from free or coord planners */ 02006 if (GET_MOTION_COORD_FLAG() == 1) 02007 { 02008 /* run coordinated trajectory planning cycle */ 02009 if (-1 ==sqRunCycle(&queue)) 02010 { 02011 reportError("Runcycle error"); 02012 sqAbort(&queue); 02013 } 02014 sqGetPosition(&queue,&emcmotStatus->pos); 02015 } 02016 else 02017 { 02018 /* run free trajectory planning cycle */ 02019 tpRunCycle(&freeAxis[0]); 02020 tpRunCycle(&freeAxis[1]); 02021 tpRunCycle(&freeAxis[2]); 02022 emcmotStatus->pos.tran.x = tpGetPos(&freeAxis[0]).tran.x; 02023 emcmotStatus->pos.tran.y = tpGetPos(&freeAxis[1]).tran.y; 02024 emcmotStatus->pos.tran.z = tpGetPos(&freeAxis[2]).tran.z; 02025 } 02026 02027 /* calculate current vel, accel */ 02028 /* FIXME-- need to handle rotational vel, accel */ 02029 02030 newVel.tran.x = emcmotStatus->pos.tran.x - oldPos.tran.x; 02031 newVel.tran.y = emcmotStatus->pos.tran.y - oldPos.tran.y; 02032 newVel.tran.z = emcmotStatus->pos.tran.z - oldPos.tran.z; 02033 oldPos = emcmotStatus->pos; 02034 02035 newAcc.tran.x = newVel.tran.x - oldVel.tran.x; 02036 newAcc.tran.y = newVel.tran.y - oldVel.tran.y; 02037 newAcc.tran.z = newVel.tran.z - oldVel.tran.z; 02038 oldVel = newVel; 02039 02040 /* log per-traj-cycle data if logging active */ 02041 logIt = 0; 02042 if (emcmotStatus->logStarted && 02043 emcmotStatus->logSkip >= 0) { 02044 02045 /* save the type with the log item */ 02046 ls.type = emcmotStatus->logType; 02047 02048 /* now log type-specific data */ 02049 switch (emcmotStatus->logType) { 02050 02051 case EMCMOT_LOG_TYPE_TRAJ_POS: 02052 if (logSkip-- <= 0) { 02053 ls.item.trajPos.time = start; 02054 ls.item.trajPos.pos = emcmotStatus->pos.tran; 02055 logSkip = emcmotStatus->logSkip; 02056 logIt = 1; 02057 } 02058 break; 02059 02060 case EMCMOT_LOG_TYPE_TRAJ_VEL: 02061 if (logSkip-- <= 0) { 02062 ls.item.trajVel.time = start; 02063 ls.item.trajVel.vel = newVel.tran; 02064 /* pmCartMag(newVel.tran, &ls.item.trajVel.mag); */ 02065 ls.item.trajVel.mag = sqGetVel(&queue); 02066 logSkip = emcmotStatus->logSkip; 02067 logIt = 1; 02068 } 02069 break; 02070 02071 case EMCMOT_LOG_TYPE_TRAJ_ACC: 02072 if (logSkip-- <= 0) { 02073 ls.item.trajAcc.time = start; 02074 ls.item.trajAcc.acc = newAcc.tran; 02075 pmCartMag(newAcc.tran, &ls.item.trajAcc.mag); 02076 logSkip = emcmotStatus->logSkip; 02077 logIt = 1; 02078 } 02079 break; 02080 02081 default: 02082 break; 02083 } /* end of switch on log type */ 02084 02085 /* now log it */ 02086 #if defined (rtlinux) && defined (RT_FIFO) 02087 if (logIt) { 02088 rtf_put(EMCMOT_LOG_RTF, &ls, sizeof(EMCMOT_LOG_STRUCT)); 02089 /* FIXME-- update emcmotStatus->logPoints */ 02090 logIt = 0; 02091 } 02092 #else 02093 if (logIt) { 02094 emcmotLogAdd(emcmotLog, ls); 02095 emcmotStatus->logPoints = emcmotLog->howmany; 02096 logIt = 0; 02097 } 02098 #endif 02099 } /* end of logging per-traj-cycle data */ 02100 02101 /* convert to joints and spline it up */ 02102 inverseKinematics(emcmotStatus->pos, trajPos); 02103 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02104 { 02105 cubicAddPoint(&cubic[axis], trajPos[axis]); 02106 } 02107 02108 /* check for limits-- only do this on a trajectory cycle */ 02109 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02110 { 02111 /* reset soft limit flags initially */ 02112 SET_AXIS_NSL_FLAG(axis, 0); 02113 SET_AXIS_PSL_FLAG(axis, 0); 02114 02115 /* check for soft or hard limits-- hard limits always, 02116 soft limits only if we've been homed. If we're not 02117 homed we ignore software limits since the position 02118 values are meaningless */ 02119 if (GET_AXIS_NHL_FLAG(axis) || 02120 GET_AXIS_PHL_FLAG(axis) || 02121 (GET_AXIS_HOMED_FLAG(axis) && 02122 (trajPos[axis] > emcmotStatus->maxLimit[axis] || 02123 trajPos[axis] < emcmotStatus->minLimit[axis])) ) 02124 { 02125 /* set axis error flag in any case */ 02126 SET_AXIS_ERROR_FLAG(axis, 1); 02127 02128 /* set appropriate flag */ 02129 if (GET_AXIS_HOMED_FLAG(axis) && 02130 trajPos[axis] < emcmotStatus->minLimit[axis]) 02131 { 02132 SET_AXIS_NSL_FLAG(axis, 1); 02133 } 02134 else if (GET_AXIS_HOMED_FLAG(axis) && 02135 trajPos[axis] > emcmotStatus->maxLimit[axis]) 02136 { 02137 SET_AXIS_PSL_FLAG(axis, 1); 02138 } 02139 02140 /* signal for abort */ 02141 if (! wasOnLimit[axis]) 02142 { 02143 if (GET_MOTION_COORD_FLAG() == 1) 02144 { 02145 sqAbort(&queue); 02146 } 02147 else if (GET_MOTION_COORD_FLAG() == 0) 02148 { 02149 tpAbort(&freeAxis[axis]); 02150 } 02151 wasOnLimit[axis] = 1; 02152 } 02153 } 02154 else 02155 { 02156 wasOnLimit[axis] = 0; 02157 } 02158 } /* for (axis = 0, ...) */ 02159 } /* while (cubicNeedNextPoint()) */ 02160 02161 /* run interpolation and compensation */ 02162 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02163 { 02164 /* interpolate */ 02165 jointPos[axis] = cubicInterpolate(&cubic[axis], 0, 0, 0, 0); 02166 02167 /* run output calculations */ 02168 if (GET_AXIS_ACTIVE_FLAG(axis)) 02169 { 02170 #ifndef STEPPER_MOTORS 02171 /* COMPENSATE: */ 02172 02173 /* 02174 compensation means compute output 02175 'emcmotStatus->output[]' based on desired position 02176 'jointPos[]' and input 'emcmotStatus->input[]'. 02177 02178 Currently the source calls for PID compensation. 02179 FIXME-- add wrapper for compensator, with ptr to 02180 emcmotStatus struct, with semantics that ->output[] 02181 needs to be filled. 02182 */ 02183 02184 /* here is PID compensation */ 02185 emcmotStatus->output[axis] = 02186 pidRunCycle(&emcmotStatus->pid[axis], 02187 emcmotStatus->input[axis], 02188 jointPos[axis]); 02189 #endif 02190 02191 /* COMPUTE FOLLOWING ERROR: */ 02192 02193 /* 02194 compute (signed) following error = commanded - actual, 02195 abort if necessary, and set or clear following error 02196 axis flag 02197 */ 02198 thisFerror[axis] = jointPos[axis] - emcmotStatus->input[axis]; 02199 /* record the max ferror for this axis */ 02200 if (emcmotStatus->ferror[axis] < fabs(thisFerror[axis])) 02201 { 02202 emcmotStatus->ferror[axis] = fabs(thisFerror[axis]); 02203 } 02204 02205 if (thisFerror[axis] > emcmotStatus->maxFerror[axis]) 02206 { 02207 /* abort! abort! following error exceeded */ 02208 SET_AXIS_ERROR_FLAG(axis, 1); 02209 SET_AXIS_FERROR_FLAG(axis, 1); 02210 if (enabling) 02211 { 02212 // report the following error just this once 02213 reportError("Axis %d following error", axis); 02214 } 02215 enabling = 0; 02216 } 02217 else 02218 { 02219 SET_AXIS_FERROR_FLAG(axis, 0); 02220 } 02221 } 02222 else 02223 { 02224 /* axis is not active-- leave the pid output where it is-- 02225 if axis is not active one can still write to the dac */ 02226 } 02227 02228 #ifndef STEPPER_MOTORS 02229 /* CLAMP OUTPUT: */ 02230 02231 /* 02232 clamp output means take 'emcmotStatus->output[]' and 02233 limit to range 02234 'emcmotStatus->minOutput[] .. emcmotStatus->maxOutput[]' 02235 */ 02236 if (emcmotStatus->output[axis] < emcmotStatus->minOutput[axis]) 02237 { 02238 emcmotStatus->output[axis] = emcmotStatus->minOutput[axis]; 02239 } 02240 else if (emcmotStatus->output[axis] > emcmotStatus->maxOutput[axis]) 02241 { 02242 emcmotStatus->output[axis] = emcmotStatus->maxOutput[axis]; 02243 } 02244 #endif 02245 02246 /* CHECK FOR LATCH CONDITION: */ 02247 02248 /* 02249 check for latch condition means if we're waiting for a 02250 latched index pulse, and we see the pulse and the home 02251 switch, we read the raw input and abort. The offset is set 02252 above in the homing section by noting that if we're homing, 02253 and waitingForLatch[] is cleared, we latched. 02254 02255 This presumes an encoder index pulse. 02256 FIXME-- remove explicit calls to encoder index pulse, to 02257 allow for open-loop control latching via switches only. 02258 */ 02259 if (waitingForLatch[axis]) 02260 { 02261 if (GET_AXIS_HOME_SWITCH_FLAG(axis)) 02262 { 02263 /* read encoder index */ 02264 #ifdef STEPPER_MOTORS 02265 /* force an "encoder latch" */ 02266 latchFlag[axis] = 1; 02267 #else 02268 extEncoderResetIndex(axis); 02269 extEncoderReadLatch(axis, &latchFlag[axis]); 02270 #endif 02271 if (latchFlag[axis]) 02272 { 02273 /* get latched position in RAW UNITS */ 02274 saveLatch[axis] = rawInput[axis]; 02275 waitingForLatch[axis] = 0; 02276 /* call for an abort-- when it's finished, code 02277 above sets inputOffset[] to saveLatch[] */ 02278 tpAbort(&freeAxis[axis]); 02279 } 02280 } 02281 } 02282 02283 } /* end of axis for-loop */ 02284 02285 } /* end of if-enabled section */ 02286 02287 else /* not enabled */ 02288 { 02289 /* set jointPos scaled input */ 02290 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02291 { 02292 jointPos[axis] = emcmotStatus->input[axis]; 02293 } 02294 02295 /* set actual pos to the Cartesian forward of the joint pos */ 02296 forwardKinematics(jointPos, &emcmotStatus->pos); 02297 02298 } /* end of if-not-enabled section */ 02299 02300 #ifndef STEPPER_MOTORS 02301 /* SCALE OUTPUTS: */ 02302 02303 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02304 { 02305 rawOutput[axis] = 02306 (emcmotStatus->output[axis] - emcmotStatus->outputOffset[axis]) / 02307 emcmotStatus->outputScale[axis]; 02308 } 02309 02310 /* WRITE OUTPUTS: */ 02311 02312 /* write DACs-- note that this is done even when not 02313 enabled, although in this case the pidOutputs are 02314 all zero unless manually overridden. They will not 02315 be set by any calculations if we're not enabled. */ 02316 extDacWriteAll(EMCMOT_MAX_AXIS, rawOutput); 02317 #endif 02318 02319 /* UPDATE THE REST OF THE DYNAMIC STATUS: */ 02320 02321 /* run actual input position at servo rate through forward kins 02322 to get actual Cartesian pos */ 02323 forwardKinematics(emcmotStatus->input, &emcmotStatus->actualPos); 02324 02325 /* health heartbeat */ 02326 emcmotStatus->heartbeat++; 02327 02328 /* motion queue status */ 02329 emcmotStatus->depth = sqGetDepth(&queue); 02330 emcmotStatus->activeDepth = sqGetDepth(&queue); 02331 emcmotStatus->qVscale = queue.feedOverrideFactor; 02332 emcmotStatus->id= sqGetID(&queue); 02333 emcmotStatus->queueFull = sqIsQueueFull(&queue); 02334 02335 02336 02337 SET_MOTION_INPOS_FLAG(0); 02338 if (sqIsDone(&queue)) 02339 { 02340 SET_MOTION_INPOS_FLAG(1); 02341 } 02342 02343 /* axis status */ 02344 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02345 { 02346 emcmotStatus->axVscale[axis] = freeAxis[axis].vScale; 02347 02348 SET_AXIS_INPOS_FLAG(axis, 0); 02349 if (tpIsDone(&freeAxis[axis])) 02350 { 02351 SET_AXIS_INPOS_FLAG(axis, 1); 02352 } 02353 } 02354 02355 /* check to see if we should pause in order to implement 02356 single stepping. This is only necessary to check 02357 when in free mode */ 02358 if (!coordinating && stepping && idForStep != emcmotStatus->id) 02359 { 02360 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02361 { 02362 tpPause(&freeAxis[axis]); 02363 } 02364 /* tpPause(&queue); */ 02365 stepping = 0; 02366 emcmotStatus->paused = 1; 02367 } 02368 02369 /* calculate elapsed time and min/max/avg */ 02370 end = etime(); 02371 delta = end - start; 02372 emcmotStatus->computeTime = delta; 02373 if (2 == whichCycle) 02374 { 02375 /* traj calcs done this cycle-- use tMin,Max,Avg */ 02376 mmxavgAdd(&tMmxavg, delta); 02377 emcmotStatus->tMin = mmxavgMin(&tMmxavg); 02378 emcmotStatus->tMax = mmxavgMax(&tMmxavg); 02379 emcmotStatus->tAvg = mmxavgAvg(&tMmxavg); 02380 } 02381 else if (1 == whichCycle) 02382 { 02383 /* servo calcs only this cycle-- use sMin,Max,Avg */ 02384 mmxavgAdd(&sMmxavg, delta); 02385 emcmotStatus->sMin = mmxavgMin(&sMmxavg); 02386 emcmotStatus->sMax = mmxavgMax(&sMmxavg); 02387 emcmotStatus->sAvg = mmxavgAvg(&sMmxavg); 02388 } 02389 else 02390 { 02391 /* calcs disabled this cycle-- use nMin,Max,Avg */ 02392 mmxavgAdd(&nMmxavg, delta); 02393 emcmotStatus->nMin = mmxavgMin(&nMmxavg); 02394 emcmotStatus->nMax = mmxavgMax(&nMmxavg); 02395 emcmotStatus->nAvg = mmxavgAvg(&nMmxavg); 02396 } 02397 02398 /* log per-servo-cycle data if logging active */ 02399 logIt = 0; 02400 if (emcmotStatus->logStarted && 02401 emcmotStatus->logSkip >= 0) { 02402 02403 /* record type here, since all will set this */ 02404 ls.type = emcmotStatus->logType; 02405 02406 /* now log type-specific data */ 02407 switch (ls.type) { 02408 02409 case EMCMOT_LOG_TYPE_AXIS_POS: 02410 if (logSkip-- <= 0) { 02411 ls.item.axisPos.time = end; 02412 ls.item.axisPos.input = emcmotStatus->input[loggingAxis]; 02413 ls.item.axisPos.output = jointPos[loggingAxis]; 02414 logSkip = emcmotStatus->logSkip; 02415 logIt = 1; 02416 } 02417 break; 02418 02419 case EMCMOT_LOG_TYPE_ALL_INPOS: 02420 if (logSkip-- <= 0) { 02421 ls.item.allInpos.time = end; 02422 for (axis = 0; 02423 axis < EMCMOT_LOG_NUM_AXES && 02424 axis < EMCMOT_MAX_AXIS; 02425 axis++) { 02426 ls.item.allInpos.input[axis] = emcmotStatus->input[axis]; 02427 } 02428 logSkip = emcmotStatus->logSkip; 02429 logIt = 1; 02430 } 02431 break; 02432 02433 case EMCMOT_LOG_TYPE_ALL_OUTPOS: 02434 if (logSkip-- <= 0) { 02435 ls.item.allOutpos.time = end; 02436 for (axis = 0; 02437 axis < EMCMOT_LOG_NUM_AXES && 02438 axis < EMCMOT_MAX_AXIS; 02439 axis++) { 02440 ls.item.allOutpos.output[axis] = jointPos[axis]; 02441 } 02442 logSkip = emcmotStatus->logSkip; 02443 logIt = 1; 02444 } 02445 break; 02446 02447 case EMCMOT_LOG_TYPE_AXIS_VEL: 02448 if (logSkip-- <= 0) { 02449 ls.item.axisVel.time = end; 02450 ls.item.axisVel.cmdVel = jointPos[loggingAxis] - 02451 oldJointPos[loggingAxis]; 02452 ls.item.axisVel.actVel = emcmotStatus->input[loggingAxis] - 02453 oldInput[loggingAxis]; 02454 logSkip = emcmotStatus->logSkip; 02455 logIt = 1; 02456 } 02457 break; 02458 02459 case EMCMOT_LOG_TYPE_ALL_FERROR: 02460 if (logSkip-- <= 0) { 02461 ls.item.allFerror.time = end; 02462 for (axis = 0; 02463 axis < EMCMOT_LOG_NUM_AXES && 02464 axis < EMCMOT_MAX_AXIS; 02465 axis++) { 02466 ls.item.allFerror.ferror[axis] = thisFerror[axis]; 02467 } 02468 logSkip = emcmotStatus->logSkip; 02469 logIt = 1; 02470 } 02471 break; 02472 02473 default: 02474 break; 02475 } /* end of switch on log type */ 02476 02477 /* now log it */ 02478 #if defined (rtlinux) && defined (RT_FIFO) 02479 if (logIt) { 02480 rtf_put(EMCMOT_LOG_RTF, &ls, sizeof(EMCMOT_LOG_STRUCT)); 02481 /* FIXME-- update emcmotStatus->logPoints */ 02482 logIt = 0; 02483 } 02484 #else 02485 if (logIt) { 02486 emcmotLogAdd(emcmotLog, ls); 02487 emcmotStatus->logPoints = emcmotLog->howmany; 02488 logIt = 0; 02489 } 02490 #endif 02491 } /* end of if logging */ 02492 02493 /* set tail to head, which has already been incremented */ 02494 emcmotStatus->tail = emcmotStatus->head; 02495 02496 /* WRITE STATUS DATA */ 02497 02498 #if defined (rtlinux) && defined (RT_FIFO) 02499 /* write to rtlinux FIFO */ 02500 rtf_put(EMCMOT_STATUS_RTF, &emcmotStatusBuffer, sizeof(EMCMOT_STATUS)); 02501 02502 #endif /* rtlinux and RT_FIFO */ 02503 02504 #ifdef rtlinux 02505 /* wait for next cycle */ 02506 rt_task_wait(); 02507 } 02508 #endif /* rtlinux */ 02509 } |
|
Definition at line 2976 of file emcsegmot.c. 02977 { 02978 int t; 02979 int len; 02980 02981 for (t = 1; t < argc; t++) 02982 { 02983 /* try -ini */ 02984 /* NOTE: motion controller cannot normally read an INI file. 02985 This is provided for main() simulation, which can read 02986 an INI file and needs it for the plant simulation parameters */ 02987 if (!strcmp(argv[t], "-ini")) 02988 { 02989 if (t == argc - 1) 02990 { 02991 diagnostics("missing -ini parameter\n"); 02992 return -1; 02993 } 02994 else 02995 { 02996 strcpy(EMCMOT_INIFILE, argv[t+1]); 02997 t++; /* step over following arg */ 02998 continue; 02999 } 03000 } 03001 03002 /* try SHMEM_BASE_ADDRESS */ 03003 len = strlen("SHMEM_BASE_ADDRESS"); 03004 if (! strncmp(argv[t], "SHMEM_BASE_ADDRESS", len)) 03005 { 03006 if (argv[t][len] != '=' || 03007 1 != sscanf(&argv[t][len + 1], "%i", &SHMEM_BASE_ADDRESS)) 03008 { 03009 diagnostics("syntax: %s SHMEM_BASE_ADDRESS=integer\n", argv[0]); 03010 return -1; 03011 } 03012 else 03013 { 03014 continue; 03015 } 03016 } 03017 03018 /* try SHMEM_KEY */ 03019 len = strlen("SHMEM_KEY"); 03020 if (! strncmp(argv[t], "SHMEM_KEY", len)) 03021 { 03022 if (argv[t][len] != '=' || 03023 1 != sscanf(&argv[t][len + 1], "%i", &SHMEM_KEY)) 03024 { 03025 diagnostics("syntax: %s SHMEM_KEY=integer\n", argv[0]); 03026 return -1; 03027 } 03028 else 03029 { 03030 continue; 03031 } 03032 } 03033 03034 /* no match-- bad arg */ 03035 diagnostics("bad argument to %s: %s\n", argv[0], argv[t]); 03036 return -1; 03037 } 03038 03039 return 0; 03040 } |
|
Definition at line 523 of file emcsegmot.c. 00524 { 00525 double joint[EMCMOT_MAX_AXIS]; 00526 int axis; 00527 00528 /* fill in all joints with 0 */ 00529 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 00530 { 00531 joint[axis] = 0.0; 00532 } 00533 00534 /* now fill in with real values, for joints that are used */ 00535 inverseKinematics(pos, joint); 00536 00537 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 00538 { 00539 if (! GET_AXIS_ACTIVE_FLAG(axis)) 00540 { 00541 /* if axis is not active, don't even look at its limits */ 00542 continue; 00543 } 00544 00545 if (joint[axis] > emcmotStatus->maxLimit[axis] || 00546 joint[axis] < emcmotStatus->minLimit[axis]) 00547 { 00548 return 0; /* can't move further past limit */ 00549 } 00550 } 00551 00552 /* okay to move */ 00553 return 1; 00554 } |
|
Definition at line 2570 of file emcsegmot.c. 02571 { 02572 int axis; 02573 int t; 02574 #ifdef rtlinux 02575 RTIME now; 02576 #endif 02577 PID_STRUCT pid; 02578 02579 #ifdef rtlinux 02580 02581 #ifdef RT_FIFO 02582 rtf_create(EMCMOT_COMMAND_RTF, sizeof(EMCMOT_COMMAND) + 1); 02583 rtf_create(EMCMOT_STATUS_RTF, sizeof(EMCMOT_STATUS) + 1); 02584 rtf_create(EMCMOT_ERROR_RTF, EMCMOT_ERROR_NUM * EMCMOT_ERROR_LEN + 1); 02585 /* log is created, deleted dynamically */ 02586 02587 /* fifo puts and gets work on buffer structs, so point to these */ 02588 emcmotCommand = &emcmotCommandBuffer; 02589 emcmotStatus = &emcmotStatusBuffer; 02590 02591 #else /* not RT_FIFO */ 02592 /* set upper memory pointers */ 02593 emcmotStruct = (EMCMOT_STRUCT *) SHMEM_BASE_ADDRESS; 02594 02595 #endif /* else not RT_FIFO */ 02596 02597 #else /* not rtlinux */ 02598 /* get command shmem */ 02599 shmem = rcs_shm_open(SHMEM_KEY, sizeof(EMCMOT_STRUCT), 1, 0666); 02600 if (NULL == shmem || 02601 NULL == shmem->addr) 02602 { 02603 diagnostics("can't get shared memory\n"); 02604 return -1; 02605 } 02606 02607 memset(shmem->addr, 0, sizeof(EMCMOT_STRUCT)); 02608 /* map shmem area into local address space */ 02609 emcmotStruct = (EMCMOT_STRUCT *) shmem->addr; 02610 02611 #endif /* else not rtlinux */ 02612 02613 #ifndef RT_FIFO 02614 /* we'll reference emcmotStruct directly */ 02615 emcmotCommand = (EMCMOT_COMMAND *) &emcmotStruct->command; 02616 emcmotStatus = (EMCMOT_STATUS *) &emcmotStruct->status; 02617 emcmotError = (EMCMOT_ERROR *) &emcmotStruct->error; 02618 emcmotLog = (EMCMOT_LOG *) &emcmotStruct->log; 02619 02620 /* zero shared memory */ 02621 for (t = 0; t < sizeof(EMCMOT_STRUCT); t++) 02622 { 02623 ((char *) emcmotStruct)[t] = 0; 02624 } 02625 #endif /* not RT_FIFO */ 02626 02627 /* init locals */ 02628 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02629 { 02630 maxLimitSwitchCount[axis] = 0; 02631 minLimitSwitchCount[axis] = 0; 02632 /* STEPPER MOTORS */ 02633 smStepsPerUnit[axis] = 1.0; 02634 smStepsAccum[axis] = 0; 02635 smClkPhase[axis] = 0; 02636 /* END STEPPER MOTORS */ 02637 } 02638 02639 #ifndef RT_FIFO 02640 /* init error struct */ 02641 emcmotErrorInit(emcmotError); 02642 #endif /* not RT_FIFO */ 02643 02644 /* init command struct */ 02645 emcmotCommand->head = 0; 02646 emcmotCommand->command = 0; 02647 emcmotCommand->commandNum = 0; 02648 emcmotCommand->tail = 0; 02649 02650 /* init status struct */ 02651 02652 emcmotStatus->head = 0; 02653 02654 emcmotStatus->motionFlag = 0; 02655 SET_MOTION_ERROR_FLAG(0); 02656 SET_MOTION_COORD_FLAG(0); 02657 emcmotStatus->split = 0; 02658 emcmotStatus->commandEcho = 0; 02659 emcmotStatus->commandNumEcho = 0; 02660 emcmotStatus->commandStatus = 0; 02661 emcmotStatus->heartbeat = 0; 02662 emcmotStatus->computeTime = 0.0; 02663 emcmotStatus->numAxes = EMCMOT_MAX_AXIS; 02664 emcmotStatus->trajCycleTime = TRAJ_CYCLE_TIME; 02665 emcmotStatus->servoCycleTime = SERVO_CYCLE_TIME; 02666 emcmotStatus->pos.tran.x = 0.0; 02667 emcmotStatus->pos.tran.y = 0.0; 02668 emcmotStatus->pos.tran.z = 0.0; 02669 emcmotStatus->actualPos.tran.x = 0.0; 02670 emcmotStatus->actualPos.tran.y = 0.0; 02671 emcmotStatus->actualPos.tran.z = 0.0; 02672 emcmotStatus->vel = VELOCITY; 02673 emcmotStatus->limitVel = VELOCITY; 02674 emcmotStatus->acc = ACCELERATION; 02675 emcmotStatus->qVscale = 1.0; 02676 emcmotStatus->id = 0; 02677 emcmotStatus->depth = 0; 02678 emcmotStatus->activeDepth = 0; 02679 emcmotStatus->paused = 0; 02680 SET_MOTION_INPOS_FLAG(1); 02681 emcmotStatus->logOpen = 0; 02682 emcmotStatus->logStarted = 0; 02683 emcmotStatus->logSize = 0; 02684 emcmotStatus->logSkip = 0; 02685 emcmotStatus->logPoints = 0; 02686 SET_MOTION_ENABLE_FLAG(0); 02687 02688 oldPos = emcmotStatus->pos; 02689 oldVel.tran.x = 0.0; 02690 oldVel.tran.y = 0.0; 02691 oldVel.tran.z = 0.0; 02692 02693 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02694 { 02695 emcmotStatus->homingVel[axis] = VELOCITY; 02696 emcmotStatus->axisFlag[axis] = 0; 02697 emcmotStatus->maxLimit[axis] = MAX_LIMIT; 02698 emcmotStatus->minLimit[axis] = MIN_LIMIT; 02699 emcmotStatus->maxOutput[axis] = MAX_OUTPUT; 02700 emcmotStatus->minOutput[axis] = MIN_OUTPUT; 02701 emcmotStatus->maxFerror[axis] = MAX_FERROR; 02702 emcmotStatus->ferror[axis] = 0.0; 02703 emcmotStatus->outputScale[axis] = OUTPUT_SCALE; 02704 emcmotStatus->outputOffset[axis] = OUTPUT_OFFSET; 02705 emcmotStatus->inputScale[axis] = INPUT_SCALE; 02706 emcmotStatus->inputOffset[axis] = INPUT_OFFSET; 02707 emcmotStatus->axVscale[axis] = 1.0; 02708 SET_AXIS_ENABLE_FLAG(axis, 0); 02709 SET_AXIS_ACTIVE_FLAG(axis, 1); /* default is use it */ 02710 SET_AXIS_NSL_FLAG(axis, 0); 02711 SET_AXIS_PSL_FLAG(axis, 0); 02712 SET_AXIS_NHL_FLAG(axis, 0); 02713 SET_AXIS_PHL_FLAG(axis, 0); 02714 SET_AXIS_INPOS_FLAG(axis, 1); 02715 SET_AXIS_HOMING_FLAG(axis, 0); 02716 SET_AXIS_HOMED_FLAG(axis, 0); 02717 SET_AXIS_FERROR_FLAG(axis, 0); 02718 SET_AXIS_FAULT_FLAG(axis, 0); 02719 SET_AXIS_ERROR_FLAG(axis, 0); 02720 emcmotStatus->axisPolarity[axis] = (EMCMOT_AXIS_FLAG) 0xFFFFFFFF; 02721 /* will read encoders directly, so don't set them here */ 02722 } 02723 02724 /* FIXME-- add emcmotError */ 02725 02726 /* init min-max-avg stats */ 02727 mmxavgInit(&tMmxavg, tMmxavgSpace, MMXAVG_SIZE); 02728 mmxavgInit(&sMmxavg, sMmxavgSpace, MMXAVG_SIZE); 02729 mmxavgInit(&nMmxavg, nMmxavgSpace, MMXAVG_SIZE); 02730 emcmotStatus->tMin = 0.0; 02731 emcmotStatus->tMax = 0.0; 02732 emcmotStatus->tAvg = 0.0; 02733 emcmotStatus->sMin = 0.0; 02734 emcmotStatus->sMax = 0.0; 02735 emcmotStatus->sAvg = 0.0; 02736 emcmotStatus->nMin = 0.0; 02737 emcmotStatus->nMax = 0.0; 02738 emcmotStatus->nAvg = 0.0; 02739 02740 /* init motion queue */ 02741 #ifdef rtlinux 02742 if ( -1 == sqInitQueue(&queue,(SEGMENT *)(SHMEM_BASE_ADDRESS+sizeof(EMCMOT_STRUCT)), TC_QUEUE_SIZE)) 02743 #else 02744 sqMemPool = (SEGMENT *)malloc(TC_QUEUE_SIZE*sizeof(SEGMENT)); 02745 if ( sqMemPool == NULL ) 02746 { 02747 diagnostics("Error in init_module(): cannot allocate memory for segmentqueue\n"); 02748 return -1; 02749 } 02750 if ( -1 == sqInitQueue(&queue,sqMemPool, TC_QUEUE_SIZE)) 02751 #endif 02752 { 02753 diagnostics("can't initialize motion queue\n"); 02754 return -1; 02755 } 02756 sqSetCycleTime(&queue, emcmotStatus->trajCycleTime); 02757 sqSetPos(&queue, emcmotStatus->pos); 02758 sqSetVmax(&queue, emcmotStatus->vel); 02759 sqSetFeed(&queue, emcmotStatus->vel); 02760 /* FIX ME ^^^^ the feed and the absolute maximum velocity 02761 are set the same number */ 02762 02763 sqSetMaxAcc(&queue, emcmotStatus->acc); 02764 sqSetMaxFeedOverride(&queue, 1.0 ); 02765 02766 /* init the axis components */ 02767 pid.p = P_GAIN; 02768 pid.i = I_GAIN; 02769 pid.d = D_GAIN; 02770 pid.ff0 = FF0_GAIN; 02771 pid.ff1 = FF1_GAIN; 02772 pid.ff2 = FF2_GAIN; 02773 pid.backlash = BACKLASH; 02774 pid.bias = BIAS; 02775 pid.maxError = MAX_ERROR; 02776 02777 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02778 { 02779 if (-1 == tpCreate(&freeAxis[axis], FREE_AXIS_QUEUE_SIZE, freeAxisTcSpace[axis])) 02780 { 02781 diagnostics("can't create axis queue %d\n", axis); 02782 return -1; 02783 } 02784 tpInit(&freeAxis[axis]); 02785 tpSetCycleTime(&freeAxis[axis], emcmotStatus->trajCycleTime); 02786 tpSetPos(&freeAxis[axis], emcmotStatus->pos); 02787 tpSetVmax(&freeAxis[axis], emcmotStatus->vel); 02788 tpSetAmax(&freeAxis[axis], emcmotStatus->acc); 02789 pidInit(&emcmotStatus->pid[axis]); 02790 pidSetGains(&emcmotStatus->pid[axis], pid); 02791 cubicInit(&cubic[axis]); 02792 } 02793 02794 /* init the time and rate using functions to affect traj, the 02795 pids, and the cubics properly, since they're coupled */ 02796 setTrajCycleTime(TRAJ_CYCLE_TIME); 02797 setServoCycleTime(SERVO_CYCLE_TIME); 02798 02799 /* init board */ 02800 if (-1 == extMotInit((const char *) 0)) 02801 { 02802 diagnostics("can't initialize motion hardware\n"); 02803 return -1; 02804 } 02805 if (-1 == extDioInit((const char *) 0)) 02806 { 02807 diagnostics("can't initialize DIO hardware\n"); 02808 return -1; 02809 } 02810 if (-1 == extAioInit((const char *) 0)) 02811 { 02812 diagnostics("can't initialize AIO hardware\n"); 02813 return -1; 02814 } 02815 02816 extEncoderSetIndexModel(EXT_ENCODER_INDEX_MODEL_MANUAL); 02817 for (axis = 0; axis < EMCMOT_MAX_AXIS; axis++) 02818 { 02819 rawInput[axis] = 0.0; 02820 rawOutput[axis] = 0.0; 02821 trajPos[axis] = 0.0; 02822 jointPos[axis] = 0.0; 02823 oldJointPos[axis] = 0.0; 02824 oldInput[axis] = 0.0; 02825 02826 waitingForLatch[axis] = 0; 02827 latchFlag[axis] = 0; 02828 saveLatch[axis] = 0.0; 02829 02830 wasOnLimit[axis] = 0; 02831 emcmotStatus->input[axis] = 0.0; 02832 lastInput[axis] = 0.0; 02833 emcmotStatus->output[axis] = 0.0; 02834 02835 extAmpEnable(axis, 02836 ! GET_AXIS_ENABLE_POLARITY(axis)); 02837 } 02838 02839 emcmotStatus->tail = 0; 02840 02841 #ifdef rtlinux 02842 02843 /* get current time as base for starting tasks */ 02844 now = rt_get_time(); 02845 02846 /* init control task */ 02847 rt_task_init(&emcmotTask, /* RT_TASK * */ 02848 emcmotController, /* task code */ 02849 0, /* startup arg to task code */ 02850 RT_TASK_STACK_SIZE, /* task stack size */ 02851 RT_TASK_PRIORITY); /* priority */ 02852 02853 /* make sure we save FP context */ 02854 02855 #if defined(rtlinux_09J) || defined(rtlinux_1_0) 02856 rt_task_use_fp(&emcmotTask, 1); 02857 #else 02858 rt_use_fp(1); 02859 #endif 02860 02861 /* schedule control task to run */ 02862 rt_task_make_periodic(&emcmotTask, 02863 now + SECS_TO_RTIME(0.010), 02864 (RTIME) (RT_TICKS_PER_SEC * 02865 emcmotStatus->servoCycleTime)); 02866 02867 #ifdef STEPPER_MOTORS 02868 02869 /* init stepper motor task */ 02870 rt_task_init(&smTask, /* RT_TASK * */ 02871 smController, /* task code */ 02872 0, /* startup arg to task code */ 02873 SM_TASK_STACK_SIZE, /* task stack size */ 02874 SM_TASK_PRIORITY); /* priority */ 02875 02876 /* make sure we save FP context */ 02877 #if defined(rtlinux_09J) || defined(rtlinux_1_0) 02878 rt_task_use_fp(&smTask, 1); 02879 #endif 02880 02881 /* schedule stepper task to run at twice servo (axis) rate */ 02882 rt_task_make_periodic(&smTask, 02883 now + SECS_TO_RTIME(0.010), 02884 ((RTIME) (RT_TICKS_PER_SEC * 02885 emcmotStatus->servoCycleTime)) >> 1); 02886 02887 #endif 02888 02889 /* FIXME */ 02890 diagnostics("inited emcmot\n"); 02891 02892 #endif /* rtlinux */ 02893 02894 #if defined (rtlinux) && defined (RT_FIFO) 02895 /* create user process handler on control channel */ 02896 rtf_create_handler(EMCMOT_COMMAND_RTF, /* fifo number */ 02897 emcmotCommandHandler /* control message handler */ 02898 ); 02899 02900 #endif /* rtlinux and RT_FIFO */ 02901 return 0; 02902 } |
|
Definition at line 3047 of file emcsegmot.c. 03048 { 03049 int t; 03050 int shm; /* shared mem key override */ 03051 int base; /* shared mem base addr override */ 03052 double delta; /* elapsed time */ 03053 03054 /* trap ^C */ 03055 signal(SIGINT, quit); 03056 03057 /* process command line args */ 03058 if (0 != getArgs(argc, argv)) 03059 { 03060 diagnostics("can't init module-- bad arguments\n"); 03061 exit(1); 03062 } 03063 03064 /* set up data structs */ 03065 if (-1 == init_module()) 03066 { 03067 diagnostics("can't init module-- execution permission problems?\n"); 03068 exit(1); 03069 } 03070 03071 while (! done) 03072 { 03073 delta = etime(); 03074 emcmotController(0); 03075 delta = etime() - delta; 03076 delta = emcmotStatus->servoCycleTime - delta; 03077 03078 if (delta > 0.0) 03079 { 03080 esleep(delta); 03081 } 03082 } 03083 03084 cleanup_module(); 03085 exit(0); 03086 } |
|
Definition at line 2968 of file emcsegmot.c. 02969 { 02970 done = 1; 02971 } |
|
Definition at line 498 of file emcsegmot.c. 00499 { 00500 va_list args; 00501 char error[EMCMOT_ERROR_LEN]; 00502 00503 va_start(args, fmt); 00504 vsprintf(error, fmt, args); 00505 va_end(args); 00506 00507 #if defined (rtlinux) && defined (RT_FIFO) 00508 rtf_put(EMCMOT_ERROR_RTF, error, EMCMOT_ERROR_LEN); 00509 00510 #else 00511 emcmotErrorPut(emcmotError, error); 00512 00513 #endif 00514 } |
|
Definition at line 642 of file emcsegmot.c. 00643 { 00644 static int t; 00645 00646 /* make sure it's not zero */ 00647 if (secs <= 0.0) 00648 { 00649 return; 00650 } 00651 00652 /* compute the interpolation rate as nearest integer to traj/servo*/ 00653 emcmotStatus->interpolationRate = 00654 (int) (emcmotStatus->trajCycleTime / secs + 0.5); 00655 00656 /* set the cubic interpolation rate and PID cycle time */ 00657 for (t = 0; t < EMCMOT_MAX_AXIS; t++) 00658 { 00659 cubicSetInterpolationRate(&cubic[t], emcmotStatus->interpolationRate); 00660 cubicSetSegmentTime(&cubic[t], secs); 00661 #ifndef STEPPER_MOTORS 00662 pidSetCycleTime(&emcmotStatus->pid[t], secs); 00663 #endif 00664 } 00665 00666 /* copy into status out */ 00667 emcmotStatus->servoCycleTime = secs; 00668 } |
|
Definition at line 613 of file emcsegmot.c. 00614 { 00615 static int t; 00616 00617 /* make sure it's not zero */ 00618 if (secs <= 0.0) 00619 { 00620 return; 00621 } 00622 00623 /* compute the interpolation rate as nearest integer to traj/servo*/ 00624 emcmotStatus->interpolationRate = 00625 (int) (secs / emcmotStatus->servoCycleTime + 0.5); 00626 00627 /* set traj planner */ 00628 sqSetCycleTime(&queue, secs); 00629 00630 /* set the free planners, cubic interpolation rate and segment time */ 00631 for (t = 0; t < EMCMOT_MAX_AXIS; t++) 00632 { 00633 tpSetCycleTime(&freeAxis[t], secs); 00634 cubicSetInterpolationRate(&cubic[t], emcmotStatus->interpolationRate); 00635 } 00636 00637 /* copy into status out */ 00638 emcmotStatus->trajCycleTime = secs; 00639 } |
|
Definition at line 455 of file emcsegmot.c. |
|
Definition at line 462 of file emcsegmot.c. |
|
Definition at line 432 of file emcsegmot.c. |
|
Definition at line 2967 of file emcsegmot.c. |
|
Definition at line 297 of file emcsegmot.c. |
|
Definition at line 301 of file emcsegmot.c. |
|
Definition at line 302 of file emcsegmot.c. |
|
Definition at line 298 of file emcsegmot.c. |
|
Definition at line 286 of file emcsegmot.c. |
|
Definition at line 461 of file emcsegmot.c. |
|
Definition at line 431 of file emcsegmot.c. |
|
Definition at line 438 of file emcsegmot.c. |
|
Definition at line 467 of file emcsegmot.c. |
|
Definition at line 225 of file emcsegmot.c. |
|
Definition at line 444 of file emcsegmot.c. |
|
Definition at line 452 of file emcsegmot.c. |
|
Definition at line 458 of file emcsegmot.c. |
|
Definition at line 308 of file emcsegmot.c. |
|
Definition at line 306 of file emcsegmot.c. |
|
Definition at line 307 of file emcsegmot.c. |
|
Definition at line 305 of file emcsegmot.c. |
|
Definition at line 317 of file emcsegmot.c. |
|
Definition at line 318 of file emcsegmot.c. |
|
Definition at line 483 of file emcsegmot.c. |
|
Definition at line 449 of file emcsegmot.c. |
|
Definition at line 448 of file emcsegmot.c. |
|
Definition at line 446 of file emcsegmot.c. |
|
Definition at line 445 of file emcsegmot.c. |
|
Definition at line 447 of file emcsegmot.c. |
|
Definition at line 448 of file emcsegmot.c. |
|
Definition at line 427 of file emcsegmot.c. |
|
Definition at line 436 of file emcsegmot.c. |
|
Definition at line 440 of file emcsegmot.c. |
|
Definition at line 441 of file emcsegmot.c. |
|
Definition at line 482 of file emcsegmot.c. |
|
Definition at line 459 of file emcsegmot.c. |
|
Definition at line 290 of file emcsegmot.c. |
|
Definition at line 476 of file emcsegmot.c. |
|
Definition at line 475 of file emcsegmot.c. |
|
Definition at line 474 of file emcsegmot.c. |
|
Definition at line 428 of file emcsegmot.c. |
|
Definition at line 466 of file emcsegmot.c. |
|
Definition at line 481 of file emcsegmot.c. |
|
Definition at line 443 of file emcsegmot.c. |
|
Definition at line 457 of file emcsegmot.c. |
|
Definition at line 464 of file emcsegmot.c. |
|
Definition at line 313 of file emcsegmot.c. |
|
Definition at line 311 of file emcsegmot.c. |
|
Definition at line 312 of file emcsegmot.c. |