/**
\brief Export pins for component(s)
\param
num: component number
addr: pointer to array of the num^th FOC channel data
*/
static int exportFOCaxis(int num, FOC_data_t * addr)
{
int retval = 0;
//
// PINS
//
// make available setpoint in hal
if ( (retval = hal_pin_float_newf(HAL_IN, &(addr->setpoint), comp_id, "hal_zed_can.%d.setpoint", num) ) < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: pin setpoint export failed with err=%d", retval);
hal_exit(comp_id);
return -1;
}
// make available position feedback in hal
if ( (retval = hal_pin_float_newf(HAL_OUT, &(addr->feedback), comp_id, "hal_zed_can.%d.feedback", num) ) < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: pin feedback export failed with err=%d", retval);
hal_exit(comp_id);
return -1;
}
//
// PARAMS
//
// instantiate a parameter
retval = hal_param_float_newf(HAL_RW, &(addr->dbg), comp_id, "hal_zed_can.%d.debug", num);
// check for failed debug space mapping
if(retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: param debug export failed with err=%d", retval);
hal_exit(comp_id);
return -1;
}
// instantiate parameter
retval = hal_pin_u32_newf(HAL_OUT, &(addr->focstatus), comp_id, "hal_zed_can.%d.status", num);
// check for failed debug space mapping
if(retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: pin status export failed with err=%d" ,retval);
hal_exit(comp_id);
return -1;
}
// instantiate parameter
retval = hal_pin_u32_newf(HAL_OUT, &(addr->focerror), comp_id, "hal_zed_can.%d.error", num);
// check for failed debug space mapping
if(retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: param error export failed with err=%d",retval);
hal_exit(comp_id);
return -1;
}
// completed successfully
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: exportFOCaxis(%d) completed successfully.\n", num);
return 0;
}
int main(int argc, char **argv)
{
int retval = 0;
modbus_t *mb_ctx;
haldata_t *haldata;
slavedata_t slavedata;
int slave;
int hal_comp_id;
struct timespec loop_timespec, remaining;
int baud, bits, stopbits, verbose;
char *device, *endarg;
char parity;
int opt;
int argindex, argvalue;
done = 0;
// assume that nothing is specified on the command line
baud = 38400;
bits = 8;
stopbits = 1;
verbose = 0;
device = "/dev/ttyS0";
parity = 'O';
/* slave / register info */
slave = 1;
slavedata.read_reg_start = START_REGISTER_R;
slavedata.read_reg_count = NUM_REGISTERS_R;
slavedata.write_reg_start = START_REGISTER_W;
slavedata.write_reg_count = NUM_REGISTERS_R;
// process command line options
while ((opt=getopt_long(argc, argv, option_string, long_options, NULL)) != -1) {
switch(opt) {
case 'b': // serial data bits, probably should be 8 (and defaults to 8)
argindex=match_string(optarg, bitstrings);
if (argindex<0) {
printf("gs2_vfd: ERROR: invalid number of bits: %s\n", optarg);
retval = -1;
goto out_noclose;
}
bits = atoi(bitstrings[argindex]);
break;
case 'd': // device name, default /dev/ttyS0
// could check the device name here, but we'll leave it to the library open
if (strlen(optarg) > FILENAME_MAX) {
printf("gs2_vfd: ERROR: device node name is too long: %s\n", optarg);
retval = -1;
goto out_noclose;
}
device = strdup(optarg);
break;
case 'g':
case 'v':
verbose = 1;
break;
case 'n': // module base name
if (strlen(optarg) > HAL_NAME_LEN-20) {
printf("gs2_vfd: ERROR: HAL module name too long: %s\n", optarg);
retval = -1;
goto out_noclose;
}
modname = strdup(optarg);
break;
case 'p': // parity, should be a string like "even", "odd", or "none"
argindex=match_string(optarg, paritystrings);
if (argindex<0) {
printf("gs2_vfd: ERROR: invalid parity: %s\n", optarg);
retval = -1;
goto out_noclose;
}
parity = paritychars[argindex];
break;
case 'r': // Baud rate, 38400 default
argindex=match_string(optarg, ratestrings);
if (argindex<0) {
printf("gs2_vfd: ERROR: invalid baud rate: %s\n", optarg);
retval = -1;
goto out_noclose;
}
baud = atoi(ratestrings[argindex]);
break;
case 's': // stop bits, defaults to 1
argindex=match_string(optarg, stopstrings);
if (argindex<0) {
printf("gs2_vfd: ERROR: invalid number of stop bits: %s\n", optarg);
retval = -1;
goto out_noclose;
}
stopbits = atoi(stopstrings[argindex]);
break;
case 't': // target number (MODBUS ID), default 1
argvalue = strtol(optarg, &endarg, 10);
if ((*endarg != '\0') || (argvalue < 1) || (argvalue > 254)) {
printf("gs2_vfd: ERROR: invalid slave number: %s\n", optarg);
retval = -1;
goto out_noclose;
}
slave = argvalue;
break;
case 'h':
default:
usage(argc, argv);
exit(0);
break;
}
}
printf("%s: device='%s', baud=%d, parity='%c', bits=%d, stopbits=%d, address=%d, verbose=%d\n",
modname, device, baud, parity, bits, stopbits, slave, verbose);
/* point TERM and INT signals at our quit function */
/* if a signal is received between here and the main loop, it should prevent
some initialization from happening */
signal(SIGINT, quit);
signal(SIGTERM, quit);
/* Assume 38.4k O-8-1 serial settings, device 1 */
mb_ctx = modbus_new_rtu(device, baud, parity, bits, stopbits);
if (mb_ctx == NULL) {
printf("%s: ERROR: couldn't open modbus serial device: %s\n", modname, modbus_strerror(errno));
goto out_noclose;
}
/* the open has got to work, or we're out of business */
if (((retval = modbus_connect(mb_ctx))!=0) || done) {
printf("%s: ERROR: couldn't open serial device: %s\n", modname, modbus_strerror(errno));
goto out_noclose;
}
modbus_set_debug(mb_ctx, verbose);
modbus_set_slave(mb_ctx, slave);
/* create HAL component */
hal_comp_id = hal_init(modname);
if ((hal_comp_id < 0) || done) {
printf("%s: ERROR: hal_init failed\n", modname);
retval = hal_comp_id;
goto out_close;
}
/* grab some shmem to store the HAL data in */
haldata = (haldata_t *)hal_malloc(sizeof(haldata_t));
if ((haldata == 0) || done) {
printf("%s: ERROR: unable to allocate shared memory\n", modname);
retval = -1;
goto out_close;
}
retval = hal_pin_s32_newf(HAL_OUT, &(haldata->stat1), hal_comp_id, "%s.status-1", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_s32_newf(HAL_OUT, &(haldata->stat2), hal_comp_id, "%s.status-2", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->freq_cmd), hal_comp_id, "%s.frequency-command", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->freq_out), hal_comp_id, "%s.frequency-out", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->curr_out), hal_comp_id, "%s.output-current", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->DCBusV), hal_comp_id, "%s.DC-bus-volts", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->outV), hal_comp_id, "%s.output-voltage", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->RPM), hal_comp_id, "%s.motor-RPM", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->scale_freq), hal_comp_id, "%s.scale-frequency", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->power_factor), hal_comp_id, "%s.power-factor", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_OUT, &(haldata->load_pct), hal_comp_id, "%s.load-percentage", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_s32_newf(HAL_OUT, &(haldata->FW_Rev), hal_comp_id, "%s.firmware-revision", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_s32_newf(HAL_RW, &(haldata->errorcount), hal_comp_id, "%s.error-count", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_float_newf(HAL_RW, &(haldata->looptime), hal_comp_id, "%s.loop-time", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_s32_newf(HAL_RW, &(haldata->retval), hal_comp_id, "%s.retval", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_OUT, &(haldata->at_speed), hal_comp_id, "%s.at-speed", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_OUT, &(haldata->is_stopped), hal_comp_id, "%s.is-stopped", modname); // JET
if (retval!=0) goto out_closeHAL;
retval = hal_pin_float_newf(HAL_IN, &(haldata->speed_command), hal_comp_id, "%s.speed-command", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_on), hal_comp_id, "%s.spindle-on", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_fwd), hal_comp_id, "%s.spindle-fwd", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_IN, &(haldata->spindle_rev), hal_comp_id, "%s.spindle-rev", modname); //JET
if (retval!=0) goto out_closeHAL;
retval = hal_pin_bit_newf(HAL_IN, &(haldata->err_reset), hal_comp_id, "%s.err-reset", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_float_newf(HAL_RW, &(haldata->speed_tolerance), hal_comp_id, "%s.tolerance", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_float_newf(HAL_RW, &(haldata->motor_hz), hal_comp_id, "%s.nameplate-HZ", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_float_newf(HAL_RW, &(haldata->motor_RPM), hal_comp_id, "%s.nameplate-RPM", modname);
if (retval!=0) goto out_closeHAL;
retval = hal_param_s32_newf(HAL_RW, &(haldata->ack_delay), hal_comp_id, "%s.ack-delay", modname);
if (retval!=0) goto out_closeHAL;
/* make default data match what we expect to use */
*(haldata->stat1) = 0;
*(haldata->stat2) = 0;
*(haldata->freq_cmd) = 0;
*(haldata->freq_out) = 0;
*(haldata->curr_out) = 0;
*(haldata->DCBusV) = 0;
*(haldata->outV) = 0;
*(haldata->RPM) = 0;
*(haldata->scale_freq) = 0;
*(haldata->power_factor) = 0;
*(haldata->load_pct) = 0;
*(haldata->FW_Rev) = 0;
haldata->errorcount = 0;
haldata->looptime = 0.1;
haldata->motor_RPM = 1730;
haldata->motor_hz = 60;
haldata->speed_tolerance = 0.01;
haldata->ack_delay = 2;
*(haldata->err_reset) = 0;
*(haldata->spindle_on) = 0;
*(haldata->spindle_fwd) = 1;
*(haldata->spindle_rev) = 0;
haldata->old_run = -1; // make sure the initial value gets output
haldata->old_dir = -1;
haldata->old_err_reset = -1;
hal_ready(hal_comp_id);
/* here's the meat of the program. loop until done (which may be never) */
while (done==0) {
read_data(mb_ctx, &slavedata, haldata);
write_data(mb_ctx, &slavedata, haldata);
/* don't want to scan too fast, and shouldn't delay more than a few seconds */
if (haldata->looptime < 0.001) haldata->looptime = 0.001;
if (haldata->looptime > 2.0) haldata->looptime = 2.0;
loop_timespec.tv_sec = (time_t)(haldata->looptime);
loop_timespec.tv_nsec = (long)((haldata->looptime - loop_timespec.tv_sec) * 1000000000l);
nanosleep(&loop_timespec, &remaining);
}
retval = 0; /* if we get here, then everything is fine, so just clean up and exit */
out_closeHAL:
hal_exit(hal_comp_id);
out_close:
modbus_close(mb_ctx);
modbus_free(mb_ctx);
out_noclose:
return retval;
}
int rtapi_app_main(void)
{
char name[HAL_NAME_LEN + 1];
int n,i , retval, num_dac, num_enc;
unsigned int base=0x300;
/* only one port at the moment */
num_ports = 1;
n = 0;
#define ISA_BASE 0xC9000
#define ISA_MAX 0x100000 /* allgemeiner Speicherzugriff */
/* STEP 1: initialise the driver */
comp_id = hal_init("hal_evoreg");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: hal_init() failed\n");
return -1;
}
/* STEP 2: allocate shared memory for EVOREG data */
port_data_array = hal_malloc(num_ports * sizeof(evoreg_t));
if (port_data_array == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/*! \todo FIXME: Test memory area and setup the card */
port_data_array->io_base = ioremap(ISA_BASE, ISA_MAX - ISA_BASE);
rtapi_print_msg(RTAPI_MSG_ERR,"EVOREG: io_base: %p \n", port_data_array->io_base);
outw(0x82c9,base); /* set indexregister */
/* Set all outputs to zero */
writew(0, port_data_array->io_base + 0x20); /* digital out 0-15 */
writew(0, port_data_array->io_base + 0x40); /* digital out 16-23 */
writew(0, port_data_array->io_base + 0x60); /* DAC 1 */
writew(0, port_data_array->io_base + 0x80); /* DAC 2 */
writew(0, port_data_array->io_base + 0xa0); /* DAC 3 */
/* Reset Encoder's */
writew(0, port_data_array->io_base + 0x02); /* ENCODER 1 */
writew(0, port_data_array->io_base + 0x0a); /* ENCODER 2 */
writew(0, port_data_array->io_base + 0x12); /* ENCODER 3 */
/* STEP 3: export the pin(s) */
/* Export DAC pin's */
for ( num_dac=1; num_dac<=MAX_DAC; num_dac++) {
retval = hal_pin_float_newf(HAL_IN, &(port_data_array->dac_out[num_dac-1]),
comp_id, "evoreg.%d.dac-%02d-out", 1, num_dac);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* Export Encoder pin's */
for ( num_enc=1; num_enc<=MAX_ENC; num_enc++) {
retval = hal_pin_float_newf(HAL_OUT, &(port_data_array->position[num_enc - 1]),
comp_id, "evoreg.%d.position-%02d-in", 1, num_enc);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* Export IO pin's */
/* export write only HAL pin's for the input bit */
for ( i=0; i<=45;i++) {
retval += hal_pin_bit_newf(HAL_OUT, &(port_data_array->digital_in[i]),
comp_id, "evoreg.%d.pin-%02d-in", 1, i);
/* export another write only HAL pin for the same bit inverted */
/*
retval += hal_pin_bit_newf(HAL_OUT, &(port_data_array->digital_in[(2*i)+1]),
comp_id, "evoreg.%d.pin-%02d-in-not", 1, i); */
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* export read only HAL pin's for the output bit */
for ( i=0; i<=23;i++) {
retval += hal_pin_bit_newf(HAL_IN, &(port_data_array->digital_out[i]),
comp_id, "evoreg.%d.pin-%02d-out", 1, i);
/* export another read only HAL pin for the same bit inverted */
/*
retval += hal_pin_bit_newf(HAL_IN, &(port_data_array->digital_out[(2*i)+1]),
comp_id, "evoreg.%d.pin-%02d-out-not", 1, i)); */
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d var export failed with err=%i\n", n + 1,
retval);
hal_exit(comp_id);
return -1;
}
}
/* export parameter for scaling */
retval = hal_param_float_newf(HAL_RW, &(port_data_array->pos_scale),
comp_id, "evoreg.%d.position-scale", 1);
if (retval != 0) {
return retval;
}
/* STEP 4: export function */
rtapi_snprintf(name, sizeof(name), "evoreg.%d.update", n + 1);
retval = hal_export_funct(name, update_port, &(port_data_array[n]), 1, 0,
comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"EVOREG: ERROR: port %d write funct export failed\n", n + 1);
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"EVOREG: installed driver for %d card(s)\n", num_ports);
hal_ready(comp_id);
return 0;
}
int lcec_stmds5k_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) {
lcec_master_t *master = slave->master;
lcec_stmds5k_data_t *hal_data;
int err;
// initialize callbacks
slave->proc_read = lcec_stmds5k_read;
slave->proc_write = lcec_stmds5k_write;
// alloc hal memory
if ((hal_data = hal_malloc(sizeof(lcec_stmds5k_data_t))) == NULL) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "hal_malloc() for slave %s.%s failed\n", master->name, slave->name);
return -EIO;
}
memset(hal_data, 0, sizeof(lcec_stmds5k_data_t));
slave->hal_data = hal_data;
// read sdos
// B18 : torque reference
if ((hal_data->sdo_torque_reference = lcec_read_sdo(slave, 0x2212, 0x00, 4)) == NULL) {
return -EIO;
}
hal_data->torque_reference = (double)EC_READ_S32(ecrt_sdo_request_data(hal_data->sdo_torque_reference)) * STMDS5K_TORQUE_REF_DIV;
if (hal_data->torque_reference > 1e-20 || hal_data->torque_reference < -1e-20) {
hal_data->torque_reference_rcpt = 1.0 / hal_data->torque_reference;
} else {
hal_data->torque_reference_rcpt = 0.0;
}
// C01 : max rpm
if ((hal_data->sdo_speed_max_rpm = lcec_read_sdo(slave, 0x2401, 0x00, 4)) == NULL) {
return -EIO;
}
hal_data->speed_max_rpm = (double)EC_READ_S32(ecrt_sdo_request_data(hal_data->sdo_speed_max_rpm));
// D02 : setpoint max rpm
if ((hal_data->sdo_speed_max_rpm_sp = lcec_read_sdo(slave, 0x2602, 0x00, 2)) == NULL) {
return -EIO;
}
hal_data->speed_max_rpm_sp = (double)EC_READ_S16(ecrt_sdo_request_data(hal_data->sdo_speed_max_rpm_sp));
if (hal_data->speed_max_rpm_sp > 1e-20 || hal_data->speed_max_rpm_sp < -1e-20) {
hal_data->speed_max_rpm_sp_rcpt = 1.0 / hal_data->speed_max_rpm_sp;
} else {
hal_data->speed_max_rpm_sp_rcpt = 0.0;
}
// initialize POD entries
// E200 : device state byte
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x28c8, 0x00, &hal_data->dev_state_pdo_os, NULL);
// E100 : speed motor (x 0.1% relative to C01)
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2864, 0x00, &hal_data->speed_mot_pdo_os, NULL);
// E02 : torque motor filterd (x 0,1 Nm)
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2802, 0x00, &hal_data->torque_mot_pdo_os, NULL);
// D200 : speed state word
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x26c8, 0x00, &hal_data->speed_state_pdo_os, NULL);
// E09 : rotor position
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x2809, 0x00, &hal_data->pos_mot_pdo_os, NULL);
// A180 : Device Control Byte
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x20b4, 0x00, &hal_data->dev_ctrl_pdo_os, NULL);
// D230 : speed setpoint (x 0.1 % relative to D02, -200.0% .. 200.0%)
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x26e6, 0x00, &hal_data->speed_sp_rel_pdo_os, NULL);
// C230 : maximum torque (x 1%, 0% .. 200%)
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x24e6, 0x00, &hal_data->torque_max_pdo_os, NULL);
// export pins
if ((err = hal_pin_float_newf(HAL_IN, &(hal_data->vel_cmd), comp_id, "%s.%s.%s.srv-vel-cmd", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-cmd failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_fb), comp_id, "%s.%s.%s.srv-vel-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_fb_rpm), comp_id, "%s.%s.%s.srv-vel-fb-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-fb-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->vel_rpm), comp_id, "%s.%s.%s.srv-vel-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-vel-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_s32_newf(HAL_OUT, &(hal_data->enc_raw), comp_id, "%s.%s.%s.srv-enc-raw", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-enc-raw failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_u32_newf(HAL_OUT, &(hal_data->pos_raw_hi), comp_id, "%s.%s.%s.srv-pos-raw-hi", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-raw-hi failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_u32_newf(HAL_OUT, &(hal_data->pos_raw_lo), comp_id, "%s.%s.%s.srv-pos-raw-lo", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-raw-lo failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->pos_fb), comp_id, "%s.%s.%s.srv-pos-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb), comp_id, "%s.%s.%s.srv-torque-fb", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb_abs), comp_id, "%s.%s.%s.srv-torque-fb-abs", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb-abs failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(hal_data->torque_fb_pct), comp_id, "%s.%s.%s.srv-torque-fb-pct", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-fb-pct failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_float_newf(HAL_IN, &(hal_data->torque_lim), comp_id, "%s.%s.%s.srv-torque-lim", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-lim failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->stopped), comp_id, "%s.%s.%s.srv-stopped", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-stopped failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->at_speed), comp_id, "%s.%s.%s.srv-at-speed", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-at-speed failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->overload), comp_id, "%s.%s.%s.srv-overload", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-overload failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->ready), comp_id, "%s.%s.%s.srv-ready", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-ready failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->error), comp_id, "%s.%s.%s.srv-error", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-error failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->toggle), comp_id, "%s.%s.%s.srv-toggle", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-toggle failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->loc_ena), comp_id, "%s.%s.%s.srv-loc-ena", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-loc-ena failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->enable), comp_id, "%s.%s.%s.srv-enable", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->err_reset), comp_id, "%s.%s.%s.srv-err-reset", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-err-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->fast_ramp), comp_id, "%s.%s.%s.srv-fast-ramp", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-fast-ramp failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->brake), comp_id, "%s.%s.%s.srv-brake", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-brake failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IO, &(hal_data->index_ena), comp_id, "%s.%s.%s.srv-index-ena", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-index-ena failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(hal_data->pos_reset), comp_id, "%s.%s.%s.srv-pos-reset", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->on_home_neg), comp_id, "%s.%s.%s.srv-on-home-neg", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-on-home-neg failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(hal_data->on_home_pos), comp_id, "%s.%s.%s.srv-on-home-pos", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-on-home-pos failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
// export parameters
if ((err = hal_param_float_newf(HAL_RW, &(hal_data->pos_scale), comp_id, "%s.%s.%s.srv-pos-scale", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-pos-scale failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_param_float_newf(HAL_RO, &(hal_data->torque_reference), comp_id, "%s.%s.%s.srv-torque-ref", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-torque-ref failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_param_float_newf(HAL_RO, &(hal_data->speed_max_rpm), comp_id, "%s.%s.%s.srv-max-rpm", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-max-rpm failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_param_float_newf(HAL_RO, &(hal_data->speed_max_rpm_sp), comp_id, "%s.%s.%s.srv-max-rpm-sp", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-max-rpm-sp failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
if ((err = hal_param_s32_newf(HAL_RW, &(hal_data->home_raw), comp_id, "%s.%s.%s.srv-home-raw", LCEC_MODULE_NAME, master->name, slave->name)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-home-raw failed\n", LCEC_MODULE_NAME, master->name, slave->name);
return err;
}
// set default pin values
*(hal_data->vel_cmd) = 0.0;
*(hal_data->vel_fb) = 0.0;
*(hal_data->vel_fb_rpm) = 1.0;
*(hal_data->vel_rpm) = 0.0;
*(hal_data->pos_raw_hi) = 0;
*(hal_data->pos_raw_lo) = 0;
*(hal_data->pos_fb) = 0.0;
*(hal_data->torque_fb) = 0.0;
*(hal_data->torque_fb_abs) = 0.0;
*(hal_data->torque_fb_pct) = 0.0;
*(hal_data->torque_lim) = 1.0;
*(hal_data->stopped) = 0;
*(hal_data->at_speed) = 0;
*(hal_data->overload) = 0;
*(hal_data->ready) = 0;
*(hal_data->error) = 0;
*(hal_data->toggle) = 0;
*(hal_data->loc_ena) = 0;
*(hal_data->enable) = 0;
*(hal_data->err_reset) = 0;
*(hal_data->fast_ramp) = 0;
*(hal_data->brake) = 0;
*(hal_data->index_ena) = 0;
*(hal_data->pos_reset) = 0;
*(hal_data->enc_raw) = 0;
*(hal_data->on_home_neg) = 0;
*(hal_data->on_home_pos) = 0;
// initialize variables
hal_data->pos_scale = 1.0;
hal_data->do_init = 1;
hal_data->pos_cnt = 0;
hal_data->index_cnt = 0;
hal_data->last_pos_cnt = 0;
hal_data->pos_scale_old = hal_data->pos_scale + 1.0;
hal_data->pos_scale_rcpt = 1.0;
hal_data->pos_scale_cnt = 1.0;
hal_data->last_index_ena = 0;
hal_data->index_ref = 0;
hal_data->home_raw = 0;
return 0;
}
/* init_hal_io() exports HAL pins and parameters making data from
the realtime control module visible and usable by the world
*/
static int init_hal_io(void)
{
int n, retval;
joint_hal_t *joint_data;
rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_hal_io() starting...\n");
/* allocate shared memory for machine data */
emcmot_hal_data = hal_malloc(sizeof(emcmot_hal_data_t));
if (emcmot_hal_data == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: emcmot_hal_data malloc failed\n"));
return -1;
}
/* export machine wide hal pins */
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->probe_input), mot_comp_id, "motion.probe-input")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IO, &(emcmot_hal_data->spindle_index_enable), mot_comp_id, "motion.spindle-index-enable")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_on), mot_comp_id, "motion.spindle-on")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_forward), mot_comp_id, "motion.spindle-forward")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_reverse), mot_comp_id, "motion.spindle-reverse")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_brake), mot_comp_id, "motion.spindle-brake")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out), mot_comp_id, "motion.spindle-speed-out")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_abs), mot_comp_id, "motion.spindle-speed-out-abs")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps), mot_comp_id, "motion.spindle-speed-out-rps")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps_abs), mot_comp_id, "motion.spindle-speed-out-rps-abs")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_cmd_rps), mot_comp_id, "motion.spindle-speed-cmd-rps")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_inhibit), mot_comp_id, "motion.spindle-inhibit")) < 0) goto error;
*(emcmot_hal_data->spindle_inhibit) = 0;
// spindle orient pins
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_angle), mot_comp_id, "motion.spindle-orient-angle")) < 0) goto error;
if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_mode), mot_comp_id, "motion.spindle-orient-mode")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient), mot_comp_id, "motion.spindle-orient")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_locked), mot_comp_id, "motion.spindle-locked")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_oriented), mot_comp_id, "motion.spindle-is-oriented")) < 0) goto error;
if ((retval = hal_pin_s32_newf(HAL_IN, &(emcmot_hal_data->spindle_orient_fault), mot_comp_id, "motion.spindle-orient-fault")) < 0) goto error;
*(emcmot_hal_data->spindle_orient_angle) = 0.0;
*(emcmot_hal_data->spindle_orient_mode) = 0;
*(emcmot_hal_data->spindle_orient) = 0;
// if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->inpos_output), mot_comp_id, "motion.motion-inpos")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_revs), mot_comp_id, "motion.spindle-revs")) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_speed_in), mot_comp_id, "motion.spindle-speed-in")) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) < 0) goto error;
*emcmot_hal_data->spindle_is_atspeed = 1;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 0) goto error;
*(emcmot_hal_data->adaptive_feed) = 1.0;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_hold), mot_comp_id, "motion.feed-hold")) < 0) goto error;
*(emcmot_hal_data->feed_hold) = 0;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_inhibit), mot_comp_id, "motion.feed-inhibit")) < 0) goto error;
*(emcmot_hal_data->feed_inhibit) = 0;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->enable), mot_comp_id, "motion.enable")) < 0) goto error;
/* export motion-synched digital output pins */
/* export motion digital input pins */
for (n = 0; n < num_dio; n++) {
if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->synch_do[n]), mot_comp_id, "motion.digital-out-%02d", n)) < 0) goto error;
if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->synch_di[n]), mot_comp_id, "motion.digital-in-%02d", n)) < 0) goto error;
}
/* export motion analog input pins */
for (n = 0; n < num_aio; n++) {
if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->analog_output[n]), mot_comp_id, "motion.analog-out-%02d", n)) < 0) goto error;
if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->analog_input[n]), mot_comp_id, "motion.analog-in-%02d", n)) < 0) goto error;
}
/* export machine wide hal parameters */
retval =
hal_pin_bit_new("motion.motion-enabled", HAL_OUT, &(emcmot_hal_data->motion_enabled),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.in-position", HAL_OUT, &(emcmot_hal_data->in_position),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.coord-mode", HAL_OUT, &(emcmot_hal_data->coord_mode),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.teleop-mode", HAL_OUT, &(emcmot_hal_data->teleop_mode),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.coord-error", HAL_OUT, &(emcmot_hal_data->coord_error),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_new("motion.on-soft-limit", HAL_OUT, &(emcmot_hal_data->on_soft_limit),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_new("motion.current-vel", HAL_OUT, &(emcmot_hal_data->current_vel),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_new("motion.requested-vel", HAL_OUT, &(emcmot_hal_data->requested_vel),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_new("motion.distance-to-go", HAL_OUT, &(emcmot_hal_data->distance_to_go),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_pin_s32_new("motion.program-line", HAL_OUT, &(emcmot_hal_data->program_line),
mot_comp_id);
if (retval != 0) {
return retval;
}
/* export debug parameters */
/* these can be used to view any internal variable, simply change a line
in control.c:output_to_hal() and recompile */
retval =
hal_param_bit_new("motion.debug-bit-0", HAL_RO, &(emcmot_hal_data->debug_bit_0),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_bit_new("motion.debug-bit-1", HAL_RO, &(emcmot_hal_data->debug_bit_1),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-0", HAL_RO, &(emcmot_hal_data->debug_float_0),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-1", HAL_RO, &(emcmot_hal_data->debug_float_1),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-2", HAL_RO, &(emcmot_hal_data->debug_float_2),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("motion.debug-float-3", HAL_RO, &(emcmot_hal_data->debug_float_3),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_s32_new("motion.debug-s32-0", HAL_RO, &(emcmot_hal_data->debug_s32_0),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_s32_new("motion.debug-s32-1", HAL_RO, &(emcmot_hal_data->debug_s32_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
retval =
hal_param_float_new("traj.pos_out", HAL_RO, &(emcmot_hal_data->traj_pos_out),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_float_new("traj.vel_out", HAL_RO, &(emcmot_hal_data->traj_vel_out),
mot_comp_id);
if (retval != 0) {
return retval;
}
retval =
hal_param_u32_new("traj.active_tc", HAL_RO, &(emcmot_hal_data->traj_active_tc),
mot_comp_id);
if (retval != 0) {
return retval;
}
for ( n = 0 ; n < 4 ; n++ ) {
retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_pos[n]), mot_comp_id, "tc.%d.pos", n);
if (retval != 0) {
return retval;
}
retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_vel[n]), mot_comp_id, "tc.%d.vel", n);
if (retval != 0) {
return retval;
}
retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_acc[n]), mot_comp_id, "tc.%d.acc", n);
if (retval != 0) {
return retval;
}
}
// end of exporting trajectory planner internals
// export timing related HAL parameters so they can be scoped
retval =
hal_param_u32_new("motion.servo.last-period", HAL_RO, &(emcmot_hal_data->last_period), mot_comp_id);
if (retval != 0) {
return retval;
}
#ifdef HAVE_CPU_KHZ
retval =
hal_param_float_new("motion.servo.last-period-ns", HAL_RO, &(emcmot_hal_data->last_period_ns), mot_comp_id);
if (retval != 0) {
return retval;
}
#endif
retval =
hal_param_u32_new("motion.servo.overruns", HAL_RW, &(emcmot_hal_data->overruns), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.x", HAL_OUT, &(emcmot_hal_data->tooloffset_x), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.y", HAL_OUT, &(emcmot_hal_data->tooloffset_y), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.z", HAL_OUT, &(emcmot_hal_data->tooloffset_z), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.a", HAL_OUT, &(emcmot_hal_data->tooloffset_a), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.b", HAL_OUT, &(emcmot_hal_data->tooloffset_b), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.c", HAL_OUT, &(emcmot_hal_data->tooloffset_c), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.u", HAL_OUT, &(emcmot_hal_data->tooloffset_u), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.v", HAL_OUT, &(emcmot_hal_data->tooloffset_v), mot_comp_id);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_new("motion.tooloffset.w", HAL_OUT, &(emcmot_hal_data->tooloffset_w), mot_comp_id);
if (retval != 0) {
return retval;
}
/* 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 < num_dio; n++) {
*(emcmot_hal_data->synch_do[n]) = 0;
*(emcmot_hal_data->synch_di[n]) = 0;
}
for (n = 0; n < num_aio; n++) {
*(emcmot_hal_data->analog_output[n]) = 0.0;
*(emcmot_hal_data->analog_input[n]) = 0.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;
*(emcmot_hal_data->on_soft_limit) = 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;
emcmot_hal_data->debug_float_2 = 0.0;
emcmot_hal_data->debug_float_3 = 0.0;
emcmot_hal_data->overruns = 0;
emcmot_hal_data->last_period = 0;
/* export joint pins and parameters */
for (n = 0; n < num_joints; n++) {
/* point to axis data */
joint_data = &(emcmot_hal_data->joint[n]);
/* export all vars */
retval = export_joint(n, joint_data);
if (retval != 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
_("MOTION: joint %d pin/param export failed\n"), n);
return -1;
}
/* init axis pins and parameters */
/* FIXME - struct members are in a state of flux - make sure to
update this - most won't need initing anyway */
*(joint_data->amp_enable) = 0;
*(joint_data->home_state) = 0;
/* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now,
because it is always supported. */
}
/* Done! */
rtapi_print_msg(RTAPI_MSG_INFO,
"MOTION: init_hal_io() complete, %d axes.\n", n);
return 0;
error:
return retval;
}
