jepler/linuxcnc
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Cleanup of motion code to prepare for deeper modifications

Browse source 
- remove dead code
- fixed comments
- slight reorganisation of source code
- remove unused variables
- convert unnessesary global varables to local

Signed-off-by: Michael Geszkiewicz <micges@wp.pl>
This commit is contained in:
Michael Geszkiewicz 2010-01-02 01:40:41 +01:00 committed by Sebastian Kuzminsky
parent cad8d03ee7
commit 1adc33f11c
10 changed files with 287 additions and 672 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

33
src/emc/motion/command.c
View file

@ -73,15 +73,11 @@
// Mark strings for translation, but defer translation to userspace // Mark strings for translation, but defer translation to userspace
#define _(s) (s) #define _(s) (s)
/* debugging functions */ static int rehomeAll;
extern void print_pose ( EmcPose *pos );
/* kinematics flags */
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags = 0;
/* loops through the active joints and checks if any are not homed */ /* loops through the active joints and checks if any are not homed */
int checkAllHomed(void) { int checkAllHomed(void)
{
int joint_num; int joint_num;
emcmot_joint_t *joint; emcmot_joint_t *joint;
@ -110,7 +106,6 @@ int checkAllHomed(void) {
return 1; return 1;
} }
/* limits_ok() returns 1 if none of the hard limits are set, /* limits_ok() returns 1 if none of the hard limits are set,
0 if any are set. Called on a linear and circular move. */ 0 if any are set. Called on a linear and circular move. */
STATIC int limits_ok(void) STATIC int limits_ok(void)
@ -277,7 +272,6 @@ void clearHomes(int joint_num)
} }
} }
void emcmotSetRotaryUnlock(int axis, int unlock) { void emcmotSetRotaryUnlock(int axis, int unlock) {
*(emcmot_hal_data->joint[axis].unlock) = unlock; *(emcmot_hal_data->joint[axis].unlock) = unlock;
} }
@ -1010,7 +1004,6 @@ void emcmotCommandHandler(void *arg, long period)
break; break;
} }
joint->vel_limit = emcmotCommand->vel; joint->vel_limit = emcmotCommand->vel;
joint->big_vel = 10 * emcmotCommand->vel;
break; break;
case EMCMOT_SET_JOINT_ACC_LIMIT: case EMCMOT_SET_JOINT_ACC_LIMIT:
@ -1266,18 +1259,6 @@ void emcmotCommandHandler(void *arg, long period)
} }
break; break;
case EMCMOT_ENABLE_WATCHDOG:
rtapi_print_msg(RTAPI_MSG_DBG, "ENABLE_WATCHDOG");
/*! \todo Another #if 0 */
#if 0
emcmotDebug->wdEnabling = 1;
emcmotDebug->wdWait = emcmotCommand->wdWait;
if (emcmotDebug->wdWait < 0) {
emcmotDebug->wdWait = 0;
}
#endif
break;
case EMCMOT_JOINT_UNHOME: case EMCMOT_JOINT_UNHOME:
/* unhome the specified joint, or all joints if -1 */ /* unhome the specified joint, or all joints if -1 */
rtapi_print_msg(RTAPI_MSG_DBG, "JOINT_UNHOME"); rtapi_print_msg(RTAPI_MSG_DBG, "JOINT_UNHOME");
@ -1338,14 +1319,6 @@ void emcmotCommandHandler(void *arg, long period)
break; break;
case EMCMOT_DISABLE_WATCHDOG:
rtapi_print_msg(RTAPI_MSG_DBG, "DISABLE_WATCHDOG");
/*! \todo Another #if 0 */
#if 0
emcmotDebug->wdEnabling = 0;
#endif
break;
case EMCMOT_CLEAR_PROBE_FLAGS: case EMCMOT_CLEAR_PROBE_FLAGS:
rtapi_print_msg(RTAPI_MSG_DBG, "CLEAR_PROBE_FLAGS"); rtapi_print_msg(RTAPI_MSG_DBG, "CLEAR_PROBE_FLAGS");
emcmotStatus->probing = 0; emcmotStatus->probing = 0;

249
src/emc/motion/control.c
View file

@ -29,25 +29,66 @@
// Mark strings for translation, but defer translation to userspace // Mark strings for translation, but defer translation to userspace
#define _(s) (s) #define _(s) (s)
/*********************************************************************** /* kinematics flags */
* LOCAL VARIABLE DECLARATIONS * KINEMATICS_FORWARD_FLAGS fflags = 0;
************************************************************************/ KINEMATICS_INVERSE_FLAGS iflags = 0;
/*! \todo FIXME - this is a leftover global, it will eventually go away */ /* 1/servo cycle time */
int rehomeAll;
/* the (nominal) period the last time the motion handler was invoked */
unsigned long last_period = 0;
/* these variables have the servo cycle time and 1/cycle time */
double servo_period;
double servo_freq; double servo_freq;
/*! \todo FIXME - debugging - uncomment the following line to log changes in /*! \todo FIXME - debugging - uncomment the following line to log changes in
JOINT_FLAG and MOTION_FLAG */ JOINT_FLAG and MOTION_FLAG */
// #define WATCH_FLAGS 1 // #define WATCH_FLAGS 1
/* debugging function - it watches a particular variable and
prints a message when the value changes. Right now there are
calls to this scattered throughout this and other files.
To disable them, comment out the following define:
*/
// #define ENABLE_CHECK_STUFF
#ifdef ENABLE_CHECK_STUFF
void check_stuff(const char *location)
{
static char *target, old = 0xFF;
/*! \todo Another #if 0 */
#if 0
/* kludge to look at emcmotDebug->enabling and emcmotStatus->motionFlag
at the same time - we simply use a high bit of the flags to
hold "enabling" */
short tmp;
if ( emcmotDebug->enabling )
tmp = 0x1000;
else
tmp = 0x0;
tmp |= emcmotStatus->motionFlag;
target = &tmp;
/* end of kluge */
#endif
target = (emcmot_hal_data->enable);
if ( old != *target ) {
rtapi_print ( "%d: watch value %02X (%s)\n", emcmotStatus->heartbeat, *target, location );
old = *target;
}
}
#else /* make it disappear */
void check_stuff(const char *location)
{
/* do nothing (I wonder if gcc is smart
enough to optimize the calls away?) */
}
#endif /* ENABLE_CHECK_STUFF */
/***********************************************************************
* LOCAL VARIABLE DECLARATIONS *
************************************************************************/
/* the (nominal) period the last time the motion handler was invoked */
static unsigned long last_period = 0;
/* servo cycle time */
static double servo_period;
/*********************************************************************** /***********************************************************************
* LOCAL FUNCTION PROTOTYPES * * LOCAL FUNCTION PROTOTYPES *
@ -80,7 +121,6 @@ static void do_forward_kins(void);
cartesian feedback position is latched when the probe fires, and it cartesian feedback position is latched when the probe fires, and it
should be based on the feedback read in on this servo cycle. should be based on the feedback read in on this servo cycle.
*/ */
static void process_probe_inputs(void); static void process_probe_inputs(void);
/* 'check_for_faults()' is responsible for detecting fault conditions /* 'check_for_faults()' is responsible for detecting fault conditions
@ -132,7 +172,6 @@ static void handle_jogwheels(void);
*/ */
static void get_pos_cmds(long period); static void get_pos_cmds(long period);
/* 'compute_screw_comp()' is responsible for calculating backlash and /* 'compute_screw_comp()' is responsible for calculating backlash and
lead screw error compensation. (Leadscrew error compensation is lead screw error compensation. (Leadscrew error compensation is
a more sophisticated version that includes backlash comp.) It uses a more sophisticated version that includes backlash comp.) It uses
@ -164,7 +203,6 @@ static void output_to_hal(void);
*/ */
static void update_status(void); static void update_status(void);
/*********************************************************************** /***********************************************************************
* PUBLIC FUNCTION CODE * * PUBLIC FUNCTION CODE *
************************************************************************/ ************************************************************************/
@ -179,7 +217,6 @@ static void update_status(void);
Inactive axes are still calculated, but the PIDs are inhibited and Inactive axes are still calculated, but the PIDs are inhibited and
the amp enable/disable are inhibited the amp enable/disable are inhibited
*/ */
void emcmotController(void *arg, long period) void emcmotController(void *arg, long period)
{ {
static long long int last = 0; static long long int last = 0;
@ -236,100 +273,6 @@ void emcmotController(void *arg, long period)
prototypes" prototypes"
*/ */
static void process_probe_inputs(void) {
static int old_probeVal = 0;
unsigned char probe_type = emcmotStatus->probe_type;
// don't error
char probe_suppress = probe_type & 1;
// trigger when the probe clears, instead of the usual case of triggering when it trips
char probe_whenclears = !!(probe_type & 2);
/* read probe input */
emcmotStatus->probeVal = !!*(emcmot_hal_data->probe_input);
if (emcmotStatus->probing) {
/* check if the probe has been tripped */
if (emcmotStatus->probeVal ^ probe_whenclears) {
/* remember the current position */
emcmotStatus->probedPos = emcmotStatus->carte_pos_fb;
/* stop! */
emcmotStatus->probing = 0;
emcmotStatus->probeTripped = 1;
tpAbort(&emcmotDebug->tp);
/* check if the probe hasn't tripped, but the move finished */
} else if (GET_MOTION_INPOS_FLAG() && tpQueueDepth(&emcmotDebug->tp) == 0) {
/* we are already stopped, but we need to remember the current
position here, because it will still be queried */
emcmotStatus->probedPos = emcmotStatus->carte_pos_fb;
emcmotStatus->probing = 0;
if (probe_suppress) {
emcmotStatus->probeTripped = 0;
} else if(probe_whenclears) {
reportError(_("G38.4 move finished without breaking contact."));
SET_MOTION_ERROR_FLAG(1);
} else {
reportError(_("G38.2 move finished without making contact."));
SET_MOTION_ERROR_FLAG(1);
}
}
} else if (!old_probeVal && emcmotStatus->probeVal) {
// not probing, but we have a rising edge on the probe.
// this could be expensive if we don't stop.
int i;
int aborted = 0;
if(!GET_MOTION_INPOS_FLAG() && tpQueueDepth(&emcmotDebug->tp) &&
tpGetExecId(&emcmotDebug->tp) <= 0) {
// running an MDI command
tpAbort(&emcmotDebug->tp);
reportError(_("Probe tripped during non-probe MDI command."));
SET_MOTION_ERROR_FLAG(1);
}
// When using same input for probe and Z home (some plasma systems),
// we don't want probe errors when homing.
// We also may not want the error while jogging
for(i=0; i<num_joints; i++) {
emcmot_joint_t *joint = &joints[i];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
/* if joint is not active, skip it */
continue;
}
// inhibit_probe_home_error is set by [TRAJ]->NO_PROBE_HOME_ERROR in the ini file
if (!emcmotConfig->inhibit_probe_home_error) {
// abort any homing
if(GET_JOINT_HOMING_FLAG(joint)) {
joint->home_state = HOME_ABORT;
joint->free_tp_enable = 0;
aborted=1;
}
}
// inhibit_probe_jog_error is set by [TRAJ]->NO_PROBE_JOG_ERROR in the ini file
if (!emcmotConfig->inhibit_probe_jog_error) {
// abort any jogs - since homing uses free_tp, make sure we are not homing
if((joint->free_tp_enable == 1) && !GET_JOINT_HOMING_FLAG(joint)) {
joint->free_tp_enable = 0;
aborted=2;
}
}
}
if(aborted == 1) {
reportError(_("Probe tripped during homing motion."));
}
if(aborted == 2) {
reportError(_("Probe tripped during a jog."));
}
}
old_probeVal = emcmotStatus->probeVal;
}
static void process_inputs(void) static void process_inputs(void)
{ {
int joint_num; int joint_num;
@ -607,6 +550,92 @@ static void do_forward_kins(void)
} }
} }
static void process_probe_inputs(void)
{
static int old_probeVal = 0;
unsigned char probe_type = emcmotStatus->probe_type;
// don't error
char probe_suppress = probe_type & 1;
// trigger when the probe clears, instead of the usual case of triggering when it trips
char probe_whenclears = !!(probe_type & 2);
/* read probe input */
emcmotStatus->probeVal = !!*(emcmot_hal_data->probe_input);
if (emcmotStatus->probing) {
/* check if the probe has been tripped */
if (emcmotStatus->probeVal ^ probe_whenclears) {
/* remember the current position */
emcmotStatus->probedPos = emcmotStatus->carte_pos_fb;
/* stop! */
emcmotStatus->probing = 0;
emcmotStatus->probeTripped = 1;
tpAbort(&emcmotDebug->tp);
/* check if the probe hasn't tripped, but the move finished */
} else if (GET_MOTION_INPOS_FLAG() && tpQueueDepth(&emcmotDebug->tp) == 0) {
/* we are already stopped, but we need to remember the current
position here, because it will still be queried */
emcmotStatus->probedPos = emcmotStatus->carte_pos_fb;
emcmotStatus->probing = 0;
if (probe_suppress) {
emcmotStatus->probeTripped = 0;
} else if(probe_whenclears) {
reportError(_("G38.4 move finished without breaking contact."));
SET_MOTION_ERROR_FLAG(1);
} else {
reportError(_("G38.2 move finished without making contact."));
SET_MOTION_ERROR_FLAG(1);
}
}
} else if (!old_probeVal && emcmotStatus->probeVal) {
// not probing, but we have a rising edge on the probe.
// this could be expensive if we don't stop.
int i;
int aborted = 0;
if(!GET_MOTION_INPOS_FLAG() && tpQueueDepth(&emcmotDebug->tp) &&
tpGetExecId(&emcmotDebug->tp) <= 0) {
// running an MDI command
tpAbort(&emcmotDebug->tp);
reportError(_("Probe tripped during non-probe MDI command."));
SET_MOTION_ERROR_FLAG(1);
}
for(i=0; i<num_joints; i++) {
emcmot_joint_t *joint = &joints[i];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
/* if joint is not active, skip it */
continue;
}
// abort any homing
if(GET_JOINT_HOMING_FLAG(joint)) {
joint->home_state = HOME_ABORT;
aborted=1;
}
// abort any jogs
if(joint->free_tp.enable == 1) {
joint->free_tp.enable = 0;
// since homing uses free_tp, this protection of aborted
// is needed so the user gets the correct error.
if(!aborted) aborted=2;
}
}
if(aborted == 1) {
reportError(_("Probe tripped during homing motion."));
}
if(aborted == 2) {
reportError(_("Probe tripped during a jog."));
}
}
old_probeVal = emcmotStatus->probeVal;
}
static void check_for_faults(void) static void check_for_faults(void)
{ {
int joint_num; int joint_num;

4
src/emc/motion/emcmotcfg.h
View file

@ -55,6 +55,10 @@
#define DEFAULT_MAX_LIMIT 1000 #define DEFAULT_MAX_LIMIT 1000
#define DEFAULT_MIN_LIMIT -1000 #define DEFAULT_MIN_LIMIT -1000
/* default number of motion io pins */
#define DEFAULT_DIO 4
#define DEFAULT_AIO 4
/* size of motion queue /* size of motion queue
* a TC_STRUCT is about 512 bytes so this queue is * a TC_STRUCT is about 512 bytes so this queue is
* about a megabyte. */ * about a megabyte. */

18
src/emc/motion/mot_priv.h
View file

@ -95,7 +95,6 @@ typedef struct {
hal_bit_t *feed_inhibit; /* RPI: set TRUE to stop motion (non maskable)*/ hal_bit_t *feed_inhibit; /* RPI: set TRUE to stop motion (non maskable)*/
hal_bit_t *motion_enabled; /* RPI: motion enable for all joints */ hal_bit_t *motion_enabled; /* RPI: motion enable for all joints */
hal_bit_t *in_position; /* RPI: all joints are in position */ hal_bit_t *in_position; /* RPI: all joints are in position */
// hal_bit_t *inpos_output; /* WPI: all joints are in position (used to power down steppers for example) */
hal_bit_t *coord_mode; /* RPA: TRUE if coord, FALSE if free */ hal_bit_t *coord_mode; /* RPA: TRUE if coord, FALSE if free */
hal_bit_t *teleop_mode; /* RPA: TRUE if teleop mode */ hal_bit_t *teleop_mode; /* RPA: TRUE if teleop mode */
hal_bit_t *coord_error; /* RPA: TRUE if coord mode error */ hal_bit_t *coord_error; /* RPA: TRUE if coord mode error */
@ -186,10 +185,7 @@ typedef struct {
* GLOBAL VARIABLE DECLARATIONS * * GLOBAL VARIABLE DECLARATIONS *
************************************************************************/ ************************************************************************/
/* HAL component ID for motion module */ /* userdefined number of joints. default is EMCMOT_MAX_JOINTS(=9),
extern int mot_comp_id;
/* userdefined number of joints. default is EMCMOT_MAX_JOINTS(=8),
but can be altered at motmod insmod time */ but can be altered at motmod insmod time */
extern int num_joints; extern int num_joints;
@ -206,29 +202,23 @@ extern emcmot_hal_data_t *emcmot_hal_data;
/* pointer to array of joint structs with all joint data */ /* pointer to array of joint structs with all joint data */
/* the actual array may be in shared memory or in kernel space, as /* the actual array may be in shared memory or in kernel space, as
determined by the init code in motion.c determined by the init code in motion.c */
*/
extern emcmot_joint_t *joints; extern emcmot_joint_t *joints;
/* Variable defs */ /* Variable defs */
extern int kinType; extern int kinType;
extern int rehomeAll;
extern int DEBUG_MOTION; extern int DEBUG_MOTION;
extern int EMCMOT_NO_FORWARD_KINEMATICS;
extern KINEMATICS_FORWARD_FLAGS fflags; extern KINEMATICS_FORWARD_FLAGS fflags;
extern KINEMATICS_INVERSE_FLAGS iflags; extern KINEMATICS_INVERSE_FLAGS iflags;
/* these variables have the servo cycle time and 1/cycle time */ /* these variable have the 1/servo cycle time */
extern double servo_period;
extern double servo_freq; extern double servo_freq;
/* Struct pointers */ /* Struct pointers */
extern struct emcmot_struct_t *emcmotStruct; extern struct emcmot_struct_t *emcmotStruct;
extern struct emcmot_command_t *emcmotCommand; extern struct emcmot_command_t *emcmotCommand;
extern struct emcmot_status_t *emcmotStatus; extern struct emcmot_status_t *emcmotStatus;
extern struct emcmot_config_t *emcmotConfig; extern struct emcmot_config_t *emcmotConfig;
extern struct emcmot_debug_t *emcmotDebug; extern struct emcmot_debug_t *emcmotDebug;
extern struct emcmot_internal_t *emcmotInternal;
extern struct emcmot_error_t *emcmotError; extern struct emcmot_error_t *emcmotError;
/*********************************************************************** /***********************************************************************
@ -258,6 +248,8 @@ extern void refresh_jog_limits(emcmot_joint_t *joint);
/* handles 'homed' flags, see command.c for details */ /* handles 'homed' flags, see command.c for details */
extern void clearHomes(int joint_num); extern void clearHomes(int joint_num);
extern void check_stuff(const char *msg);
extern void emcmot_config_change(void); extern void emcmot_config_change(void);
extern void reportError(const char *fmt, ...) __attribute((format(printf,1,2))); /* Use the rtapi_print call */ extern void reportError(const char *fmt, ...) __attribute((format(printf,1,2))); /* Use the rtapi_print call */

591
src/emc/motion/motion.c
View file

@ -27,8 +27,6 @@
* KERNEL MODULE PARAMETERS * * KERNEL MODULE PARAMETERS *
************************************************************************/ ************************************************************************/
static int key = DEFAULT_SHMEM_KEY; /* the shared memory key, default value */
/* module information */ /* module information */
/* register symbols to be modified by insmod /* register symbols to be modified by insmod
see "Linux Device Drivers", Alessandro Rubini, p. 385 see "Linux Device Drivers", Alessandro Rubini, p. 385
@ -37,13 +35,9 @@ MODULE_AUTHOR("Matt Shaver/John Kasunich");
MODULE_DESCRIPTION("Motion Controller for EMC"); MODULE_DESCRIPTION("Motion Controller for EMC");
MODULE_LICENSE("GPL"); MODULE_LICENSE("GPL");
/*! \todo FIXME - find a better way to do this */
int DEBUG_MOTION = 0;
RTAPI_MP_INT(DEBUG_MOTION, "debug motion");
/* RTAPI shmem key - for comms with higher level user space stuff */ /* RTAPI shmem key - for comms with higher level user space stuff */
static int key = DEFAULT_SHMEM_KEY; /* the shared memory key, default value */
RTAPI_MP_INT(key, "shared memory key"); RTAPI_MP_INT(key, "shared memory key");
static long base_period_nsec = 0; /* fastest thread period */ static long base_period_nsec = 0; /* fastest thread period */
RTAPI_MP_LONG(base_period_nsec, "fastest thread period (nsecs)"); RTAPI_MP_LONG(base_period_nsec, "fastest thread period (nsecs)");
int base_thread_fp = 0; /* default is no floating point in base thread */ int base_thread_fp = 0; /* default is no floating point in base thread */
@ -54,9 +48,9 @@ static long traj_period_nsec = 0; /* trajectory planner period */
RTAPI_MP_LONG(traj_period_nsec, "trajectory planner period (nsecs)"); RTAPI_MP_LONG(traj_period_nsec, "trajectory planner period (nsecs)");
int num_joints = EMCMOT_MAX_JOINTS; /* default number of joints present */ int num_joints = EMCMOT_MAX_JOINTS; /* default number of joints present */
RTAPI_MP_INT(num_joints, "number of joints"); RTAPI_MP_INT(num_joints, "number of joints");
int num_dio = 4; /* default number of motion synched DIO */ int num_dio = DEFAULT_DIO; /* default number of motion synched DIO */
RTAPI_MP_INT(num_dio, "number of digital inputs/outputs"); RTAPI_MP_INT(num_dio, "number of digital inputs/outputs");
int num_aio = 4; /* default number of motion synched AIO */ int num_aio = DEFAULT_AIO; /* default number of motion synched AIO */
RTAPI_MP_INT(num_aio, "number of analog inputs/outputs"); RTAPI_MP_INT(num_aio, "number of analog inputs/outputs");
/*********************************************************************** /***********************************************************************
@ -74,7 +68,6 @@ emcmot_joint_t *joints = 0;
emcmot_joint_t joint_array[EMCMOT_MAX_JOINTS]; emcmot_joint_t joint_array[EMCMOT_MAX_JOINTS];
#endif #endif
int mot_comp_id; /* component ID for motion module */
int kinType = 0; int kinType = 0;
/* /*
@ -90,13 +83,11 @@ int kinType = 0;
emcmotError points to emcmotStruct->error, and emcmotError points to emcmotStruct->error, and
*/ */
emcmot_struct_t *emcmotStruct = 0; emcmot_struct_t *emcmotStruct = 0;
/* ptrs to either buffered copies or direct memory for /* ptrs to either buffered copies or direct memory for command and status */
command and status */
struct emcmot_command_t *emcmotCommand = 0; struct emcmot_command_t *emcmotCommand = 0;
struct emcmot_status_t *emcmotStatus = 0; struct emcmot_status_t *emcmotStatus = 0;
struct emcmot_config_t *emcmotConfig = 0; struct emcmot_config_t *emcmotConfig = 0;
struct emcmot_debug_t *emcmotDebug = 0; struct emcmot_debug_t *emcmotDebug = 0;
struct emcmot_internal_t *emcmotInternal = 0;
struct emcmot_error_t *emcmotError = 0; /* unused for RT_FIFO */ struct emcmot_error_t *emcmotError = 0; /* unused for RT_FIFO */
/*********************************************************************** /***********************************************************************
@ -106,6 +97,8 @@ struct emcmot_error_t *emcmotError = 0; /* unused for RT_FIFO */
/* RTAPI shmem ID - for comms with higher level user space stuff */ /* RTAPI shmem ID - for comms with higher level user space stuff */
static int emc_shmem_id; /* the shared memory ID */ static int emc_shmem_id; /* the shared memory ID */
static int mot_comp_id; /* component ID for motion module */
/*********************************************************************** /***********************************************************************
* LOCAL FUNCTION PROTOTYPES * * LOCAL FUNCTION PROTOTYPES *
************************************************************************/ ************************************************************************/
@ -185,22 +178,22 @@ int rtapi_app_main(void)
} }
if (( num_joints < 1 ) || ( num_joints > EMCMOT_MAX_JOINTS )) { if (( num_joints < 1 ) || ( num_joints > EMCMOT_MAX_JOINTS )) {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: num_joints is %d, must be between 1 and %d\n"), _("MOTION: num_joints is %d, must be between 1 and %d\n"), num_joints, EMCMOT_MAX_JOINTS);
num_joints, EMCMOT_MAX_JOINTS); hal_exit(mot_comp_id);
return -1; return -1;
} }
if (( num_dio < 1 ) || ( num_dio > EMCMOT_MAX_DIO )) { if (( num_dio < 1 ) || ( num_dio > EMCMOT_MAX_DIO )) {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: num_dio is %d, must be between 1 and %d\n"), _("MOTION: num_dio is %d, must be between 1 and %d\n"), num_dio, EMCMOT_MAX_DIO);
num_dio, EMCMOT_MAX_DIO); hal_exit(mot_comp_id);
return -1; return -1;
} }
if (( num_aio < 1 ) || ( num_aio > EMCMOT_MAX_AIO )) { if (( num_aio < 1 ) || ( num_aio > EMCMOT_MAX_AIO )) {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: num_aio is %d, must be between 1 and %d\n"), _("MOTION: num_aio is %d, must be between 1 and %d\n"), num_aio, EMCMOT_MAX_AIO);
num_aio, EMCMOT_MAX_AIO); hal_exit(mot_comp_id);
return -1; return -1;
} }
@ -215,8 +208,7 @@ int rtapi_app_main(void)
/* allocate/initialize user space comm buffers (cmd/status/err) */ /* allocate/initialize user space comm buffers (cmd/status/err) */
retval = init_comm_buffers(); retval = init_comm_buffers();
if (retval != 0) { if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_comm_buffers() failed\n"));
_("MOTION: init_comm_buffers() failed\n"));
hal_exit(mot_comp_id); hal_exit(mot_comp_id);
return -1; return -1;
} }
@ -283,25 +275,24 @@ static int init_hal_io(void)
/* allocate shared memory for machine data */ /* allocate shared memory for machine data */
emcmot_hal_data = hal_malloc(sizeof(emcmot_hal_data_t)); emcmot_hal_data = hal_malloc(sizeof(emcmot_hal_data_t));
if (emcmot_hal_data == 0) { if (emcmot_hal_data == 0) {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: emcmot_hal_data malloc failed\n"));
_("MOTION: emcmot_hal_data malloc failed\n"));
return -1; return -1;
} }
/* export machine wide hal pins */ /* export machine wide hal pins */
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->probe_input), mot_comp_id, "motion.probe-input")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->probe_input), mot_comp_id, "motion.probe-input")) != 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IO, &(emcmot_hal_data->spindle_index_enable), mot_comp_id, "motion.spindle-index-enable")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IO, &(emcmot_hal_data->spindle_index_enable), mot_comp_id, "motion.spindle-index-enable")) != 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_on), mot_comp_id, "motion.spindle-on")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_on), mot_comp_id, "motion.spindle-on")) != 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_forward), mot_comp_id, "motion.spindle-forward")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_forward), mot_comp_id, "motion.spindle-forward")) != 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_reverse), mot_comp_id, "motion.spindle-reverse")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_reverse), mot_comp_id, "motion.spindle-reverse")) != 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_brake), mot_comp_id, "motion.spindle-brake")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_brake), mot_comp_id, "motion.spindle-brake")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out), mot_comp_id, "motion.spindle-speed-out")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out), mot_comp_id, "motion.spindle-speed-out")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_abs), mot_comp_id, "motion.spindle-speed-out-abs")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_abs), mot_comp_id, "motion.spindle-speed-out-abs")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps), mot_comp_id, "motion.spindle-speed-out-rps")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps), mot_comp_id, "motion.spindle-speed-out-rps")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps_abs), mot_comp_id, "motion.spindle-speed-out-rps-abs")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps_abs), mot_comp_id, "motion.spindle-speed-out-rps-abs")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_cmd_rps), mot_comp_id, "motion.spindle-speed-cmd-rps")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_cmd_rps), mot_comp_id, "motion.spindle-speed-cmd-rps")) != 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_inhibit), mot_comp_id, "motion.spindle-inhibit")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_inhibit), mot_comp_id, "motion.spindle-inhibit")) != 0) goto error;
*(emcmot_hal_data->spindle_inhibit) = 0; *(emcmot_hal_data->spindle_inhibit) = 0;
// spindle orient pins // spindle orient pins
@ -315,246 +306,90 @@ static int init_hal_io(void)
*(emcmot_hal_data->spindle_orient_mode) = 0; *(emcmot_hal_data->spindle_orient_mode) = 0;
*(emcmot_hal_data->spindle_orient) = 0; *(emcmot_hal_data->spindle_orient) = 0;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_revs), mot_comp_id, "motion.spindle-revs")) != 0) goto error;
// if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->inpos_output), mot_comp_id, "motion.motion-inpos")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_speed_in), mot_comp_id, "motion.spindle-speed-in")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_revs), mot_comp_id, "motion.spindle-revs")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_speed_in), mot_comp_id, "motion.spindle-speed-in")) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) != 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_hold), mot_comp_id, "motion.feed-hold")) != 0) goto error;
*emcmot_hal_data->spindle_is_atspeed = 1; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_inhibit), mot_comp_id, "motion.feed-inhibit")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->enable), mot_comp_id, "motion.enable")) != 0) goto error;
*(emcmot_hal_data->adaptive_feed) = 1.0;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_hold), mot_comp_id, "motion.feed-hold")) < 0) goto error;
*(emcmot_hal_data->feed_hold) = 0;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_inhibit), mot_comp_id, "motion.feed-inhibit")) < 0) goto error;
*(emcmot_hal_data->feed_inhibit) = 0;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->enable), mot_comp_id, "motion.enable")) < 0) goto error;
/* export motion-synched digital output pins */ /* export motion-synched digital output pins */
/* export motion digital input pins */ /* export motion digital input pins */
for (n = 0; n < num_dio; n++) { for (n = 0; n < num_dio; n++) {
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->synch_do[n]), mot_comp_id, "motion.digital-out-%02d", n)) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->synch_do[n]), mot_comp_id, "motion.digital-out-%02d", n)) != 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->synch_di[n]), mot_comp_id, "motion.digital-in-%02d", n)) < 0) goto error; if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->synch_di[n]), mot_comp_id, "motion.digital-in-%02d", n)) != 0) goto error;
} }
/* export motion-synched analog output pins */
/* export motion analog input pins */ /* export motion analog input pins */
for (n = 0; n < num_aio; n++) { for (n = 0; n < num_aio; n++) {
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->analog_output[n]), mot_comp_id, "motion.analog-out-%02d", n)) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->analog_output[n]), mot_comp_id, "motion.analog-out-%02d", n)) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->analog_input[n]), mot_comp_id, "motion.analog-in-%02d", n)) < 0) goto error; if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->analog_input[n]), mot_comp_id, "motion.analog-in-%02d", n)) != 0) goto error;
} }
/* export machine wide hal parameters */ /* export machine wide hal pins */
retval = if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->motion_enabled), mot_comp_id, "motion.motion-enabled")) != 0) goto error;
hal_pin_bit_new("motion.motion-enabled", HAL_OUT, &(emcmot_hal_data->motion_enabled), if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->in_position), mot_comp_id, "motion.in-position")) != 0) goto error;
mot_comp_id); if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->motion_type), mot_comp_id, "motion.motion-type")) != 0) goto error;
if (retval != 0) { if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->coord_mode), mot_comp_id, "motion.coord-mode")) != 0) goto error;
return retval; if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->teleop_mode), mot_comp_id, "motion.teleop-mode")) != 0) goto error;
} if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->coord_error), mot_comp_id, "motion.coord-error")) != 0) goto error;
retval = if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->on_soft_limit), mot_comp_id, "motion.on-soft-limit")) != 0) goto error;
hal_pin_bit_new("motion.in-position", HAL_OUT, &(emcmot_hal_data->in_position), if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->current_vel), mot_comp_id, "motion.current-vel")) != 0) goto error;
mot_comp_id); if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->requested_vel), mot_comp_id, "motion.requested-vel")) != 0) goto error;
if (retval != 0) { if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->distance_to_go), mot_comp_id, "motion.distance-to-go")) != 0) goto error;
return retval; if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->program_line), mot_comp_id, "motion.program-line")) != 0) goto error;
}
retval =
hal_pin_s32_new("motion.motion-type", HAL_OUT, &(emcmot_hal_data->motion_type),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.coord-mode", HAL_OUT, &(emcmot_hal_data->coord_mode),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.teleop-mode", HAL_OUT, &(emcmot_hal_data->teleop_mode),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.coord-error", HAL_OUT, &(emcmot_hal_data->coord_error),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.on-soft-limit", HAL_OUT, &(emcmot_hal_data->on_soft_limit),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_new("motion.current-vel", HAL_OUT, &(emcmot_hal_data->current_vel),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_new("motion.requested-vel", HAL_OUT, &(emcmot_hal_data->requested_vel),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_new("motion.distance-to-go", HAL_OUT, &(emcmot_hal_data->distance_to_go),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_s32_new("motion.program-line", HAL_OUT, &(emcmot_hal_data->program_line),
mot_comp_id);
if (retval != 0) {
return retval;
}
/* export debug parameters */ /* export debug parameters */
/* these can be used to view any internal variable, simply change a line /* these can be used to view any internal variable, simply change a line
in control.c:output_to_hal() and recompile */ in control.c:output_to_hal() and recompile */
retval = if ((retval = hal_param_bit_newf(HAL_RO, &(emcmot_hal_data->debug_bit_0), mot_comp_id, "motion.debug-bit-0")) != 0) goto error;
hal_param_bit_new("motion.debug-bit-0", HAL_RO, &(emcmot_hal_data->debug_bit_0), if ((retval = hal_param_bit_newf(HAL_RO, &(emcmot_hal_data->debug_bit_1), mot_comp_id, "motion.debug-bit-1")) != 0) goto error;
mot_comp_id); if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->debug_float_0), mot_comp_id, "motion.debug-float-0")) != 0) goto error;
if (retval != 0) { if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->debug_float_1), mot_comp_id, "motion.debug-float-1")) != 0) goto error;
return retval; if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->debug_float_2), mot_comp_id, "motion.debug-float-2")) != 0) goto error;
} if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->debug_float_3), mot_comp_id, "motion.debug-float-3")) != 0) goto error;
retval = if ((retval = hal_param_s32_newf(HAL_RO, &(emcmot_hal_data->debug_s32_0), mot_comp_id, "motion.debug-s32-0")) != 0) goto error;
hal_param_bit_new("motion.debug-bit-1", HAL_RO, &(emcmot_hal_data->debug_bit_1), if ((retval = hal_param_s32_newf(HAL_RO, &(emcmot_hal_data->debug_s32_1), mot_comp_id, "motion.debug-s32-1")) != 0) goto error;
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-0", HAL_RO, &(emcmot_hal_data->debug_float_0),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-1", HAL_RO, &(emcmot_hal_data->debug_float_1),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-2", HAL_RO, &(emcmot_hal_data->debug_float_2),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-3", HAL_RO, &(emcmot_hal_data->debug_float_3),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_s32_new("motion.debug-s32-0", HAL_RO, &(emcmot_hal_data->debug_s32_0),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_s32_new("motion.debug-s32-1", HAL_RO, &(emcmot_hal_data->debug_s32_1),
mot_comp_id);
if (retval != 0) {
return retval;
}
// FIXME - debug only, remove later // FIXME - debug only, remove later
// export HAL parameters for some trajectory planner internal variables // export HAL parameters for some trajectory planner internal variables
// so they can be scoped // so they can be scoped
retval = if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->traj_pos_out), mot_comp_id, "traj.pos_out")) != 0) goto error;
hal_param_float_new("traj.pos_out", HAL_RO, &(emcmot_hal_data->traj_pos_out), if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->traj_vel_out), mot_comp_id, "traj.vel_out")) != 0) goto error;
mot_comp_id); if ((retval = hal_param_u32_newf(HAL_RO, &(emcmot_hal_data->traj_active_tc), mot_comp_id, "traj.active_tc")) != 0) goto error;
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("traj.vel_out", HAL_RO, &(emcmot_hal_data->traj_vel_out),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_u32_new("traj.active_tc", HAL_RO, &(emcmot_hal_data->traj_active_tc),
mot_comp_id);
if (retval != 0) {
return retval;
}
for ( n = 0 ; n < 4 ; n++ ) { for ( n = 0 ; n < 4 ; n++ ) {
retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_pos[n]), mot_comp_id, "tc.%d.pos", n); if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_pos[n]), mot_comp_id, "tc.%d.pos", n)) != 0) goto error;
if (retval != 0) { if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_vel[n]), mot_comp_id, "tc.%d.vel", n)) != 0) goto error;
return retval; if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_acc[n]), mot_comp_id, "tc.%d.acc", n)) != 0) goto error;
}
retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_vel[n]), mot_comp_id, "tc.%d.vel", n);
if (retval != 0) {
return retval;
}
retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_acc[n]), mot_comp_id, "tc.%d.acc", n);
if (retval != 0) {
return retval;
}
} }
// end of exporting trajectory planner internals // end of exporting trajectory planner internals
// export timing related HAL parameters so they can be scoped // export timing related HAL parameters so they can be scoped
retval = if ((retval = hal_param_u32_newf(HAL_RO, &(emcmot_hal_data->last_period), mot_comp_id, "motion.servo.last-period")) != 0) goto error;
hal_param_u32_new("motion.servo.last-period", HAL_RO, &(emcmot_hal_data->last_period), mot_comp_id);
if (retval != 0) {
return retval;
}
#ifdef HAVE_CPU_KHZ #ifdef HAVE_CPU_KHZ
retval = if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->last_period_ns), mot_comp_id, "motion.servo.last-period-ns")) != 0) goto error;
hal_param_float_new("motion.servo.last-period-ns", HAL_RO, &(emcmot_hal_data->last_period_ns), mot_comp_id);
if (retval != 0) {
return retval;
}
#endif #endif
if ((retval = hal_param_u32_newf(HAL_RO, &(emcmot_hal_data->overruns), mot_comp_id, "motion.servo.overruns")) != 0) goto error;
retval = hal_pin_float_new("motion.tooloffset.x", HAL_OUT, &(emcmot_hal_data->tooloffset_x), mot_comp_id); if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_x), mot_comp_id, "motion.tooloffset.x")) != 0) goto error;
if (retval != 0) { if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_y), mot_comp_id, "motion.tooloffset.y")) != 0) goto error;
return retval; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_z), mot_comp_id, "motion.tooloffset.z")) != 0) goto error;
} if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_a), mot_comp_id, "motion.tooloffset.a")) != 0) goto error;
retval = hal_pin_float_new("motion.tooloffset.y", HAL_OUT, &(emcmot_hal_data->tooloffset_y), mot_comp_id); if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_b), mot_comp_id, "motion.tooloffset.b")) != 0) goto error;
if (retval != 0) { if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_c), mot_comp_id, "motion.tooloffset.c")) != 0) goto error;
return retval; if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_u), mot_comp_id, "motion.tooloffset.u")) != 0) goto error;
} if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_v), mot_comp_id, "motion.tooloffset.v")) != 0) goto error;
retval = hal_pin_float_new("motion.tooloffset.z", HAL_OUT, &(emcmot_hal_data->tooloffset_z), mot_comp_id); if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_w), mot_comp_id, "motion.tooloffset.w")) != 0) goto error;
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.a", HAL_OUT, &(emcmot_hal_data->tooloffset_a), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.b", HAL_OUT, &(emcmot_hal_data->tooloffset_b), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.c", HAL_OUT, &(emcmot_hal_data->tooloffset_c), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.u", HAL_OUT, &(emcmot_hal_data->tooloffset_u), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.v", HAL_OUT, &(emcmot_hal_data->tooloffset_v), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.w", HAL_OUT, &(emcmot_hal_data->tooloffset_w), mot_comp_id);
if (retval != 0) {
return retval;
}
/* initialize machine wide pins and parameters */ /* initialize machine wide pins and parameters */
*emcmot_hal_data->spindle_is_atspeed = 1;
*(emcmot_hal_data->adaptive_feed) = 1.0;
*(emcmot_hal_data->feed_hold) = 0;
*(emcmot_hal_data->feed_inhibit) = 0;
*(emcmot_hal_data->probe_input) = 0; *(emcmot_hal_data->probe_input) = 0;
/* default value of enable is TRUE, so simple machines /* default value of enable is TRUE, so simple machines
can leave it disconnected */ can leave it disconnected */
@ -598,13 +433,10 @@ static int init_hal_io(void)
/* export all vars */ /* export all vars */
retval = export_joint(n, joint_data); retval = export_joint(n, joint_data);
if (retval != 0) { if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: joint %d pin/param export failed\n"), n);
_("MOTION: joint %d pin/param export failed\n"), n);
return -1; return -1;
} }
/* init axis pins and parameters */ /* init axis pins and parameters */
/* FIXME - struct members are in a state of flux - make sure to
update this - most won't need initing anyway */
*(joint_data->amp_enable) = 0; *(joint_data->amp_enable) = 0;
*(joint_data->home_state) = 0; *(joint_data->home_state) = 0;
/* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now, /* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now,
@ -631,179 +463,49 @@ static int export_joint(int num, joint_hal_t * addr)
msg = rtapi_get_msg_level(); msg = rtapi_get_msg_level();
rtapi_set_msg_level(RTAPI_MSG_WARN); rtapi_set_msg_level(RTAPI_MSG_WARN);
/* export joint pins */ //FIXME-AJ: changing these will bork configs, still we should do it /* export joint pins */
retval = if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->coarse_pos_cmd), mot_comp_id, "joint.%d.coarse-pos-cmd", num)) != 0) return retval;
hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_cmd), mot_comp_id, "joint.%d.pos-cmd", num); if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_cmd), mot_comp_id, "joint.%d.pos-cmd", num)) != 0) return retval;
if (retval != 0) { if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_fb), mot_comp_id, "joint.%d.pos-fb", num)) != 0) return retval;
return retval; if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->motor_pos_cmd), mot_comp_id, "joint.%d.motor-pos-cmd", num)) != 0) return retval;
} if ((retval = hal_pin_float_newf(HAL_IN, &(addr->motor_pos_fb), mot_comp_id, "joint.%d.motor-pos-fb", num)) != 0) return retval;
retval = if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->motor_offset), mot_comp_id, "joint.%d.motor-offset", num)) != 0) return retval;
hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_fb), mot_comp_id, "joint.%d.pos-fb", num); if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->pos_lim_sw), mot_comp_id, "joint.%d.pos-lim-sw-in", num)) != 0) return retval;
if (retval != 0) { if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->neg_lim_sw), mot_comp_id, "joint.%d.neg-lim-sw-in", num)) != 0) return retval;
return retval; if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->home_sw), mot_comp_id, "joint.%d.home-sw-in", num)) != 0) return retval;
} if ((retval = hal_pin_bit_newf(HAL_IO, &(addr->index_enable), mot_comp_id, "joint.%d.index_enable", num)) != 0) return retval;
retval = if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->amp_enable), mot_comp_id, "joint.%d.amp-enable-out", num)) != 0) return retval;
hal_pin_float_newf(HAL_OUT, &(addr->motor_pos_cmd), mot_comp_id, "joint.%d.motor-pos-cmd", num); if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->amp_fault), mot_comp_id, "joint.%d.amp-fault-in", num)) != 0) return retval;
if (retval != 0) { if ((retval = hal_pin_s32_newf(HAL_IN, &(addr->jog_counts), mot_comp_id, "joint.%d.jog-counts", num)) != 0) return retval;
return retval; if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_enable), mot_comp_id, "joint.%d.jog-enable", num)) != 0) return retval;
} if ((retval = hal_pin_float_newf(HAL_IN, &(addr->jog_scale), mot_comp_id, "joint.%d.jog-scale", num)) != 0) return retval;
retval = if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->joint_vel_cmd), mot_comp_id, "joint.%d.vel-cmd", num)) != 0) return retval;
hal_pin_float_newf(HAL_OUT, &(addr->motor_offset), mot_comp_id, "joint.%d.motor-offset", num); if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->backlash_corr), mot_comp_id, "joint.%d.backlash-corr", num)) != 0) return retval;
if (retval != 0) { if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->backlash_filt), mot_comp_id, "joint.%d.backlash-filt", num)) != 0) return retval;
return retval; if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->backlash_vel), mot_comp_id, "joint.%d.backlash-vel", num)) != 0) return retval;
} if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->f_error), mot_comp_id, "joint.%d.f-error", num)) != 0) return retval;
retval = if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->f_error_lim), mot_comp_id, "joint.%d.f-error-lim", num)) != 0) return retval;
hal_pin_float_newf(HAL_IN, &(addr->motor_pos_fb), mot_comp_id, "joint.%d.motor-pos-fb", num); if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->free_pos_cmd), mot_comp_id, "joint.%d.free-pos-cmd", num)) != 0) return retval;
if (retval != 0) { if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->free_vel_lim), mot_comp_id, "joint.%d.free-vel-lim", num)) != 0) return retval;
return retval; if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->free_tp_enable), mot_comp_id, "joint.%d.free-tp-enable", num)) != 0) return retval;
} if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->kb_jog_active), mot_comp_id, "joint.%d.kb-jog-active", num)) != 0) return retval;
retval = hal_pin_bit_newf(HAL_IN, &(addr->pos_lim_sw), mot_comp_id, "joint.%d.pos-lim-sw-in", num); if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->wheel_jog_active), mot_comp_id, "joint.%d.wheel-jog-active", num)) != 0) return retval;
if (retval != 0) { if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->in_position), mot_comp_id, "joint.%d.in-position", num)) != 0) return retval;
return retval; if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->phl), mot_comp_id, "joint.%d.pos-hard-limit", num)) != 0) return retval;
} if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->nhl), mot_comp_id, "joint.%d.neg-hard-limit", num)) != 0) return retval;
retval = hal_pin_bit_newf(HAL_IN, &(addr->neg_lim_sw), mot_comp_id, "joint.%d.neg-lim-sw-in", num); if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->active), mot_comp_id, "joint.%d.active", num)) != 0) return retval;
if (retval != 0) { if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->error), mot_comp_id, "joint.%d.error", num)) != 0) return retval;
return retval; if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->f_errored), mot_comp_id, "joint.%d.f-errored", num)) != 0) return retval;
} if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->faulted), mot_comp_id, "joint.%d.faulted", num)) != 0) return retval;
retval = hal_pin_bit_newf(HAL_IN, &(addr->home_sw), mot_comp_id, "joint.%d.home-sw-in", num); if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->homed), mot_comp_id, "joint.%d.homed", num)) != 0) return retval;
if (retval != 0) { if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->homing), mot_comp_id, "joint.%d.homing", num)) != 0) return retval;
return retval; if ((retval = hal_pin_s32_newf(HAL_OUT, &(addr->home_state), mot_comp_id, "joint.%d.home-state", num)) != 0) return retval;
} if(num >= 3 && num <= 5) {
retval = hal_pin_bit_newf(HAL_IO, &(addr->index_enable), mot_comp_id, "joint.%d.index-enable", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->amp_enable), mot_comp_id, "joint.%d.amp-enable-out", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->amp_fault), mot_comp_id, "joint.%d.amp-fault-in", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_s32_newf(HAL_IN, &(addr->jog_counts), mot_comp_id, "joint.%d.jog-counts", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_enable), mot_comp_id, "joint.%d.jog-enable", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(addr->jog_scale), mot_comp_id, "joint.%d.jog-scale", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_vel_mode), mot_comp_id, "joint.%d.jog-vel-mode", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->homing), mot_comp_id, "joint.%d.homing", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->coarse_pos_cmd), mot_comp_id, "joint.%d.coarse-pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->joint_vel_cmd), mot_comp_id, "joint.%d.vel-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->backlash_corr), mot_comp_id, "joint.%d.backlash-corr", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->backlash_filt), mot_comp_id, "joint.%d.backlash-filt", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->backlash_vel), mot_comp_id, "joint.%d.backlash-vel", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->f_error), mot_comp_id, "joint.%d.f-error", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->f_error_lim), mot_comp_id, "joint.%d.f-error-lim", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->free_pos_cmd), mot_comp_id, "joint.%d.free-pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->free_vel_lim), mot_comp_id, "joint.%d.free-vel-lim", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->free_tp_enable), mot_comp_id, "joint.%d.free-tp-enable", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->kb_jog_active), mot_comp_id, "joint.%d.kb-jog-active", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->wheel_jog_active), mot_comp_id, "joint.%d.wheel-jog-active", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->active), mot_comp_id, "joint.%d.active", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->in_position), mot_comp_id, "joint.%d.in-position", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->error), mot_comp_id, "joint.%d.error", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->phl), mot_comp_id, "joint.%d.pos-hard-limit", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->nhl), mot_comp_id, "joint.%d.neg-hard-limit", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->homed), mot_comp_id, "joint.%d.homed", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->f_errored), mot_comp_id, "joint.%d.f-errored", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->faulted), mot_comp_id, "joint.%d.faulted", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_s32_newf(HAL_OUT, &(addr->home_state), mot_comp_id, "joint.%d.home-state", num);
if (retval != 0) {
return retval;
}
if(num >=3 && num <= 5) {
// for rotaries only... // for rotaries only...
retval = hal_pin_bit_newf(HAL_OUT, &(addr->unlock), mot_comp_id, "joint.%d.unlock", num); if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->unlock), mot_comp_id, "joint.%d.unlock", num)) != 0) return retval;
if (retval != 0) return retval; if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->is_unlocked), mot_comp_id, "joint.%d.is-unlocked", num)) != 0) return retval;
retval = hal_pin_bit_newf(HAL_IN, &(addr->is_unlocked), mot_comp_id, "joint.%d.is-unlocked", num);
if (retval != 0) return retval;
} }
/* restore saved message level */ /* restore saved message level */
rtapi_set_msg_level(msg); rtapi_set_msg_level(msg);
return 0; return 0;
@ -819,8 +521,7 @@ static int init_comm_buffers(void)
emcmot_joint_t *joint; emcmot_joint_t *joint;
int retval; int retval;
rtapi_print_msg(RTAPI_MSG_INFO, rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() starting...\n");
"MOTION: init_comm_buffers() starting...\n");
emcmotStruct = 0; emcmotStruct = 0;
emcmotDebug = 0; emcmotDebug = 0;
@ -828,9 +529,6 @@ static int init_comm_buffers(void)
emcmotCommand = 0; emcmotCommand = 0;
emcmotConfig = 0; emcmotConfig = 0;
/* record the kinematics type of the machine */
kinType = kinematicsType();
/* allocate and initialize the shared memory structure */ /* allocate and initialize the shared memory structure */
emc_shmem_id = rtapi_shmem_new(key, mot_comp_id, sizeof(emcmot_struct_t)); emc_shmem_id = rtapi_shmem_new(key, mot_comp_id, sizeof(emcmot_struct_t));
if (emc_shmem_id < 0) { if (emc_shmem_id < 0) {
@ -853,7 +551,6 @@ static int init_comm_buffers(void)
emcmotStatus = &emcmotStruct->status; emcmotStatus = &emcmotStruct->status;
emcmotConfig = &emcmotStruct->config; emcmotConfig = &emcmotStruct->config;
emcmotDebug = &emcmotStruct->debug; emcmotDebug = &emcmotStruct->debug;
emcmotInternal = &emcmotStruct->internal;
emcmotError = &emcmotStruct->error; emcmotError = &emcmotStruct->error;
/* init error struct */ /* init error struct */
@ -883,7 +580,6 @@ static int init_comm_buffers(void)
SET_MOTION_TELEOP_FLAG(0); SET_MOTION_TELEOP_FLAG(0);
emcmotDebug->split = 0; emcmotDebug->split = 0;
emcmotStatus->heartbeat = 0; emcmotStatus->heartbeat = 0;
emcmotStatus->computeTime = 0.0;
emcmotConfig->numJoints = num_joints; emcmotConfig->numJoints = num_joints;
ZERO_EMC_POSE(emcmotStatus->carte_pos_cmd); ZERO_EMC_POSE(emcmotStatus->carte_pos_cmd);
@ -907,11 +603,10 @@ static int init_comm_buffers(void)
emcmotStatus->spindle.speed = 0.0; emcmotStatus->spindle.speed = 0.0;
SET_MOTION_INPOS_FLAG(1); SET_MOTION_INPOS_FLAG(1);
SET_MOTION_ENABLE_FLAG(0); SET_MOTION_ENABLE_FLAG(0);
/* record the kinematics type of the machine */
kinType = kinematicsType();
emcmotConfig->kinematics_type = kinType; emcmotConfig->kinematics_type = kinType;
emcmotDebug->oldPos = emcmotStatus->carte_pos_cmd;
ZERO_EMC_POSE(emcmotDebug->oldVel);
emcmot_config_change(); emcmot_config_change();
/* init pointer to joint structs */ /* init pointer to joint structs */
@ -927,7 +622,6 @@ static int init_comm_buffers(void)
joint = &joints[joint_num]; joint = &joints[joint_num];
/* init the config fields with some "reasonable" defaults" */ /* init the config fields with some "reasonable" defaults" */
joint->type = 0; joint->type = 0;
joint->max_pos_limit = 1.0; joint->max_pos_limit = 1.0;
joint->min_pos_limit = -1.0; joint->min_pos_limit = -1.0;
@ -942,6 +636,7 @@ static int init_comm_buffers(void)
joint->home = 0.0; joint->home = 0.0;
joint->home_flags = 0; joint->home_flags = 0;
joint->home_sequence = -1; joint->home_sequence = -1;
joint->home_state = HOME_IDLE;
joint->backlash = 0.0; joint->backlash = 0.0;
joint->comp.entries = 0; joint->comp.entries = 0;
@ -962,8 +657,11 @@ static int init_comm_buffers(void)
joint->comp.array[n].rev_slope = 0.0; joint->comp.array[n].rev_slope = 0.0;
} }
/* init status info */ /* init joint flags */
joint->flag = 0; joint->flag = 0;
SET_JOINT_INPOS_FLAG(joint, 1);
/* init status info */
joint->coarse_pos = 0.0; joint->coarse_pos = 0.0;
joint->pos_cmd = 0.0; joint->pos_cmd = 0.0;
joint->vel_cmd = 0.0; joint->vel_cmd = 0.0;
@ -979,24 +677,6 @@ static int init_comm_buffers(void)
/* init internal info */ /* init internal info */
cubicInit(&(joint->cubic)); cubicInit(&(joint->cubic));
/* init misc other stuff in joint structure */
joint->big_vel = 10.0 * joint->vel_limit;
joint->home_state = 0;
/* init joint flags (reduntant, since flag = 0 */
SET_JOINT_ENABLE_FLAG(joint, 0);
SET_JOINT_ACTIVE_FLAG(joint, 0);
SET_JOINT_NHL_FLAG(joint, 0);
SET_JOINT_PHL_FLAG(joint, 0);
SET_JOINT_INPOS_FLAG(joint, 1);
SET_JOINT_HOMING_FLAG(joint, 0);
SET_JOINT_HOMED_FLAG(joint, 0);
SET_JOINT_FERROR_FLAG(joint, 0);
SET_JOINT_FAULT_FLAG(joint, 0);
SET_JOINT_ERROR_FLAG(joint, 0);
} }
/*! \todo FIXME-- add emcmotError */ /*! \todo FIXME-- add emcmotError */
@ -1130,13 +810,15 @@ static int init_threads(void)
return 0; return 0;
} }
void emcmotSetCycleTime(unsigned long nsec ) { void emcmotSetCycleTime(unsigned long nsec )
{
int servo_mult; int servo_mult;
servo_mult = traj_period_nsec / nsec; servo_mult = traj_period_nsec / nsec;
if(servo_mult < 0) servo_mult = 1; if(servo_mult < 0) servo_mult = 1;
setTrajCycleTime(nsec * 1e-9); setTrajCycleTime(nsec * 1e-9);
setServoCycleTime(nsec * servo_mult * 1e-9); setServoCycleTime(nsec * servo_mult * 1e-9);
} }
/* call this when setting the trajectory cycle time */ /* call this when setting the trajectory cycle time */
static int setTrajCycleTime(double secs) static int setTrajCycleTime(double secs)
{ {
@ -1147,7 +829,6 @@ static int setTrajCycleTime(double secs)
/* make sure it's not zero */ /* make sure it's not zero */
if (secs <= 0.0) { if (secs <= 0.0) {
return -1; return -1;
} }

28
src/emc/motion/motion.h
View file

@ -107,8 +107,6 @@ extern "C" {
EMCMOT_ABORT = 1, /* abort all motion */ EMCMOT_ABORT = 1, /* abort all motion */
EMCMOT_ENABLE, /* enable servos for active joints */ EMCMOT_ENABLE, /* enable servos for active joints */
EMCMOT_DISABLE, /* disable servos for active joints */ EMCMOT_DISABLE, /* disable servos for active joints */
EMCMOT_ENABLE_WATCHDOG, /* enable watchdog sound, parport */
EMCMOT_DISABLE_WATCHDOG, /* enable watchdog sound, parport */
EMCMOT_PAUSE, /* pause motion */ EMCMOT_PAUSE, /* pause motion */
EMCMOT_RESUME, /* resume motion */ EMCMOT_RESUME, /* resume motion */
@ -537,10 +535,6 @@ Suggestion: Split this in to an Error and a Status flag register..
double motor_offset; /* diff between internal and motor pos, used double motor_offset; /* diff between internal and motor pos, used
to set position to zero during homing */ to set position to zero during homing */
int old_jog_counts; /* prior value, used for deltas */ int old_jog_counts; /* prior value, used for deltas */
/* stuff moved from the other structs that might be needed (or might
not!) */
double big_vel; /* used for "debouncing" velocity */
} emcmot_joint_t; } emcmot_joint_t;
/* This structure contains only the "status" data associated with /* This structure contains only the "status" data associated with
@ -556,7 +550,6 @@ Suggestion: Split this in to an Error and a Status flag register..
*/ */
typedef struct { typedef struct {
EMCMOT_JOINT_FLAG flag; /* see above for bit details */ EMCMOT_JOINT_FLAG flag; /* see above for bit details */
double pos_cmd; /* commanded joint position */ double pos_cmd; /* commanded joint position */
double pos_fb; /* position feedback, comp removed */ double pos_fb; /* position feedback, comp removed */
@ -664,7 +657,6 @@ Suggestion: Split this in to an Error and a Status flag register..
unsigned int heartbeat; unsigned int heartbeat;
int config_num; /* incremented whenever configuration int config_num; /* incremented whenever configuration
changed. */ changed. */
double computeTime;
int id; /* id for executing motion */ int id; /* id for executing motion */
int depth; /* motion queue depth */ int depth; /* motion queue depth */
int activeDepth; /* depth of active blend elements */ int activeDepth; /* depth of active blend elements */
@ -679,7 +671,6 @@ Suggestion: Split this in to an Error and a Status flag register..
double vel; /* scalar max vel */ double vel; /* scalar max vel */
double acc; /* scalar max accel */ double acc; /* scalar max accel */
int level;
int motionType; int motionType;
double distance_to_go; /* in this move */ double distance_to_go; /* in this move */
EmcPose dtg; EmcPose dtg;
@ -754,25 +745,6 @@ Suggestion: Split this in to an Error and a Status flag register..
int inhibit_probe_home_error; int inhibit_probe_home_error;
} emcmot_config_t; } emcmot_config_t;
/*********************************
INTERNAL STRUCTURE
*********************************/
/* This is the internal structure. It contains stuff that is used
internally by the motion controller that does not need to be in
shared memory. It will wind up with a lot of the stuff that got
tossed into the debug structure.
FIXME - so far most if the stuff that was tossed in here got
moved back out, maybe don't need it after all?
*/
typedef struct emcmot_internal_t {
unsigned char head; /* flag count for mutex detect */
unsigned char tail; /* flag count for mutex detect */
} emcmot_internal_t;
/* error structure - A ring buffer used to pass formatted printf stings to usr space */ /* error structure - A ring buffer used to pass formatted printf stings to usr space */
typedef struct emcmot_error_t { typedef struct emcmot_error_t {
unsigned char head; /* flag count for mutex detect */ unsigned char head; /* flag count for mutex detect */

18
src/emc/motion/motion_debug.h
View file

@ -60,12 +60,6 @@ typedef struct emcmot_debug_t {
EMC_TELEOP_DATA teleop_data; EMC_TELEOP_DATA teleop_data;
int split; /* number of split command reads */ int split; /* number of split command reads */
/* flag for enabling, disabling watchdog; multiple for down-stepping */
int wdEnabling;
int wdEnabled;
int wdWait;
int wdCount;
unsigned char wdToggle;
/* flag that all active axes are homed */ /* flag that all active axes are homed */
unsigned char allHomed; unsigned char allHomed;
@ -76,21 +70,9 @@ typedef struct emcmot_debug_t {
/*! \todo FIXME-- default is used; dynamic is not honored */ /*! \todo FIXME-- default is used; dynamic is not honored */
TC_STRUCT queueTcSpace[DEFAULT_TC_QUEUE_SIZE + 10]; TC_STRUCT queueTcSpace[DEFAULT_TC_QUEUE_SIZE + 10];
EmcPose oldPos; /* last position, used for vel differencing */
EmcPose oldVel, newVel; /* velocities, used for acc differencing */
EmcPose newAcc; /* differenced acc */
int enabling; /* starts up disabled */ int enabling; /* starts up disabled */
int coordinating; /* starts up in free mode */ int coordinating; /* starts up in free mode */
int teleoperating; /* starts up in free mode */ int teleoperating; /* starts up in free mode */
#if 0
int wasOnLimit; /* non-zero means we already aborted
everything due to a soft limit, and need
not re-abort. It's cleared only when all
limits are cleared. */
int onLimit; /* non-zero means at least one axis is on a
soft limit */
#endif
int overriding; /* non-zero means we've initiated an joint int overriding; /* non-zero means we've initiated an joint
move while overriding limits */ move while overriding limits */

2
src/emc/motion/motion_struct.h
View file

@ -20,8 +20,6 @@
to the RT module from usr space */ to the RT module from usr space */
struct emcmot_status_t status; /* Struct used to store RT status */ struct emcmot_status_t status; /* Struct used to store RT status */
struct emcmot_config_t config; /* Struct used to store RT config */ struct emcmot_config_t config; /* Struct used to store RT config */
struct emcmot_internal_t internal; /*! \todo FIXME - doesn't need to be in
shared memory */
struct emcmot_error_t error; /* ring buffer for error messages */ struct emcmot_error_t error; /* ring buffer for error messages */
struct emcmot_debug_t debug; /* Struct used to store RT status and debug struct emcmot_debug_t debug; /* Struct used to store RT status and debug
data - 2nd largest block */ data - 2nd largest block */

15
src/emc/motion/simple_tp.c
View file

@ -13,21 +13,6 @@
#include "simple_tp.h" #include "simple_tp.h"
#include "rtapi_math.h" #include "rtapi_math.h"
/***********************************************************************
* LOCAL VARIABLE DECLARATIONS *
************************************************************************/
/***********************************************************************
* LOCAL FUNCTION PROTOTYPES *
************************************************************************/
/***********************************************************************
* PUBLIC FUNCTION CODE *
************************************************************************/
void simple_tp_update(simple_tp_t *tp, double period) void simple_tp_update(simple_tp_t *tp, double period)
{ {
double max_dv, tiny_dp, pos_err, vel_req; double max_dv, tiny_dp, pos_err, vel_req;

1
src/emc/motion/usrmotintf.cc
View file

@ -478,7 +478,6 @@ void usrmotPrintEmcmotStatus(emcmot_status_t *s, int which)
printf("cmd: \t%d\n", s->commandEcho); printf("cmd: \t%d\n", s->commandEcho);
printf("cmd num: \t%d\n", s->commandNumEcho); printf("cmd num: \t%d\n", s->commandNumEcho);
printf("heartbeat: \t%u\n", s->heartbeat); printf("heartbeat: \t%u\n", s->heartbeat);
printf("compute time: \t%f\n", s->computeTime);
/*! \todo Another #if 0 */ /*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - change to work with joint #if 0 /*! \todo FIXME - change to work with joint
structures */ structures */