control.c

Source code for an Numeric Cmputer

		/* next state */
		joint->home_state = HOME_FINAL_BACKOFF_WAIT;
		break;

	    case HOME_FINAL_BACKOFF_WAIT:
		/* This state is called while the machine is moving off of
		   the home switch after finding it's approximate location.
		   It terminates when the switch is cleared successfully.  If
		   the move ends or hits a limit before it clears the switch,
		   the home is aborted. */
		/* are we off home switch yet? */
		if (home_sw_fall) {
		    /* yes, stop motion */
		    joint->free_tp_enable = 0;
		    /* begin final search */
		    joint->home_state = HOME_RISE_SEARCH_START;
		    immediate_state = 1;
		    break;
		}
		home_do_moving_checks(joint);
		break;

	    case HOME_RISE_SEARCH_START:
		/* This state is called to start the final search for the
		   point where the home switch trips.  It moves at
		   'latch_vel' and looks for a rising edge on the switch */
		/* is the joint already moving? */
		if (joint->free_tp_active) {
		    /* yes, reset delay, wait until joint stops */
		    joint->home_pause_timer = 0;
		    break;
		}
		/* has delay timed out? */
		if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
		    /* no, update timer and wait some more */
		    joint->home_pause_timer++;
		    break;
		}
		joint->home_pause_timer = 0;
		/* set up a move at 'latch_vel' to locate the switch */
		home_start_move(joint, joint->home_latch_vel);
		/* next state */
		joint->home_state = HOME_RISE_SEARCH_WAIT;
		break;

	    case HOME_RISE_SEARCH_WAIT:
		/* This state is called while the machine is moving towards
		   the home switch on it's final, low speed pass.  It
		   terminates when the switch is detected. If the move ends
		   or hits a limit before it hits the switch, the home is
		   aborted. */
		/* have we hit the home switch yet? */
		if (home_sw_rise) {
		    /* yes, where do we go next? */
		    if (joint->home_flags & HOME_USE_INDEX) {
			/* look for index pulse */
			joint->home_state = HOME_INDEX_SEARCH_WAIT;
			immediate_state = 1;
			break;
		    } else {
			/* no index pulse, stop motion */
			joint->free_tp_enable = 0;
			/* go to next step */
			joint->home_state = HOME_SET_FINAL_POSITION;
			immediate_state = 1;
			break;
		    }
		}
		home_do_moving_checks(joint);
		break;

	    case HOME_FALL_SEARCH_START:
		/* This state is called to start the final search for the
		   point where the home switch releases.  It moves at
		   'latch_vel' and looks for a falling edge on the switch */
		/* is the joint already moving? */
		if (joint->free_tp_active) {
		    /* yes, reset delay, wait until joint stops */
		    joint->home_pause_timer = 0;
		    break;
		}
		/* has delay timed out? */
		if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
		    /* no, update timer and wait some more */
		    joint->home_pause_timer++;
		    break;
		}
		joint->home_pause_timer = 0;
		/* set up a move at 'latch_vel' to locate the switch */
		home_start_move(joint, joint->home_latch_vel);
		/* next state */
		joint->home_state = HOME_FALL_SEARCH_WAIT;
		break;

	    case HOME_FALL_SEARCH_WAIT:
		/* This state is called while the machine is moving away from
		   the home switch on it's final, low speed pass.  It
		   terminates when the switch is cleared. If the move ends or
		   hits a limit before it clears the switch, the home is
		   aborted. */
		/* have we cleared the home switch yet? */
		if (home_sw_fall) {
		    /* yes, where do we go next? */
		    if (joint->home_flags & HOME_USE_INDEX) {
			/* look for index pulse */
			joint->home_state = HOME_INDEX_SEARCH_WAIT;
			immediate_state = 1;
			break;
		    } else {
			/* no index pulse, stop motion */
			joint->free_tp_enable = 0;
			/* go to next step */
			joint->home_state = HOME_SET_FINAL_POSITION;
			immediate_state = 1;
			break;
		    }
		}
		home_do_moving_checks(joint);
		break;

	    case HOME_INDEX_SEARCH_WAIT:
		/* This state is called after the machine has made a low
		   speed pass to determine the limit switch location. It
		   continues at low speed until an index pulse is detected,
		   at which point it latches the final home position.  If the
		   move ends or hits a limit before an index pulse occurs, the 
		   home is aborted. */
		/* have we gotten an index pulse yet? */
		if (joint->index_pulse_edge) {
		    /* yes, stop motion */
		    joint->free_tp_enable = 0;
		    /* go to next step */
		    joint->home_state = HOME_SET_FINAL_POSITION;
		    immediate_state = 1;
		    break;
		}
		home_do_moving_checks(joint);
		break;

	    case HOME_SET_FINAL_POSITION:
		/* This state is called when the machine has determined the
		   home position as accurately as possible.  It sets the
		   current joint position to 'home_offset', which is the
		   location of the home switch (or index pulse) in joint
		   coordinates. */
		/* set the current position to 'home_offset' */
		offset = joint->home_offset - joint->pos_fb;
		/* this moves the internal position but does not affect the
		   motor position */
		joint->pos_cmd += offset;
		joint->pos_fb += offset;
		joint->free_pos_cmd += offset;
		joint->motor_offset -= offset;
		/* next state */
		joint->home_state = HOME_FINAL_MOVE_START;
		immediate_state = 1;
		break;

	    case HOME_FINAL_MOVE_START:
		/* This state is called once the joint coordinate system is
		   set properly.  It moves to the actual 'home' position,
		   which is not neccessarily the position of the home switch
		   or index pulse. */
		/* is the joint already moving? */
		if (joint->free_tp_active) {
		    /* yes, reset delay, wait until joint stops */
		    joint->home_pause_timer = 0;
		    break;
		}
		/* has delay timed out? */
		if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
		    /* no, update timer and wait some more */
		    joint->home_pause_timer++;
		    break;
		}
		joint->home_pause_timer = 0;
		/* plan a move to home position */
		joint->free_pos_cmd = joint->home;
		/* do the move at max speed */
		/*! \todo FIXME - should this be search_vel? or another user
		   specified speed? or is a rapid OK? */
		joint->free_vel_lim = joint->vel_limit;
		/* start the move */
		joint->free_tp_enable = 1;
		joint->home_state = HOME_FINAL_MOVE_WAIT;
		break;

	    case HOME_FINAL_MOVE_WAIT:
		/* This state is called while the machine makes its final
		   move to the home position.  It terminates when the machine 
		   arrives at the final location. If the move hits a limit
		   before it arrives, the home is aborted. */
		/* have we arrived (and stopped) at home? */
		if (!joint->free_tp_active) {
		    /* yes, stop motion */
		    joint->free_tp_enable = 0;
		    /* we're finally done */
		    joint->home_state = HOME_FINISHED;
		    immediate_state = 1;
		    break;
		}
		if (joint->pos_limit_latch || joint->neg_limit_latch) {
		    /* on limit, check to see if we should trip */
		    if (!(joint->home_flags & HOME_IGNORE_LIMITS)) {
			/* not ignoring limits, time to quit */
			reportError("hit limit in home state %d",
			    joint->home_state);
			joint->home_state = HOME_ABORT;
			immediate_state = 1;
			break;
		    }
		}
		break;

	    case HOME_FINISHED:
		SET_JOINT_HOMING_FLAG(joint, 0);
		SET_JOINT_HOMED_FLAG(joint, 1);
		SET_JOINT_AT_HOME_FLAG(joint, 1);
		joint->home_state = HOME_IDLE;
		immediate_state = 1;
		break;

	    case HOME_ABORT:
		SET_JOINT_HOMING_FLAG(joint, 0);
		SET_JOINT_HOMED_FLAG(joint, 0);
		SET_JOINT_AT_HOME_FLAG(joint, 0);
		joint->free_tp_enable = 0;
		joint->home_state = HOME_IDLE;
		immediate_state = 1;
		break;

	    default:
		/* should never get here */
		reportError("unknown state '%d' during homing",
		    joint->home_state);
		joint->home_state = EMCMOT_ABORT;
		immediate_state = 1;
		break;
	    }			/* end of switch(joint->home_state) */
	} while (immediate_state);
    }
}

static void get_pos_cmds(void)
{
    int joint_num, all_homed, all_at_home, result;
    emcmot_joint_t *joint;
    double positions[EMCMOT_MAX_AXIS];
/*! \todo Another #if 0 */
#if 0
    static int interpolationCounter = 0;
#endif
    double old_pos_cmd;
    double max_dv, tiny_dp, pos_err, vel_req, vel_lim;

    /* used in teleop mode to compute the max accell requested */
    double accell_mag;

    /* RUN MOTION CALCULATIONS: */

    /* run traj planner code depending on the state */
    switch ( emcmotStatus->motion_state) {
    case EMCMOT_MOTION_FREE:
	/* in free mode, each joint is planned independently */
	/* Each joint has a very simple "trajectory planner".  If the planner
	   is enabled (free_tp_enable), then it moves toward free_pos_cmd at
	   free_vel_lim, obeying the joint's accel and velocity limits, and
	   stopping when it gets there.  If it is not enabled, it stops as
	   quickly as possible, again obeying the accel limit.  When
	   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++) {
	    /* point to joint struct */
	    joint = &joints[joint_num];
	    //AJ: only need to worry about free mode if joint is active
	    if (GET_JOINT_ACTIVE_FLAG(joint)) {
		joint->free_tp_active = 0;
		/* compute max change in velocity per servo period */
		max_dv = joint->acc_limit * servo_period;
		/* compute a tiny position range, to be treated as zero */
		tiny_dp = max_dv * servo_period * 0.001;
		/* calculate desired velocity */
		if (joint->free_tp_enable) {
		    /* planner enabled, request a velocity that tends to drive
		       pos_err to zero, but allows for stopping without position
		       overshoot */
		    pos_err = joint->free_pos_cmd - joint->pos_cmd;
		    /* positive and negative errors require some sign flipping to
		       avoid sqrt(negative) */
		    if (pos_err > tiny_dp) {
			vel_req =
			    -max_dv + sqrt(2.0 * joint->acc_limit * pos_err +
			    max_dv * max_dv);
			/* mark joint active */
			joint->free_tp_active = 1;
		    } else if (pos_err < -tiny_dp) {
			vel_req =
			    max_dv - sqrt(-2.0 * joint->acc_limit * pos_err +
			    max_dv * max_dv);
			/* mark joint active */
			joint->free_tp_active = 1;
		    } else {
			/* within 'tiny_dp' of desired pos, no need to move */
			vel_req = 0.0;
		    }
		} else {
		    /* planner disabled, request zero velocity */
		    vel_req = 0.0;
		    /* and set command to present position to avoid movement when
		       next enabled */
		    joint->free_pos_cmd = joint->pos_cmd;
		}
		/* if we move at all, clear AT_HOME flag */
		if (joint->free_tp_active) {
		    SET_JOINT_AT_HOME_FLAG(joint, 0);
		}
		/* velocity limit = planner limit * global scale factor */
		/* the global factor is used for feedrate override */
		vel_lim = joint->free_vel_lim * emcmotStatus->qVscale;
		/* must not be greater than the joint physical limit */
		if (vel_lim > joint->vel_limit) {
		    vel_lim = joint->vel_limit;
		}
		/* limit velocity request */
		if (vel_req > vel_lim) {
		    vel_req = vel_lim;
		} else if (vel_req < -vel_lim) {
		    vel_req = -vel_lim;
		}
		/* ramp velocity toward request at axis accel limit */
		if (vel_req > joint->vel_cmd + max_dv) {
		    joint->vel_cmd += max_dv;
		} else if (vel_req < joint->vel_cmd - max_dv) {
		    joint->vel_cmd -= max_dv;
		} else {
		    joint->vel_cmd = vel_req;
		}
		/* check for still moving */
		if (joint->vel_cmd != 0.0) {
		    /* yes, mark joint active */
		    joint->free_tp_active = 1;
		}
		/* integrate velocity to get new position */
		joint->pos_cmd += joint->vel_cmd * servo_period;
		/* copy to coarse_pos */
		joint->coarse_pos = joint->pos_cmd;
		/* update joint status flag and overall status flag */
		if ( joint->free_tp_active ) {
		    /* active TP means we're moving, so not in position */
		    SET_JOINT_INPOS_FLAG(joint, 0);
		    SET_MOTION_INPOS_FLAG(0);
		    /* check to see if this move is with limits disabled */
		    if ( emcmotStatus->overrideLimits ) {
			emcmotDebug->overriding = 1;
		    }
		} else {
		    SET_JOINT_INPOS_FLAG(joint, 1);
		}
	    }//if (GET_JOINT_ACTIVE_FLAG(join))
	}//for loop for joints
	/* if overriding is true and we're in position, the jog
	   is complete, and the limits should be re-enabled */
	if ( (emcmotDebug->overriding ) && ( GET_MOTION_INPOS_FLAG() ) ) {
	    emcmotStatus->overrideLimits = 0;
	    emcmotDebug->overriding = 0;
	}
	/*! \todo FIXME - this should run at the traj rate */
	all_homed = 1;
	all_at_home = 1;
	/* copy joint position feedback to local array */