control.c

Source code for an Numeric Cmputer

	for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
	    /* point to joint struct */
	    joint = &joints[joint_num];
	    /* copy coarse command */
	    positions[joint_num] = joint->coarse_pos;
	    /* check for homed */
	    if (!GET_JOINT_HOMED_FLAG(joint)) {
		all_homed = 0;
		all_at_home = 0;
	    } else if (!GET_JOINT_AT_HOME_FLAG(joint)) {
		all_at_home = 0;
	    }
	}
	switch (kinType) {

	case KINEMATICS_IDENTITY:
	    kinematicsForward(positions, &emcmotStatus->carte_pos_cmd, &fflags, &iflags);
	    if (checkAllHomed()) {
		emcmotStatus->carte_pos_cmd_ok = 1;
	    } else {
		emcmotStatus->carte_pos_cmd_ok = 0;
	    }
	    break;

	case KINEMATICS_BOTH:
	    if (checkAllHomed()) {
		/* is previous value suitable for use as initial guess? */
		if (!emcmotStatus->carte_pos_cmd_ok) {
		    /* no, use home position as initial guess */
		    emcmotStatus->carte_pos_cmd = emcmotStatus->world_home;
		}
		/* calculate Cartesean position command from joint coarse pos cmd */
		result =
		    kinematicsForward(positions, &emcmotStatus->carte_pos_cmd, &fflags, &iflags);
		/* check to make sure kinematics converged */
		if (result < 0) {
		    /* error during kinematics calculations */
		    emcmotStatus->carte_pos_cmd_ok = 0;
		} else {
		    /* it worked! */
		    emcmotStatus->carte_pos_cmd_ok = 1;
		}
	    } else {
		emcmotStatus->carte_pos_cmd_ok = 0;
	    }
	    break;

	case KINEMATICS_INVERSE_ONLY:
	    emcmotStatus->carte_pos_cmd_ok = 0;
	    break;

	default:
	    emcmotStatus->carte_pos_cmd_ok = 0;
	    break;
	}
        /* end of FREE mode */
	break;
    case EMCMOT_MOTION_COORD:
	/* check joint 0 to see if the interpolators are empty */
	while (cubicNeedNextPoint(&(joints[0].cubic))) {
	    /* they're empty, pull next point(s) off Cartesian planner */
	    /* run coordinated trajectory planning cycle */
	    tpRunCycle(&emcmotDebug->queue);
	    /* gt new commanded traj pos */
	    emcmotStatus->carte_pos_cmd = tpGetPos(&emcmotDebug->queue);
	    /* OUTPUT KINEMATICS - convert to joints in local array */
	    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++) {
		/* point to joint struct */
		joint = &joints[joint_num];
		joint->coarse_pos = positions[joint_num];
		/* spline joints up-- note that we may be adding points
		   that fail soft limits, but we'll abort at the end of
		   this cycle so it doesn't really matter */
		cubicAddPoint(&(joint->cubic), joint->coarse_pos);
	    }
	    /* END OF OUTPUT KINS */
	}
	/* there is data in the interpolators */
	/* run interpolation */
	for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
	    /* point to joint struct */
	    joint = &joints[joint_num];
	    /* save old command */
	    old_pos_cmd = joint->pos_cmd;
	    /* interpolate to get new one */
	    joint->pos_cmd = cubicInterpolate(&(joint->cubic), 0, 0, 0, 0);
	    joint->vel_cmd = (joint->pos_cmd - old_pos_cmd) * servo_freq;
	}
	/* report motion status */
	SET_MOTION_INPOS_FLAG(0);
	if (tpIsDone(&emcmotDebug->queue)) {
	    SET_MOTION_INPOS_FLAG(1);
	}
	break;
    case EMCMOT_MOTION_TELEOP:

	/* first the desired Accell's are computed based on
	    desired Velocity, current velocity and period */
	emcmotDebug->teleop_data.desiredAccell.tran.x =
	    (emcmotDebug->teleop_data.desiredVel.tran.x -
	    emcmotDebug->teleop_data.currentVel.tran.x) /
	    servo_period;
	emcmotDebug->teleop_data.desiredAccell.tran.y =
	    (emcmotDebug->teleop_data.desiredVel.tran.y -
	    emcmotDebug->teleop_data.currentVel.tran.y) /
	    servo_period;
	emcmotDebug->teleop_data.desiredAccell.tran.z =
	    (emcmotDebug->teleop_data.desiredVel.tran.z -
	    emcmotDebug->teleop_data.currentVel.tran.z) /
	    servo_period;

	/* a Carthesian Accell is computed */
	pmCartMag(emcmotDebug->teleop_data.desiredAccell.tran,
	    &accell_mag);

	/* then the accells for the rotary axes */
	emcmotDebug->teleop_data.desiredAccell.a =
	    (emcmotDebug->teleop_data.desiredVel.a -
	    emcmotDebug->teleop_data.currentVel.a) /
	    servo_period;
	emcmotDebug->teleop_data.desiredAccell.b =
	    (emcmotDebug->teleop_data.desiredVel.b -
	    emcmotDebug->teleop_data.currentVel.b) /
	    servo_period;
	emcmotDebug->teleop_data.desiredAccell.c =
	    (emcmotDebug->teleop_data.desiredVel.c -
	    emcmotDebug->teleop_data.currentVel.c) /
	    servo_period;
	if (emcmotDebug->teleop_data.desiredAccell.a > accell_mag) {
	    accell_mag = emcmotDebug->teleop_data.desiredAccell.a;
	}
	if (emcmotDebug->teleop_data.desiredAccell.b > accell_mag) {
	    accell_mag = emcmotDebug->teleop_data.desiredAccell.b;
	}
	if (emcmotDebug->teleop_data.desiredAccell.c > accell_mag) {
	    accell_mag = emcmotDebug->teleop_data.desiredAccell.c;
	}
	
	/* accell_mag should now hold the max accell */
	
	if (accell_mag > emcmotStatus->acc) {
	    /* if accell_mag is too great, all need resizing */
	    pmCartScalMult(emcmotDebug->teleop_data.desiredAccell.tran, 
		emcmotStatus->acc / accell_mag,
		&emcmotDebug->teleop_data.currentAccell.tran);
	    emcmotDebug->teleop_data.currentAccell.a =
		emcmotDebug->teleop_data.desiredAccell.a *
		emcmotStatus->acc / accell_mag;
	    emcmotDebug->teleop_data.currentAccell.b =
		emcmotDebug->teleop_data.desiredAccell.b *
		emcmotStatus->acc / accell_mag;
	    emcmotDebug->teleop_data.currentAccell.c =
		emcmotDebug->teleop_data.desiredAccell.c *
		emcmotStatus->acc / accell_mag;
	    emcmotDebug->teleop_data.currentVel.tran.x +=
		emcmotDebug->teleop_data.currentAccell.tran.x *
		servo_period;
	    emcmotDebug->teleop_data.currentVel.tran.y +=
		emcmotDebug->teleop_data.currentAccell.tran.y *
		servo_period;
	    emcmotDebug->teleop_data.currentVel.tran.z +=
		emcmotDebug->teleop_data.currentAccell.tran.z *
		servo_period;
	    emcmotDebug->teleop_data.currentVel.a +=
		emcmotDebug->teleop_data.currentAccell.a *
		servo_period;
	    emcmotDebug->teleop_data.currentVel.b +=
		emcmotDebug->teleop_data.currentAccell.b *
		servo_period;
	    emcmotDebug->teleop_data.currentVel.c +=
		emcmotDebug->teleop_data.currentAccell.c *
		servo_period;
	} else {
	    /* if accell_mag is not greater, the computed accell's stay as is */
	    emcmotDebug->teleop_data.currentAccell =
		emcmotDebug->teleop_data.desiredAccell;
	    emcmotDebug->teleop_data.currentVel =
		emcmotDebug->teleop_data.desiredVel;
	}


	/* based on curent position, current vel and period, 
	   the next position is computed */
	emcmotStatus->carte_pos_cmd.tran.x +=
	    emcmotDebug->teleop_data.currentVel.tran.x *
	    servo_period;
	emcmotStatus->carte_pos_cmd.tran.y +=
	    emcmotDebug->teleop_data.currentVel.tran.y *
	    servo_period;
	emcmotStatus->carte_pos_cmd.tran.z +=
	    emcmotDebug->teleop_data.currentVel.tran.z *
	    servo_period;
	emcmotStatus->carte_pos_cmd.a +=
	    emcmotDebug->teleop_data.currentVel.a *
	    servo_period;
	emcmotStatus->carte_pos_cmd.b +=
	    emcmotDebug->teleop_data.currentVel.b *
	    servo_period;
	emcmotStatus->carte_pos_cmd.c +=
	    emcmotDebug->teleop_data.currentVel.c *
	    servo_period;

	/* the next position then gets run through the inverse kins,
	    to compute the next positions of the joints */

	/* OUTPUT KINEMATICS - convert to joints in local array */
	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++) {
	    /* point to joint struct */
	    joint = &joints[joint_num];
	    joint->coarse_pos = positions[joint_num];
	    /* spline joints up-- note that we may be adding points
		   that fail soft limits, but we'll abort at the end of
		   this cycle so it doesn't really matter */
	    cubicAddPoint(&(joint->cubic), joint->coarse_pos);
	}
	/* END OF OUTPUT KINS */

	/* there is data in the interpolators */
	/* run interpolation */
	for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
	    /* point to joint struct */
	    joint = &joints[joint_num];
	    /* save old command */
	    old_pos_cmd = joint->pos_cmd;
	    /* interpolate to get new one */
	    joint->pos_cmd = cubicInterpolate(&(joint->cubic), 0, 0, 0, 0);
	    joint->vel_cmd = (joint->pos_cmd - old_pos_cmd) * servo_freq;
	}

	/* end of teleop mode */
	break;

    case EMCMOT_MOTION_DISABLED:
	/* 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++) {
	    /* point to joint struct */
	    joint = &joints[joint_num];
	    /* save old command */
	    joint->pos_cmd = joint->pos_fb;
	    /* set joint velocity to zero */
	    joint->vel_cmd = 0.0;
	}
	
	break;
    default:
	break;
    }

/* NOTES:  These notes are just my understanding of how things work.

There are seven sets of position information.

1) emcmotStatus->carte_pos_cmd
2) emcmotStatus->joints[n].coarse_pos
3) emcmotStatus->joints[n].pos_cmd
4) emcmotStatus->joints[n].motor_pos_cmd
5) emcmotStatus->joints[n].motor_pos_fb
6) emcmotStatus->joints[n].pos_fb
7) emcmotStatus->carte_pos_fb

Their exact contents and meaning are as follows:

1) This is the desired position, in Cartesean coordinates.  It is
   updated at the traj rate, not the servo rate.
   In coord mode, it is determined by the traj planner
   In teleop mode, it is determined by the traj planner?
   In free mode, it is not used, since free mode motion takes
     place in joint space, not cartesean space.  It may be
     displayed by the GUI however, so it is updated by
     applying forward kins to (2), unless forward kins are
     not available, in which case it is copied from (7).

2) This is the desired position, in joint coordinates, but
   before interpolation.  It is updated at the traj rate, not
   the servo rate..
   In coord mode, it is generated by applying inverse kins to (1)
   In teleop mode, it is generated by applying inverse kins to (1)
   In free mode, it is not used, since the free mode planner generates
     a new (3) position every servo period without interpolation.
     However, it is used indirectly by GUIs, so it is copied from (3).

3) This is the desired position, in joint coords, after interpolation.
   A new set of these coords is generated every servo period.
   In coord mode, it is generated from (2) by the interpolator.
   In teleop mode, it is generated from (2) by the interpolator.
   In free mode, it is generated by the simple free mode traj planner.

4) This is the desired position, in motor coords.  Motor coords are
   generated by adding backlash compensation, lead screw error
   compensation, and offset (for homing) to (3).
   It is generated the same way regardless of the mode, and is the
   output to the PID loop or other position loop.

5) This is the actual position, in motor coords.  It is the input from
   encoders or other feedback device (or from virtual encoders on open
   loop machines).  It is "generated" by reading the feedback device.

6) This is the actual position, in joint coordinates.  It is generated
   by subtracting offset, lead screw error compensation, and backlash
   compensation from (5).  It is generated the same way regardless of
   the operating mode.

7) This is the actual position, in Cartesean coordinates.  It is updated
   at the traj rate, not the servo rate.
   OLD VERSION:
   In the old version, there are four sets of code to generate actualPos.
   One for each operating mode, and one for when motion is disabled.
   The code for coord and teleop modes is identical.  The code for free
   mode is somewhat different, in particular to deal with the case where
   one or more axes are not homed.  The disabled code is quite similar,
   but not identical, to the coord mode code.  In general, the code
   calculates actualPos by applying the forward kins to (6).  However,
   where forward kins are not available, actualPos is either copied
   from (1) (assumes no following error), or simply left alone.
   These special cases are handled differently for each operating mode.
   NEW VERSION:
   I would like to both simplify and relocate this.  As far as I can
   tell, actualPos should _always_ be the best estimate of the actual
   machine position in Cartesean coordinates.  So it should always be
   calculated the same way.
   In addition to always using the same code to calculate actualPos,
   I want to move that code.  It is really a feedback calculation, and
   as such it belongs with the rest of the feedback calculations early
   in control.c, not as part of the output generation code as it is now.
   Ideally, actualPos would always be calculated by applying forward
   kinematics to (6).  However, forward kinematics may not be available,
   or they may be unusable because one or more axes aren't homed.  In
   that case, the options are: A) fake it by copying (1), or B) admit
   that we don't really know the Cartesean coordinates, and simply
   don't update actualPos.  Whatever approach is used, I can see no
   reason not to do it the same way regardless of the operating mode.
   I would propose the following:  If there are forward kins, use them,
   unless they don't work because of unhomed axes or other problems,
   in which case do (B).  If no forward kins, do (A), since otherwise
   actualPos would _never_ get updated.

*/

/* there was a large snip of old code here, 
   removed since the new implementation works.
   if the old code is needed it's in CVS revision <= 1.46 */

}

static void compute_backlash(void)
{
    int joint_num;
    emcmot_joint_t *joint;
    double max_delta_pos, dist_to_go;

    for (joint_num = 0; joint_num < EMCMOT_MAX_AXIS; joint_num++) {
	/* point to joint struct */
	joint = &joints[joint_num];
	/* determine which way the compensation should be applied */
	if (joint->vel_cmd > 0.0) {
	    /* moving "up". apply positive backlash comp */
	    /*! \todo FIXME - the more sophisticated axisComp should be applied
	       here, if available */
	    joint->backlash_corr = 0.5 * joint->backlash;
	} else if (joint->vel_cmd < 0.0) {
	    /* moving "down". apply negative backlash comp */
	    /*! \todo FIXME - the more sophisticated axisComp should be app