rendered paste bodyIndex: configs/stepper-xyza/stepper_xyza.hal
===================================================================
RCS file: /cvs/emc2/configs/stepper-xyza/stepper_xyza.hal,v
retrieving revision 1.7
diff -u -r1.7 stepper_xyza.hal
--- configs/stepper-xyza/stepper_xyza.hal 28 Jan 2007 22:37:27 -0000 1.7
+++ configs/stepper-xyza/stepper_xyza.hal 24 Feb 2007 23:01:02 -0000
@@ -4,7 +4,7 @@
# kinematics
loadrt trivkins
# motion controller, get name and thread periods from ini file
-loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD traj_period_nsec=[EMCMOT]TRAJ_PERIOD key=[EMCMOT]SHMEM_KEY
+loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD traj_period_nsec=[EMCMOT]TRAJ_PERIOD key=[EMCMOT]SHMEM_KEY max_joints=[TRAJ]AXES
# stepper module
loadrt stepgen step_type=0,0,0,0
Index: src/emc/motion/command.c
===================================================================
RCS file: /cvs/emc2/src/emc/motion/command.c,v
retrieving revision 1.75
diff -u -r1.75 command.c
--- src/emc/motion/command.c 11 Feb 2007 17:11:32 -0000 1.75
+++ src/emc/motion/command.c 24 Feb 2007 23:01:05 -0000
@@ -94,7 +94,7 @@
if (emcmotDebug->allHomed) return 1;
}
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
@@ -122,7 +122,7 @@
int joint_num;
emcmot_joint_t *joint;
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
@@ -154,7 +154,7 @@
return 1; /* okay to jog when limits overridden */
}
- if (joint_num < 0 || joint_num >= EMCMOT_MAX_AXIS) {
+ if (joint_num < 0 || joint_num >= EMCMOT_MAX_JOINTS) {
reportError("Can't jog invalid axis number %d.", joint_num);
return 0;
}
@@ -220,14 +220,14 @@
emcmot_joint_t *joint;
/* fill in all joints with 0 */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
joint_pos[joint_num] = 0.0;
}
/* now fill in with real values, for joints that are used */
kinematicsInverse(&pos, joint_pos, &iflags, &fflags);
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
@@ -259,7 +259,7 @@
if (kinType == KINEMATICS_INVERSE_ONLY) {
if (rehomeAll) {
- for (n = 0; n < EMCMOT_MAX_AXIS; n++) {
+ for (n = 0; n < EMCMOT_MAX_JOINTS; n++) {
/* point at joint data */
joint = &(joints[n]);
/* clear flag */
@@ -351,7 +351,7 @@
to point to the joint data. All the individual commands need to do
is verify that "joint" is non-zero. */
joint_num = emcmotCommand->axis;
- if (joint_num >= 0 && joint_num < EMCMOT_MAX_AXIS) {
+ if (joint_num >= 0 && joint_num < EMCMOT_MAX_JOINTS) {
/* valid joint, point to it's data */
joint = &joints[joint_num];
} else {
@@ -387,7 +387,7 @@
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(0);
} else {
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* tell joint planner to stop */
@@ -399,7 +399,7 @@
}
}
/* clear axis errors (regardless of mode */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* update status flags */
@@ -504,7 +504,7 @@
for a value to be used as an index and here it's a value to be
used as a counting number. The indenting is different here so
as not to match macro editing on that other bunch. */
- if (joint_num <= 0 || joint_num > EMCMOT_MAX_AXIS) {
+ if (joint_num <= 0 || joint_num > EMCMOT_MAX_JOINTS) {
break;
}
num_axes = joint_num;
@@ -546,7 +546,7 @@
emcmotStatus->overrideLimits = 1;
}
emcmotDebug->overriding = 0;
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point at joint data */
joint = &joints[joint_num];
/* clear joint errors */
Index: src/emc/motion/control.c
===================================================================
RCS file: /cvs/emc2/src/emc/motion/control.c,v
retrieving revision 1.90
diff -u -r1.90 control.c
--- src/emc/motion/control.c 18 Feb 2007 05:00:18 -0000 1.90
+++ src/emc/motion/control.c 24 Feb 2007 23:01:08 -0000
@@ -392,7 +392,7 @@
tpAbort(&emcmotDebug->queue); //if we are in coordinated move, abort
SET_MOTION_ERROR_FLAG(0); //and clear any error flag which might have been set by the abort
} else { //free mode, abort any joint
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* tell joint planner to stop */
@@ -448,7 +448,7 @@
emcmotStatus->net_spindle_scale = scale;
/* read and process per-joint inputs */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to axis HAL data */
axis_data = &(emcmot_hal_data->axis[joint_num]);
/* point to joint data */
@@ -582,7 +582,7 @@
all_homed = 1;
all_at_home = 1;
/* copy joint position feedback to local array */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* copy feedback */
@@ -660,7 +660,7 @@
int tmp;
/* check for limits on all active axes */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
/* clear soft limits */
@@ -683,7 +683,7 @@
/* check flags, see if any joint is in limit */
tmp = 0;
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
if (GET_JOINT_PSL_FLAG(joint) || GET_JOINT_NSL_FLAG(joint)) {
@@ -699,7 +699,7 @@
emcmotStatus->onSoftLimit = 1;
/* abort everything, regardless of coord or free mode */
tpAbort(&emcmotDebug->queue);
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
/* shut off free mode planner */
@@ -729,7 +729,7 @@
}
}
/* check for various joint fault conditions */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
/* only check active, enabled axes */
@@ -786,7 +786,7 @@
if (!emcmotDebug->enabling && GET_MOTION_ENABLE_FLAG()) {
/* clear out the motion emcmotDebug->queue and interpolators */
tpClear(&emcmotDebug->queue);
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
axis_data = &(emcmot_hal_data->axis[joint_num]);
joint = &joints[joint_num];
@@ -819,7 +819,7 @@
/* check for emcmotDebug->enabling */
if (emcmotDebug->enabling && !GET_MOTION_ENABLE_FLAG()) {
tpSetPos(&emcmotDebug->queue, emcmotStatus->carte_pos_cmd);
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
axis_data = &(emcmot_hal_data->axis[joint_num]);
joint = &joints[joint_num];
@@ -846,7 +846,7 @@
/* update coordinated emcmotDebug->queue position */
tpSetPos(&emcmotDebug->queue, emcmotStatus->carte_pos_cmd);
/* drain the cubics so they'll synch up */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
cubicDrain(&(joint->cubic));
@@ -887,7 +887,7 @@
if (!emcmotDebug->teleoperating && GET_MOTION_TELEOP_FLAG()) {
SET_MOTION_TELEOP_FLAG(0);
if (!emcmotDebug->coordinating) {
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS;
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS;
joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
@@ -904,7 +904,7 @@
/* preset traj planner to current position */
tpSetPos(&emcmotDebug->queue, emcmotStatus->carte_pos_cmd);
/* drain the cubics so they'll synch up */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
cubicDrain(&(joint->cubic));
@@ -924,7 +924,7 @@
/* check entering free space mode */
if (!emcmotDebug->coordinating && GET_MOTION_COORD_FLAG()) {
if (GET_MOTION_INPOS_FLAG()) {
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
/* set joint planner pos cmd to current location */
@@ -963,7 +963,7 @@
int new_jog_counts, delta;
double distance, pos;
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
axis_data = &(emcmot_hal_data->axis[joint_num]);
joint = &joints[joint_num];
@@ -1114,7 +1114,7 @@
break;
case HOME_SEQUENCE_START:
- for(i=0; i < EMCMOT_MAX_AXIS; i++) {
+ for(i=0; i < EMCMOT_MAX_JOINTS; i++) {
emcmot_joint_t *joint = &joints[i];
if(joint->home_state != HOME_IDLE) {
emcmotStatus->homingSequenceState = HOME_SEQUENCE_IDLE; return;
@@ -1123,7 +1123,7 @@
}
case HOME_SEQUENCE_START_JOINTS:
- for(i=0; i < EMCMOT_MAX_AXIS; i++) {
+ for(i=0; i < EMCMOT_MAX_JOINTS; i++) {
emcmot_joint_t *joint = &joints[i];
int j = joint->home_sequence;
if(j == home_sequence) {
@@ -1139,7 +1139,7 @@
break;
case HOME_SEQUENCE_WAIT_JOINTS:
- for(i=0; i < EMCMOT_MAX_AXIS; i++) {
+ for(i=0; i < EMCMOT_MAX_JOINTS; i++) {
emcmot_joint_t *joint = &joints[i];
int j = joint->home_sequence;
if(j != home_sequence) continue;
@@ -1178,7 +1178,7 @@
return;
}
/* loop thru axis, treat each one individually */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
@@ -1702,7 +1702,7 @@
disabled, free_pos_cmd is set to the current position. */
/* initial value for flag, if needed it will be cleared below */
SET_MOTION_INPOS_FLAG(1);
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
//AJ: only need to worry about free mode if joint is active
@@ -1806,7 +1806,7 @@
all_homed = 1;
all_at_home = 1;
/* copy joint position feedback to local array */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* copy coarse command */
@@ -1875,7 +1875,7 @@
kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions,
&iflags, &fflags);
/* copy to joint structures and spline them up */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
joint->coarse_pos = positions[joint_num];
@@ -1888,7 +1888,7 @@
}
/* there is data in the interpolators */
/* run interpolation */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* save old command */
@@ -2018,7 +2018,7 @@
kinematicsInverse(&emcmotStatus->carte_pos_cmd, positions,
&iflags, &fflags);
/* copy to joint structures and spline them up */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
joint->coarse_pos = positions[joint_num];
@@ -2031,7 +2031,7 @@
/* there is data in the interpolators */
/* run interpolation */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* save old command */
@@ -2048,7 +2048,7 @@
/* set position commands to match feedbacks, this avoids
disturbances and/or following errors when enabling */
emcmotStatus->carte_pos_cmd = emcmotStatus->carte_pos_fb;
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* save old command */
@@ -2168,7 +2168,7 @@
/* compute the correction */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
@@ -2394,7 +2394,7 @@
old_hal_index = *emcmot_hal_data->spindle_index_enable;
/* output axis info to HAL for scoping, etc */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* apply backlash and motor offset to output */
@@ -2448,7 +2448,7 @@
/* copy status info from private joint structure to status
struct in shared memory */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to joint data */
joint = &joints[joint_num];
/* point to joint status */
Index: src/emc/motion/mot_priv.h
===================================================================
RCS file: /cvs/emc2/src/emc/motion/mot_priv.h,v
retrieving revision 1.53
diff -u -r1.53 mot_priv.h
--- src/emc/motion/mot_priv.h 18 Feb 2007 04:54:58 -0000 1.53
+++ src/emc/motion/mot_priv.h 24 Feb 2007 23:01:08 -0000
@@ -150,6 +150,10 @@
/* HAL component ID for motion module */
extern int mot_comp_id;
+/* userdefined number of max joints. default is EMCMOT_MAX_AXIS(=8),
+ but can be altered at motmod insmod time */
+extern int EMCMOT_MAX_JOINTS;
+
/* pointer to emcmot_hal_data_t struct in HAL shmem, with all HAL data */
extern emcmot_hal_data_t *emcmot_hal_data;
Index: src/emc/motion/motion.c
===================================================================
RCS file: /cvs/emc2/src/emc/motion/motion.c,v
retrieving revision 1.91
diff -u -r1.91 motion.c
--- src/emc/motion/motion.c 18 Feb 2007 04:54:58 -0000 1.91
+++ src/emc/motion/motion.c 24 Feb 2007 23:01:09 -0000
@@ -64,6 +64,8 @@
RTAPI_MP_LONG(servo_period_nsec, "servo thread period (nsecs)");
static long traj_period_nsec = 0; /* trajectory planner period */
RTAPI_MP_LONG(traj_period_nsec, "trajectory planner period (nsecs)");
+static int max_joints = EMCMOT_MAX_AXIS; /* default number of joints present */
+RTAPI_MP_INT(max_joints, "number of available joints");
#endif
/***********************************************************************
@@ -83,6 +85,7 @@
int mot_comp_id; /* component ID for motion module */
int first_pass = 1; /* used to set initial conditions */
+int EMCMOT_MAX_JOINTS = EMCMOT_MAX_AXIS; /* used to limit the number of exported joints */
int kinType = 0;
@@ -195,6 +198,8 @@
return -1;
}
+ EMCMOT_MAX_JOINTS = max_joints;
+
/* initialize/export HAL pins and parameters */
retval = init_hal_io();
if (retval != 0) {
@@ -484,11 +489,11 @@
emcmot_hal_data->last_period = 0;
/* export axis pins and parameters */
- for (n = 0; n < EMCMOT_MAX_AXIS; n++) {
+ for (n = 0; n < EMCMOT_MAX_JOINTS; n++) {
/* point to axis data */
axis_data = &(emcmot_hal_data->axis[n]);
/* export all vars */
- retval = export_axis(n, axis_data);
+ retval = export_axis(n, axis_data);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: axis %d pin/param export failed\n", n);
@@ -504,7 +509,7 @@
}
/* Done! */
rtapi_print_msg(RTAPI_MSG_INFO,
- "MOTION: init_hal_io() complete, %d axes.\n", EMCMOT_MAX_AXIS);
+ "MOTION: init_hal_io() complete, %d axes.\n", n);
return 0;
error:
@@ -797,7 +802,7 @@
emcmotDebug->split = 0;
emcmotStatus->heartbeat = 0;
emcmotStatus->computeTime = 0.0;
- emcmotConfig->numAxes = EMCMOT_MAX_AXIS;
+ emcmotConfig->numAxes = EMCMOT_MAX_JOINTS;
emcmotStatus->carte_pos_cmd.tran.x = 0.0;
emcmotStatus->carte_pos_cmd.tran.y = 0.0;
@@ -840,7 +845,7 @@
#endif
/* init per-axis stuff */
- for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
+ for (joint_num = 0; joint_num < EMCMOT_MAX_JOINTS; joint_num++) {
/* point to structure for this joint */
joint = &joints[joint_num];
@@ -1082,7 +1087,7 @@
tpSetCycleTime(&emcmotDebug->queue, secs);
/* set the free planners, cubic interpolation rate and segment time */
- for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
+ for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
cubicSetInterpolationRate(&(joints[t].cubic),
emcmotConfig->interpolationRate);
}
@@ -1113,7 +1118,7 @@
(int) (emcmotConfig->trajCycleTime / secs + 0.5);
/* set the cubic interpolation rate and PID cycle time */
- for (t = 0; t < EMCMOT_MAX_AXIS; t++) {
+ for (t = 0; t < EMCMOT_MAX_JOINTS; t++) {
cubicSetInterpolationRate(&(joints[t].cubic),
emcmotConfig->interpolationRate);
cubicSetSegmentTime(&(joints[t].cubic), secs);