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
#define _(s) (s)
/* debugging functions */
extern void print_pose ( EmcPose *pos );
/* kinematics flags */
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags = 0;
static int rehomeAll;
/* loops through the active joints and checks if any are not homed */
int checkAllHomed(void) {
int checkAllHomed(void)
{
int joint_num;
emcmot_joint_t *joint;
@ -110,7 +106,6 @@ int checkAllHomed(void) {
return 1;
}
/* limits_ok() returns 1 if none of the hard limits are set,
0 if any are set. Called on a linear and circular move. */
STATIC int limits_ok(void)
@ -277,7 +272,6 @@ void clearHomes(int joint_num)
}
}
void emcmotSetRotaryUnlock(int axis, int unlock) {
*(emcmot_hal_data->joint[axis].unlock) = unlock;
}
@ -1010,7 +1004,6 @@ void emcmotCommandHandler(void *arg, long period)
break;
}
joint->vel_limit = emcmotCommand->vel;
joint->big_vel = 10 * emcmotCommand->vel;
break;
case EMCMOT_SET_JOINT_ACC_LIMIT:
@ -1266,18 +1259,6 @@ void emcmotCommandHandler(void *arg, long period)
}
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:
/* unhome the specified joint, or all joints if -1 */
rtapi_print_msg(RTAPI_MSG_DBG, "JOINT_UNHOME");
@ -1338,14 +1319,6 @@ void emcmotCommandHandler(void *arg, long period)
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:
rtapi_print_msg(RTAPI_MSG_DBG, "CLEAR_PROBE_FLAGS");
emcmotStatus->probing = 0;

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

@ -29,25 +29,66 @@
// Mark strings for translation, but defer translation to userspace
#define _(s) (s)
/***********************************************************************
* LOCAL VARIABLE DECLARATIONS *
************************************************************************/
/* kinematics flags */
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags = 0;
/*! \todo FIXME - this is a leftover global, it will eventually go away */
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;
/* 1/servo cycle time */
double servo_freq;
/*! \todo FIXME - debugging - uncomment the following line to log changes in
JOINT_FLAG and MOTION_FLAG */
// #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 *
@ -80,7 +121,6 @@ static void do_forward_kins(void);
cartesian feedback position is latched when the probe fires, and it
should be based on the feedback read in on this servo cycle.
*/
static void process_probe_inputs(void);
/* '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);
/* 'compute_screw_comp()' is responsible for calculating backlash and
lead screw error compensation. (Leadscrew error compensation is
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);
/***********************************************************************
* PUBLIC FUNCTION CODE *
************************************************************************/
@ -179,7 +217,6 @@ static void update_status(void);
Inactive axes are still calculated, but the PIDs are inhibited and
the amp enable/disable are inhibited
*/
void emcmotController(void *arg, long period)
{
static long long int last = 0;
@ -236,100 +273,6 @@ void emcmotController(void *arg, long period)
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)
{
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)
{
int joint_num;

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

@ -55,6 +55,10 @@
#define DEFAULT_MAX_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
* a TC_STRUCT is about 512 bytes so this queue is
* 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 *motion_enabled; /* RPI: motion enable for all joints */
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 *teleop_mode; /* RPA: TRUE if teleop mode */
hal_bit_t *coord_error; /* RPA: TRUE if coord mode error */
@ -186,10 +185,7 @@ typedef struct {
* GLOBAL VARIABLE DECLARATIONS *
************************************************************************/
/* HAL component ID for motion module */
extern int mot_comp_id;
/* userdefined number of joints. default is EMCMOT_MAX_JOINTS(=8),
/* userdefined number of joints. default is EMCMOT_MAX_JOINTS(=9),
but can be altered at motmod insmod time */
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 */
/* 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;
/* Variable defs */
extern int kinType;
extern int rehomeAll;
extern int DEBUG_MOTION;
extern int EMCMOT_NO_FORWARD_KINEMATICS;
extern KINEMATICS_FORWARD_FLAGS fflags;
extern KINEMATICS_INVERSE_FLAGS iflags;
/* these variables have the servo cycle time and 1/cycle time */
extern double servo_period;
/* these variable have the 1/servo cycle time */
extern double servo_freq;
/* Struct pointers */
extern struct emcmot_struct_t *emcmotStruct;
extern struct emcmot_command_t *emcmotCommand;
extern struct emcmot_status_t *emcmotStatus;
extern struct emcmot_config_t *emcmotConfig;
extern struct emcmot_debug_t *emcmotDebug;
extern struct emcmot_internal_t *emcmotInternal;
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 */
extern void clearHomes(int joint_num);
extern void check_stuff(const char *msg);
extern void emcmot_config_change(void);
extern void reportError(const char *fmt, ...) __attribute((format(printf,1,2))); /* Use the rtapi_print call */

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

@ -27,8 +27,6 @@
* KERNEL MODULE PARAMETERS *
************************************************************************/
static int key = DEFAULT_SHMEM_KEY; /* the shared memory key, default value */
/* module information */
/* register symbols to be modified by insmod
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_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 */
static int key = DEFAULT_SHMEM_KEY; /* the shared memory key, default value */
RTAPI_MP_INT(key, "shared memory key");
static long base_period_nsec = 0; /* fastest thread period */
RTAPI_MP_LONG(base_period_nsec, "fastest thread period (nsecs)");
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)");
int num_joints = EMCMOT_MAX_JOINTS; /* default number of joints present */
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");
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");
/***********************************************************************
@ -74,7 +68,6 @@ emcmot_joint_t *joints = 0;
emcmot_joint_t joint_array[EMCMOT_MAX_JOINTS];
#endif
int mot_comp_id; /* component ID for motion module */
int kinType = 0;
/*
@ -90,13 +83,11 @@ int kinType = 0;
emcmotError points to emcmotStruct->error, and
*/
emcmot_struct_t *emcmotStruct = 0;
/* ptrs to either buffered copies or direct memory for
command and status */
/* ptrs to either buffered copies or direct memory for command and status */
struct emcmot_command_t *emcmotCommand = 0;
struct emcmot_status_t *emcmotStatus = 0;
struct emcmot_config_t *emcmotConfig = 0;
struct emcmot_debug_t *emcmotDebug = 0;
struct emcmot_internal_t *emcmotInternal = 0;
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 */
static int emc_shmem_id; /* the shared memory ID */
static int mot_comp_id; /* component ID for motion module */
/***********************************************************************
* LOCAL FUNCTION PROTOTYPES *
************************************************************************/
@ -185,22 +178,22 @@ int rtapi_app_main(void)
}
if (( num_joints < 1 ) || ( num_joints > EMCMOT_MAX_JOINTS )) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: num_joints is %d, must be between 1 and %d\n"),
num_joints, EMCMOT_MAX_JOINTS);
_("MOTION: num_joints is %d, must be between 1 and %d\n"), num_joints, EMCMOT_MAX_JOINTS);
hal_exit(mot_comp_id);
return -1;
}
if (( num_dio < 1 ) || ( num_dio > EMCMOT_MAX_DIO )) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: num_dio is %d, must be between 1 and %d\n"),
num_dio, EMCMOT_MAX_DIO);
_("MOTION: num_dio is %d, must be between 1 and %d\n"), num_dio, EMCMOT_MAX_DIO);
hal_exit(mot_comp_id);
return -1;
}
if (( num_aio < 1 ) || ( num_aio > EMCMOT_MAX_AIO )) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: num_aio is %d, must be between 1 and %d\n"),
num_aio, EMCMOT_MAX_AIO);
_("MOTION: num_aio is %d, must be between 1 and %d\n"), num_aio, EMCMOT_MAX_AIO);
hal_exit(mot_comp_id);
return -1;
}
@ -215,8 +208,7 @@ int rtapi_app_main(void)
/* allocate/initialize user space comm buffers (cmd/status/err) */
retval = init_comm_buffers();
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: init_comm_buffers() failed\n"));
rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_comm_buffers() failed\n"));
hal_exit(mot_comp_id);
return -1;
}
@ -283,25 +275,24 @@ static int init_hal_io(void)
/* allocate shared memory for machine data */
emcmot_hal_data = hal_malloc(sizeof(emcmot_hal_data_t));
if (emcmot_hal_data == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: emcmot_hal_data malloc failed\n"));
rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: emcmot_hal_data malloc failed\n"));
return -1;
}
/* 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_IO, &(emcmot_hal_data->spindle_index_enable), mot_comp_id, "motion.spindle-index-enable")) < 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_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_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_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_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_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_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_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_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_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_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;
*(emcmot_hal_data->spindle_inhibit) = 0;
// 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) = 0;
// 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_revs), mot_comp_id, "motion.spindle-revs")) < 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_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) < 0) goto error;
*emcmot_hal_data->spindle_is_atspeed = 1;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 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;
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_float_newf(HAL_IN, &(emcmot_hal_data->spindle_speed_in), mot_comp_id, "motion.spindle-speed-in")) != 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->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) != 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;
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_bit_newf(HAL_IN, &(emcmot_hal_data->enable), mot_comp_id, "motion.enable")) != 0) goto error;
/* export motion-synched digital output pins */
/* export motion digital input pins */
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_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_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;
}
/* export motion-synched analog output pins */
/* export motion analog input pins */
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_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_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;
}
/* export machine wide hal parameters */
retval =
hal_pin_bit_new("motion.motion-enabled", HAL_OUT, &(emcmot_hal_data->motion_enabled),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.in-position", HAL_OUT, &(emcmot_hal_data->in_position),
mot_comp_id);
if (retval != 0) {
return retval;
}
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 machine wide hal pins */
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->motion_enabled), mot_comp_id, "motion.motion-enabled")) != 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->in_position), mot_comp_id, "motion.in-position")) != 0) goto error;
if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->motion_type), mot_comp_id, "motion.motion-type")) != 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->coord_mode), mot_comp_id, "motion.coord-mode")) != 0) goto error;
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;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->on_soft_limit), mot_comp_id, "motion.on-soft-limit")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->current_vel), mot_comp_id, "motion.current-vel")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->requested_vel), mot_comp_id, "motion.requested-vel")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->distance_to_go), mot_comp_id, "motion.distance-to-go")) != 0) goto error;
if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->program_line), mot_comp_id, "motion.program-line")) != 0) goto error;
/* export debug parameters */
/* these can be used to view any internal variable, simply change a line
in control.c:output_to_hal() and recompile */
retval =
hal_param_bit_new("motion.debug-bit-0", HAL_RO, &(emcmot_hal_data->debug_bit_0),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_bit_new("motion.debug-bit-1", HAL_RO, &(emcmot_hal_data->debug_bit_1),
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;
}
if ((retval = hal_param_bit_newf(HAL_RO, &(emcmot_hal_data->debug_bit_0), mot_comp_id, "motion.debug-bit-0")) != 0) goto error;
if ((retval = hal_param_bit_newf(HAL_RO, &(emcmot_hal_data->debug_bit_1), mot_comp_id, "motion.debug-bit-1")) != 0) goto error;
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 = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->debug_float_1), mot_comp_id, "motion.debug-float-1")) != 0) goto error;
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;
if ((retval = hal_param_s32_newf(HAL_RO, &(emcmot_hal_data->debug_s32_0), mot_comp_id, "motion.debug-s32-0")) != 0) goto error;
if ((retval = hal_param_s32_newf(HAL_RO, &(emcmot_hal_data->debug_s32_1), mot_comp_id, "motion.debug-s32-1")) != 0) goto error;
// FIXME - debug only, remove later
// export HAL parameters for some trajectory planner internal variables
// so they can be scoped
retval =
hal_param_float_new("traj.pos_out", HAL_RO, &(emcmot_hal_data->traj_pos_out),
mot_comp_id);
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;
}
if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->traj_pos_out), mot_comp_id, "traj.pos_out")) != 0) goto error;
if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->traj_vel_out), mot_comp_id, "traj.vel_out")) != 0) goto error;
if ((retval = hal_param_u32_newf(HAL_RO, &(emcmot_hal_data->traj_active_tc), mot_comp_id, "traj.active_tc")) != 0) goto error;
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 != 0) {
return retval;
}
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;
}
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 = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_vel[n]), mot_comp_id, "tc.%d.vel", n)) != 0) goto error;
if ((retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_acc[n]), mot_comp_id, "tc.%d.acc", n)) != 0) goto error;
}
// end of exporting trajectory planner internals
// export timing related HAL parameters so they can be scoped
retval =
hal_param_u32_new("motion.servo.last-period", HAL_RO, &(emcmot_hal_data->last_period), mot_comp_id);
if (retval != 0) {
return retval;
}
if ((retval = hal_param_u32_newf(HAL_RO, &(emcmot_hal_data->last_period), mot_comp_id, "motion.servo.last-period")) != 0) goto error;
#ifdef HAVE_CPU_KHZ
retval =
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;
}
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;
#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 != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.y", HAL_OUT, &(emcmot_hal_data->tooloffset_y), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.z", HAL_OUT, &(emcmot_hal_data->tooloffset_z), mot_comp_id);
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;
}
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_x), mot_comp_id, "motion.tooloffset.x")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_y), mot_comp_id, "motion.tooloffset.y")) != 0) goto error;
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;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_b), mot_comp_id, "motion.tooloffset.b")) != 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_c), mot_comp_id, "motion.tooloffset.c")) != 0) goto error;
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;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->tooloffset_w), mot_comp_id, "motion.tooloffset.w")) != 0) goto error;
/* 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;
/* default value of enable is TRUE, so simple machines
can leave it disconnected */
@ -598,13 +433,10 @@ static int init_hal_io(void)
/* export all vars */
retval = export_joint(n, joint_data);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: joint %d pin/param export failed\n"), n);
rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: joint %d pin/param export failed\n"), n);
return -1;
}
/* 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->home_state) = 0;
/* 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();
rtapi_set_msg_level(RTAPI_MSG_WARN);
/* export joint pins */ //FIXME-AJ: changing these will bork configs, still we should do it
retval =
hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_cmd), mot_comp_id, "joint.%d.pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_fb), mot_comp_id, "joint.%d.pos-fb", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->motor_pos_cmd), mot_comp_id, "joint.%d.motor-pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->motor_offset), mot_comp_id, "joint.%d.motor-offset", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_IN, &(addr->motor_pos_fb), mot_comp_id, "joint.%d.motor-pos-fb", num);
if (retval != 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 != 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 != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->home_sw), mot_comp_id, "joint.%d.home-sw-in", num);
if (retval != 0) {
return retval;
}
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;
}
/* export joint pins */
if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->coarse_pos_cmd), mot_comp_id, "joint.%d.coarse-pos-cmd", num)) != 0) return retval;
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 = hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_fb), mot_comp_id, "joint.%d.pos-fb", num)) != 0) 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;
if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->motor_offset), mot_comp_id, "joint.%d.motor-offset", num)) != 0) return retval;
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 = hal_pin_bit_newf(HAL_IN, &(addr->neg_lim_sw), mot_comp_id, "joint.%d.neg-lim-sw-in", num)) != 0) 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;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->amp_enable), mot_comp_id, "joint.%d.amp-enable-out", num)) != 0) return retval;
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 = hal_pin_s32_newf(HAL_IN, &(addr->jog_counts), mot_comp_id, "joint.%d.jog-counts", num)) != 0) 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;
if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->joint_vel_cmd), mot_comp_id, "joint.%d.vel-cmd", num)) != 0) return retval;
if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->backlash_corr), mot_comp_id, "joint.%d.backlash-corr", num)) != 0) return retval;
if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->backlash_filt), mot_comp_id, "joint.%d.backlash-filt", num)) != 0) 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;
if ((retval = hal_pin_float_newf(HAL_OUT, &(addr->f_error_lim), mot_comp_id, "joint.%d.f-error-lim", num)) != 0) return retval;
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 = hal_pin_float_newf(HAL_OUT, &(addr->free_vel_lim), mot_comp_id, "joint.%d.free-vel-lim", num)) != 0) 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;
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 = hal_pin_bit_newf(HAL_OUT, &(addr->in_position), mot_comp_id, "joint.%d.in-position", num)) != 0) 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;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->active), mot_comp_id, "joint.%d.active", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->error), mot_comp_id, "joint.%d.error", num)) != 0) 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;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->homed), mot_comp_id, "joint.%d.homed", num)) != 0) return retval;
if ((retval = hal_pin_bit_newf(HAL_IN, &(addr->homing), mot_comp_id, "joint.%d.homing", num)) != 0) 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) {
// for rotaries only...
retval = hal_pin_bit_newf(HAL_OUT, &(addr->unlock), mot_comp_id, "joint.%d.unlock", num);
if (retval != 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;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(addr->unlock), mot_comp_id, "joint.%d.unlock", num)) != 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;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
@ -819,8 +521,7 @@ static int init_comm_buffers(void)
emcmot_joint_t *joint;
int retval;
rtapi_print_msg(RTAPI_MSG_INFO,
"MOTION: init_comm_buffers() starting...\n");
rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() starting...\n");
emcmotStruct = 0;
emcmotDebug = 0;
@ -828,9 +529,6 @@ static int init_comm_buffers(void)
emcmotCommand = 0;
emcmotConfig = 0;
/* record the kinematics type of the machine */
kinType = kinematicsType();
/* allocate and initialize the shared memory structure */
emc_shmem_id = rtapi_shmem_new(key, mot_comp_id, sizeof(emcmot_struct_t));
if (emc_shmem_id < 0) {
@ -853,7 +551,6 @@ static int init_comm_buffers(void)
emcmotStatus = &emcmotStruct->status;
emcmotConfig = &emcmotStruct->config;
emcmotDebug = &emcmotStruct->debug;
emcmotInternal = &emcmotStruct->internal;
emcmotError = &emcmotStruct->error;
/* init error struct */
@ -883,7 +580,6 @@ static int init_comm_buffers(void)
SET_MOTION_TELEOP_FLAG(0);
emcmotDebug->split = 0;
emcmotStatus->heartbeat = 0;
emcmotStatus->computeTime = 0.0;
emcmotConfig->numJoints = num_joints;
ZERO_EMC_POSE(emcmotStatus->carte_pos_cmd);
@ -907,11 +603,10 @@ static int init_comm_buffers(void)
emcmotStatus->spindle.speed = 0.0;
SET_MOTION_INPOS_FLAG(1);
SET_MOTION_ENABLE_FLAG(0);
/* record the kinematics type of the machine */
kinType = kinematicsType();
emcmotConfig->kinematics_type = kinType;
emcmotDebug->oldPos = emcmotStatus->carte_pos_cmd;
ZERO_EMC_POSE(emcmotDebug->oldVel);
emcmot_config_change();
/* init pointer to joint structs */
@ -927,7 +622,6 @@ static int init_comm_buffers(void)
joint = &joints[joint_num];
/* init the config fields with some "reasonable" defaults" */
joint->type = 0;
joint->max_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_flags = 0;
joint->home_sequence = -1;
joint->home_state = HOME_IDLE;
joint->backlash = 0.0;
joint->comp.entries = 0;
@ -962,8 +657,11 @@ static int init_comm_buffers(void)
joint->comp.array[n].rev_slope = 0.0;
}
/* init status info */
/* init joint flags */
joint->flag = 0;
SET_JOINT_INPOS_FLAG(joint, 1);
/* init status info */
joint->coarse_pos = 0.0;
joint->pos_cmd = 0.0;
joint->vel_cmd = 0.0;
@ -979,24 +677,6 @@ static int init_comm_buffers(void)
/* init internal info */
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 */
@ -1130,13 +810,15 @@ static int init_threads(void)
return 0;
}
void emcmotSetCycleTime(unsigned long nsec ) {
void emcmotSetCycleTime(unsigned long nsec )
{
int servo_mult;
servo_mult = traj_period_nsec / nsec;
if(servo_mult < 0) servo_mult = 1;
setTrajCycleTime(nsec * 1e-9);
setServoCycleTime(nsec * servo_mult * 1e-9);
}
/* call this when setting the trajectory cycle time */
static int setTrajCycleTime(double secs)
{
@ -1147,7 +829,6 @@ static int setTrajCycleTime(double secs)
/* make sure it's not zero */
if (secs <= 0.0) {
return -1;
}

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

@ -107,8 +107,6 @@ extern "C" {
EMCMOT_ABORT = 1, /* abort all motion */
EMCMOT_ENABLE, /* enable 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_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
to set position to zero during homing */
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;
/* 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 {
EMCMOT_JOINT_FLAG flag; /* see above for bit details */
double pos_cmd; /* commanded joint position */
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;
int config_num; /* incremented whenever configuration
changed. */
double computeTime;
int id; /* id for executing motion */
int depth; /* motion queue depth */
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 acc; /* scalar max accel */
int level;
int motionType;
double distance_to_go; /* in this move */
EmcPose dtg;
@ -754,25 +745,6 @@ Suggestion: Split this in to an Error and a Status flag register..
int inhibit_probe_home_error;
} 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 */
typedef struct emcmot_error_t {
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;
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 */
unsigned char allHomed;
@ -76,21 +70,9 @@ typedef struct emcmot_debug_t {
/*! \todo FIXME-- default is used; dynamic is not honored */
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 coordinating; /* 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
move while overriding limits */

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

@ -20,8 +20,6 @@
to the RT module from usr space */
struct emcmot_status_t status; /* Struct used to store RT status */
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_debug_t debug; /* Struct used to store RT status and debug
data - 2nd largest block */

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

@ -13,21 +13,6 @@
#include "simple_tp.h"
#include "rtapi_math.h"
/***********************************************************************
* LOCAL VARIABLE DECLARATIONS *
************************************************************************/
/***********************************************************************
* LOCAL FUNCTION PROTOTYPES *
************************************************************************/
/***********************************************************************
* PUBLIC FUNCTION CODE *
************************************************************************/
void simple_tp_update(simple_tp_t *tp, double period)
{
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 num: \t%d\n", s->commandNumEcho);
printf("heartbeat: \t%u\n", s->heartbeat);
printf("compute time: \t%f\n", s->computeTime);
/*! \todo Another #if 0 */
#if 0 /*! \todo FIXME - change to work with joint
structures */