motion.c

Source code for an Numeric Cmputer

    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.debug-bit-1");
    retval =
	hal_param_bit_new(buf, HAL_RD, &(emcmot_hal_data->debug_bit_1),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.debug-float-0");
    retval =
	hal_param_float_new(buf, HAL_RD, &(emcmot_hal_data->debug_float_0),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "motion.debug-float-1");
    retval =
	hal_param_float_new(buf, HAL_RD, &(emcmot_hal_data->debug_float_1),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    // FIXME - debug only, remove later
    // export HAL parameters for some trajectory planner internal variables
    // so they can be scoped
    rtapi_snprintf(buf, HAL_NAME_LEN, "traj.pos_out");
    retval =
	hal_param_float_new(buf, HAL_RD, &(emcmot_hal_data->traj_pos_out),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "traj.vel_out");
    retval =
	hal_param_float_new(buf, HAL_RD, &(emcmot_hal_data->traj_vel_out),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "traj.active_tc");
    retval =
	hal_param_u8_new(buf, HAL_RD, &(emcmot_hal_data->traj_active_tc),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    for ( n = 0 ; n < 4 ; n++ ) {
	rtapi_snprintf(buf, HAL_NAME_LEN, "tc.%d.pos", n);
	retval = hal_param_float_new(buf, HAL_RD, &(emcmot_hal_data->tc_pos[n]), mot_comp_id);
	if (retval != 0) {
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "tc.%d.vel", n);
	retval = hal_param_float_new(buf, HAL_RD, &(emcmot_hal_data->tc_vel[n]), mot_comp_id);
	if (retval != 0) {
	    return retval;
	}
	rtapi_snprintf(buf, HAL_NAME_LEN, "tc.%d.acc", n);
	retval = hal_param_float_new(buf, HAL_RD, &(emcmot_hal_data->tc_acc[n]), mot_comp_id);
	if (retval != 0) {
	    return retval;
	}
    }
    // end of exporting trajectory planner internals

    /* initialize machine wide pins and parameters */
    *(emcmot_hal_data->probe_input) = 0;
    /* default value of enable is TRUE, so simple machines
       can leave it disconnected */
    *(emcmot_hal_data->enable) = 1;
    
    /* motion synched dio, init to not enabled */
    for (n = 0; n < EMCMOT_MAX_DIO; n++) {
	 *(emcmot_hal_data->synch_do[n]) = 0;
    }
    
    /*! \todo FIXME - these don't really need initialized, since they are written
       with data from the emcmotStatus struct */
    emcmot_hal_data->motion_enabled = 0;
    emcmot_hal_data->in_position = 0;
    emcmot_hal_data->coord_mode = 0;
    emcmot_hal_data->teleop_mode = 0;
    emcmot_hal_data->coord_error = 0;

    /* init debug parameters */
    emcmot_hal_data->debug_bit_0 = 0;
    emcmot_hal_data->debug_bit_1 = 0;
    emcmot_hal_data->debug_float_0 = 0.0;
    emcmot_hal_data->debug_float_1 = 0.0;

    /* export axis pins and parameters */
    for (n = 0; n < EMCMOT_MAX_AXIS; n++) {
	/* point to axis data */
	axis_data = &(emcmot_hal_data->axis[n]);
	/* export all vars */
	retval = export_axis(n, axis_data);
	if (retval != 0) {
	    rtapi_print_msg(RTAPI_MSG_ERR,
		"MOTION: axis %d pin/param export failed\n", n);
	    return -1;
	}
	/* init axis pins and parameters */
	/*! \todo FIXME - struct members are in a state of flux - make sure to
	   update this - most won't need initing anyway */
	*(axis_data->amp_enable) = 0;
	axis_data->home_state = 0;
	/* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now,
	   because it is always supported. */
/*! \todo Another #if 0 */
#if 0
	*(axis_data->mode) = EXT_ENCODER_INDEX_MODEL_RAW;
	*(axis_data->reset) = 0;
#endif
    }
    /* Done! */
    rtapi_print_msg(RTAPI_MSG_INFO,
	"MOTION: init_hal_io() complete, %d axes.\n", EMCMOT_MAX_AXIS);
    return 0;
}

static int export_axis(int num, axis_hal_t * addr)
{
    int retval, msg;
    char buf[HAL_NAME_LEN + 2];

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    /* export axis pins */
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.motor-pos-cmd", num);
    retval =
	hal_pin_float_new(buf, HAL_WR, &(addr->motor_pos_cmd), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.motor-pos-fb", num);
    retval =
	hal_pin_float_new(buf, HAL_RD, &(addr->motor_pos_fb), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.pos-lim-sw-in", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->pos_lim_sw), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.neg-lim-sw-in", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->neg_lim_sw), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.home-sw-in", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->home_sw), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.index-pulse-in", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->index_pulse), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.amp-enable-out", num);
    retval = hal_pin_bit_new(buf, HAL_WR, &(addr->amp_enable), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.amp-fault-in", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->amp_fault), mot_comp_id);
    if (retval != 0) {
	return retval;
    }

    /* export axis parameters */
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.coarse-pos-cmd", num);
    retval =
	hal_param_float_new(buf, HAL_RD, &(addr->coarse_pos_cmd),
	mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.joint-pos-cmd", num);
    retval =
	hal_param_float_new(buf, HAL_RD, &(addr->joint_pos_cmd), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.joint-vel-cmd", num);
    retval =
	hal_param_float_new(buf, HAL_RD, &(addr->joint_vel_cmd), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.backlash-corr", num);
    retval =
	hal_param_float_new(buf, HAL_RD, &(addr->backlash_corr), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.backlash-filt", num);
    retval =
	hal_param_float_new(buf, HAL_RD, &(addr->backlash_filt), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.joint-pos-fb", num);
    retval =
	hal_param_float_new(buf, HAL_RD, &(addr->joint_pos_fb), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.f-error", num);
    retval = hal_param_float_new(buf, HAL_RD, &(addr->f_error), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.f-error-lim", num);
    retval =
	hal_param_float_new(buf, HAL_RD, &(addr->f_error_lim), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.free-pos-cmd", num);
    retval =
	hal_param_float_new(buf, HAL_RD, &(addr->free_pos_cmd), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.free-vel-lim", num);
    retval =
	hal_param_float_new(buf, HAL_RD, &(addr->free_vel_lim), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.free-tp-enable", num);
    retval =
	hal_param_bit_new(buf, HAL_RD, &(addr->free_tp_enable), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.active", num);
    retval = hal_param_bit_new(buf, HAL_RD, &(addr->active), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.in-position", num);
    retval =
	hal_param_bit_new(buf, HAL_RD, &(addr->in_position), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.error", num);
    retval = hal_param_bit_new(buf, HAL_RD, &(addr->error), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.pos-soft-limit", num);
    retval = hal_param_bit_new(buf, HAL_RD, &(addr->psl), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.neg-soft-limit", num);
    retval = hal_param_bit_new(buf, HAL_RD, &(addr->nsl), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.pos-hard-limit", num);
    retval = hal_param_bit_new(buf, HAL_RD, &(addr->phl), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.neg-hard-limit", num);
    retval = hal_param_bit_new(buf, HAL_RD, &(addr->nhl), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.homing", num);
    retval = hal_param_bit_new(buf, HAL_RD, &(addr->homing), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.homed", num);
    retval = hal_param_bit_new(buf, HAL_RD, &(addr->homed), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.f-errored", num);
    retval = hal_param_bit_new(buf, HAL_RD, &(addr->f_errored), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.faulted", num);
    retval = hal_param_bit_new(buf, HAL_RD, &(addr->faulted), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.home-state", num);
    retval = hal_param_s8_new(buf, HAL_RD, &(addr->home_state), mot_comp_id);
    if (retval != 0) {
	return retval;
    }

/*! \todo FIXME - these have been temporarily? deleted */
/*! \todo Another #if 0 */
#if 0
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.mode", num);
    retval = hal_pin_u32_new(buf, HAL_WR, &(addr->mode), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.model", num);
    retval = hal_pin_u32_new(buf, HAL_RD, &(addr->model), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.reset", num);
    retval = hal_pin_bit_new(buf, HAL_WR, &(addr->reset), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.latch", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->latch), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
    rtapi_snprintf(buf, HAL_NAME_LEN, "axis.%d.index", num);
    retval = hal_pin_bit_new(buf, HAL_RD, &(addr->index), mot_comp_id);
    if (retval != 0) {
	return retval;
    }
#endif

    /* restore saved message level */
    rtapi_set_msg_level(msg);

    return 0;
}

/* init_comm_buffers() allocates and initializes the command,
   status, and error buffers used to communicate with the user
   space parts of emc.
*/
static int init_comm_buffers(void)
{
    int joint_num;
    emcmot_joint_t *joint;
    int retval;

    rtapi_print_msg(RTAPI_MSG_INFO,
	"MOTION: init_comm_buffers() starting...\n");

    emcmotStruct = 0;
    emcmotDebug = 0;
    emcmotStatus = 0;
    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) {
	rtapi_print_msg(RTAPI_MSG_ERR,
	    "MOTION: rtapi_shmem_new failed, returned %d\n", emc_shmem_id);
	return -1;
    }
    retval = rtapi_shmem_getptr(emc_shmem_id, (void **) &emcmotStruct);