Untitled

static void handle_jogwheels(void)
{
    int joint_num;
    emcmot_joint_t *joint;
    axis_hal_t *axis_data;
    int new_jog_counts, delta;
    double distance, pos;

    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];
	if (!GET_JOINT_ACTIVE_FLAG(joint)) {
	    /* if joint is not active, skip it */
	    continue;
	}
	/* get counts from jogwheel */
	new_jog_counts = *(axis_data->jog_counts);
	delta = new_jog_counts - joint->old_jog_counts;
	/* save value for next time */
	joint->old_jog_counts = new_jog_counts;
	/* did the wheel move? */
	if ( delta == 0 ) {
	    /* no, nothing to do, except clear the JOGWHEEL souce flag */
	    if (joint->free_tp_source == FREE_TP_SOURCE_JOGWHEEL)
		joint->free_tp_source = FREE_TP_SOURCE_NONE;
	    continue;
	}
	/* must be in free mode and enabled */
	if (GET_MOTION_COORD_FLAG()) {
	    continue;
	}
	if (!GET_MOTION_ENABLE_FLAG()) {
	    continue;
	}
	if ( first_pass ) {
	    continue;
	}
	/* the jogwheel input for this axis must be enabled */
	if ( *(axis_data->jog_enable) == 0 ) {
	    continue;
	}
	/* calculate distance to jog */
	distance = delta * *(axis_data->jog_scale);
	/* check for joint already on soft limit */
	if (distance > 0.0 && GET_JOINT_PSL_FLAG(joint)) {
	    continue;
	}
	if (distance < 0.0 && GET_JOINT_NSL_FLAG(joint)) {
	    continue;
	}
	/* check for joint already on hard limit */
	if (distance > 0.0 && GET_JOINT_PHL_FLAG(joint)) {
	    continue;
	}
	if (distance < 0.0 && GET_JOINT_NHL_FLAG(joint)) {
	    continue;
	}
	/* calc target position for jog */
	pos = joint->free_pos_cmd + distance;
	/* don't jog past limits */
	refresh_jog_limits(joint);
	if (pos > joint->max_jog_limit) {
	    continue;
	}
	if (pos < joint->min_jog_limit) {
	    continue;
	}
	/* see if it's safe for us to take control of the free planner... */
        if (joint->free_tp_source == FREE_TP_SOURCE_HOME) {
            continue;
        }
        if (joint->free_tp_active && joint->free_tp_source != FREE_TP_SOURCE_JOGWHEEL) {
            continue;
        }
        /* set target position and use full velocity */
        joint->free_pos_cmd = pos;
        joint->free_vel_lim = joint->vel_limit;
        /* and let it go */
        joint->free_tp_enable = 1;
        joint->free_tp_source = FREE_TP_SOURCE_JOGWHEEL;
        
	SET_JOINT_ERROR_FLAG(joint, 0);
	/* clear axis homed flag(s) if we don't have forward kins.
	   Otherwise, a transition into coordinated mode will incorrectly
	   assume the homed position. Do all if they've all been moved
	   since homing, otherwise just do this one */
	clearHomes(joint_num);
    }
}