static int export_dac(int num, vti_struct * 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 pin for voltage received by the board() */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.dac-value", num);
retval = hal_pin_float_new(buf, HAL_IN, &addr->dac_value[num], comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for offset */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.dac-offset", num);
retval =
hal_param_float_new(buf, HAL_RW, &addr->dac_offset[num], comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for gain */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.dac-gain", num);
retval = hal_param_float_new(buf, HAL_RW, &addr->dac_gain[num], comp_id);
if (retval != 0) {
return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
/***********************************************************************
* LOCAL FUNCTION DEFINITIONS *
* these are functions used for exporting various HAL pins/prams *
************************************************************************/
static int export_counter(int num, vti_struct * 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 pin for counts captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.counts", num);
retval = hal_pin_s32_new(buf, HAL_OUT, &addr->count[num], comp_id);
if (retval != 0) {
return retval;
}
/* export pin for scaled position captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.position", num);
retval = hal_pin_float_new(buf, HAL_OUT, &addr->pos[num], comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for scaling */
rtapi_snprintf(buf, HAL_NAME_LEN, "vti.%d.position-scale", num);
retval = hal_param_float_new(buf, HAL_RW, &addr->pos_scale[num], comp_id);
if (retval != 0) {
return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
void halcmd_info(const char *format, ...) {
va_list ap;
if(rtapi_get_msg_level() < RTAPI_MSG_INFO) return;
fprintf(stderr, "%s:%d: ", halcmd_get_filename(), halcmd_get_linenumber());
va_start(ap, format);
vfprintf(stderr, format, ap);
va_end(ap);
}
static int export_dio_pins(int io_points)
{
int i, msg, retval=0;
/* 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 */
if (io_points == 0) return 0;
msg = rtapi_get_msg_level();
rtapi_set_msg_level(RTAPI_MSG_WARN);
for (i = 0; i < io_points; i++) {
/* point, direction, driver */
retval |= export_pin(i, vti_driver->dir_bits[i / 4], vti_driver);
}
/* restore saved message level */ rtapi_set_msg_level(msg);
return retval;
}
int main( int argc, char *argv[] )
{
int used=0, NumRung;
static int old_level ;
bindtextdomain("linuxcnc", EMC2_PO_DIR);
setlocale(LC_MESSAGES,"");
setlocale(LC_CTYPE,"");
textdomain("linuxcnc");
old_level = rtapi_get_msg_level();
compId=hal_init("classicladder"); //emc
if (compId<0) return -1; //emc
signal(SIGTERM,do_exit); //emc
InitModbusMasterBeforeReadConf( );
if (ClassicLadder_AllocAll())
{
char ProjectLoadedOk=TRUE;
process_options (argc, argv);
if (nogui==TRUE)
{
rtapi_print(_("INFO CLASSICLADDER- No ladder GUI requested-Realtime runs till HAL closes.\n"));
ClassicLadder_InitAllDatas( );
ProjectLoadedOk = LoadProjectFiles( InfosGene->CurrentProjectFileName );
if (pathswitch){ strcpy( InfosGene->CurrentProjectFileName, NewPath ); }
InfosGene->LadderState = STATE_RUN;
ClassicLadder_FreeAll(TRUE);
hal_ready(compId);
hal_exit(compId);
return 0;
} else {
for(NumRung=0;NumRung<NBR_RUNGS;NumRung++) { if ( RungArray[NumRung].Used ) used++; }
if((used==0) || ( (argc - optind) != 0) )
{
ClassicLadder_InitAllDatas( );
ProjectLoadedOk = LoadProjectFiles( InfosGene->CurrentProjectFileName );
InitGtkWindows( argc, argv );
UpdateAllGtkWindows();
if (pathswitch){ strcpy( InfosGene->CurrentProjectFileName, NewPath ); }
UpdateWindowTitleWithProjectName( );
MessageInStatusBar( ProjectLoadedOk?_("Project loaded and running"):_("Project failed to load..."));
if (!ProjectLoadedOk)
{
ClassicLadder_InitAllDatas( );
if (modmaster) { PrepareModbusMaster( ); }
}
}else{
InitGtkWindows( argc, argv );
UpdateAllGtkWindows();
if (pathswitch){ strcpy( InfosGene->CurrentProjectFileName, NewPath ); }
UpdateWindowTitleWithProjectName( );
MessageInStatusBar(_("GUI reloaded with existing ladder program"));
if (modmaster) { PrepareModbusMaster( ); }
}
if (modslave) { InitSocketServer( 0/*UseUdpMode*/, ModbusServerPort/*PortNbr*/); }
InfosGene->LadderState = STATE_RUN;
hal_ready(compId);
gtk_main();
rtapi_print(_("INFO CLASSICLADDER- Ladder GUI closed. Realtime runs till HAL closes\n"));
ClassicLadder_FreeAll(TRUE);
hal_exit(compId);
return 0;
}
}
rtapi_print(_("ERROR CLASSICLADDER- Ladder memory allocation error\n"));
ClassicLadder_FreeAll(TRUE);
rtapi_set_msg_level(old_level);
hal_exit(compId);
return 0;
}
int rtapi_app_main(void)
{
int retval;
dlist_init_entry(&head);
if ((comp_id = hal_xinit(TYPE_RT, 0, 0,
instantiate_encoder_pair,
delete_encoder_pair,
compname)) < 0)
return comp_id;
hal_export_xfunct_args_t u = {
.type = FS_XTHREADFUNC,
.funct.x = update,
.arg = &head,
.uses_fp = 1,
.reentrant = 0,
.owner_id = comp_id
};
if ((retval = hal_export_xfunctf(&u, "%s.update",
prefix)) < 0)
return retval;
hal_export_xfunct_args_t mp = {
.type = FS_XTHREADFUNC,
.funct.x = sample,
.arg = &head,
.uses_fp = 0,
.reentrant = 0,
.owner_id = comp_id
};
if ((retval = hal_export_xfunctf(&mp, "%s.sample",
prefix)) < 0)
return retval;
hal_ready(comp_id);
return 0;
}
void rtapi_app_exit(void)
{
hal_exit(comp_id);
}
static int instantiate_encoder_pair(const int argc, const char**argv)
{
encoder_pair_t *p;
int retval;
int msg;
const char* name;
if(argc >= 2)
name = argv[1];
else
HALFAIL_RC(EINVAL, "ERROR: insufficient args in argv");
/* 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. */
msg = rtapi_get_msg_level();
#ifndef VERBOSE_SETUP
rtapi_set_msg_level(RTAPI_MSG_WARN);
#endif
if ((retval = hal_inst_create(name, comp_id, sizeof(encoder_pair_t), (void **)&p)) < 0)
return retval;
p->inst_id = retval;
if ((retval = export_encoder_pair(name, p->inst_id, p)) != 0)
HALFAIL_RC(retval, "%s: ERROR: export(%s) failed", compname, name);
// append to instance list
dlist_init_entry(&p->list);
dlist_add_after(&p->list, &head);
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
static int export_counter(int num, counter_t * addr)
{
int retval, msg;
/* 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 pins for the quadrature inputs */
retval = hal_pin_bit_newf(HAL_IN, &(addr->phaseA), comp_id,
"encoder.%d.phase-A", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->phaseB), comp_id,
"encoder.%d.phase-B", num);
if (retval != 0) {
return retval;
}
/* export pin for the index input */
retval = hal_pin_bit_newf(HAL_IN, &(addr->phaseZ), comp_id,
"encoder.%d.phase-Z", num);
if (retval != 0) {
return retval;
}
/* export pin for the index enable input */
retval = hal_pin_bit_newf(HAL_IO, &(addr->index_ena), comp_id,
"encoder.%d.index-enable", num);
if (retval != 0) {
return retval;
}
/* export pin for the reset input */
retval = hal_pin_bit_newf(HAL_IN, &(addr->reset), comp_id,
"encoder.%d.reset", num);
if (retval != 0) {
return retval;
}
/* export pins for position latching */
retval = hal_pin_bit_newf(HAL_IN, &(addr->latch_in), comp_id,
"encoder.%d.latch-input", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->latch_rising), comp_id,
"encoder.%d.latch-rising", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->latch_falling), comp_id,
"encoder.%d.latch-falling", num);
if (retval != 0) {
return retval;
}
/* export parameter for raw counts */
retval = hal_pin_s32_newf(HAL_OUT, &(addr->raw_counts), comp_id,
"encoder.%d.rawcounts", num);
if (retval != 0) {
return retval;
}
/* export pin for counts captured by capture() */
retval = hal_pin_s32_newf(HAL_OUT, &(addr->count), comp_id,
"encoder.%d.counts", num);
if (retval != 0) {
return retval;
}
/* export pin for counts latched by capture() */
retval = hal_pin_s32_newf(HAL_OUT, &(addr->count_latch), comp_id,
"encoder.%d.counts-latched", num);
if (retval != 0) {
return retval;
}
/* export pin for minimum speed estimated by capture() */
retval = hal_pin_float_newf(HAL_IN, &(addr->min_speed), comp_id,
"encoder.%d.min-speed-estimate", num);
if (retval != 0) {
return retval;
}
/* export pin for scaled position captured by capture() */
retval = hal_pin_float_newf(HAL_OUT, &(addr->pos), comp_id,
"encoder.%d.position", num);
if (retval != 0) {
return retval;
}
/* export pin for scaled and interpolated position captured by capture() */
retval = hal_pin_float_newf(HAL_OUT, &(addr->pos_interp), comp_id,
"encoder.%d.position-interpolated", num);
if (retval != 0) {
return retval;
}
/* export pin for latched position captured by capture() */
retval = hal_pin_float_newf(HAL_OUT, &(addr->pos_latch), comp_id,
"encoder.%d.position-latched", num);
if (retval != 0) {
return retval;
}
/* export pin for scaled velocity captured by capture() */
retval = hal_pin_float_newf(HAL_OUT, &(addr->vel), comp_id,
"encoder.%d.velocity", num);
if (retval != 0) {
return retval;
}
/* export pin for scaling */
retval = hal_pin_float_newf(HAL_IO, &(addr->pos_scale), comp_id,
"encoder.%d.position-scale", num);
if (retval != 0) {
return retval;
}
/* export pin for x4 mode */
retval = hal_pin_bit_newf(HAL_IO, &(addr->x4_mode), comp_id,
"encoder.%d.x4-mode", num);
if (retval != 0) {
return retval;
}
/* export pin for counter mode */
retval = hal_pin_bit_newf(HAL_IO, &(addr->counter_mode), comp_id,
"encoder.%d.counter-mode", num);
if (retval != 0) {
return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
static int export_joint(int num, joint_hal_t * addr)
{
int retval, msg;
/* 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 joint pins */ //FIXME-AJ: changing these will bork configs, still we should do it
retval =
hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_cmd), mot_comp_id, "axis.%d.joint-pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_fb), mot_comp_id, "axis.%d.joint-pos-fb", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->motor_pos_cmd), mot_comp_id, "axis.%d.motor-pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->motor_offset), mot_comp_id, "axis.%d.motor-offset", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_IN, &(addr->motor_pos_fb), mot_comp_id, "axis.%d.motor-pos-fb", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->pos_lim_sw), mot_comp_id, "axis.%d.pos-lim-sw-in", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->neg_lim_sw), mot_comp_id, "axis.%d.neg-lim-sw-in", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->home_sw), mot_comp_id, "axis.%d.home-sw-in", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IO, &(addr->index_enable), mot_comp_id, "axis.%d.index-enable", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->amp_enable), mot_comp_id, "axis.%d.amp-enable-out", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->amp_fault), mot_comp_id, "axis.%d.amp-fault-in", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_s32_newf(HAL_IN, &(addr->jog_counts), mot_comp_id, "axis.%d.jog-counts", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_enable), mot_comp_id, "axis.%d.jog-enable", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IN, &(addr->jog_scale), mot_comp_id, "axis.%d.jog-scale", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_vel_mode), mot_comp_id, "axis.%d.jog-vel-mode", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->homing), mot_comp_id, "axis.%d.homing", num);
if (retval != 0) {
return retval;
}
/* export joint parameters */ //FIXME-AJ: changing these to joints will break configs.
retval =
hal_pin_float_newf(HAL_OUT, &(addr->coarse_pos_cmd),
mot_comp_id, "axis.%d.coarse-pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->joint_vel_cmd), mot_comp_id, "axis.%d.joint-vel-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->backlash_corr), mot_comp_id, "axis.%d.backlash-corr", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->backlash_filt), mot_comp_id, "axis.%d.backlash-filt", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->backlash_vel), mot_comp_id, "axis.%d.backlash-vel", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->f_error), mot_comp_id, "axis.%d.f-error", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->f_error_lim), mot_comp_id, "axis.%d.f-error-lim", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->free_pos_cmd), mot_comp_id, "axis.%d.free-pos-cmd", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_float_newf(HAL_OUT, &(addr->free_vel_lim), mot_comp_id, "axis.%d.free-vel-lim", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->free_tp_enable), mot_comp_id, "axis.%d.free-tp-enable", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->kb_jog_active), mot_comp_id, "axis.%d.kb-jog-active", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->wheel_jog_active), mot_comp_id, "axis.%d.wheel-jog-active", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->active), mot_comp_id, "axis.%d.active", num);
if (retval != 0) {
return retval;
}
retval =
hal_pin_bit_newf(HAL_OUT, &(addr->in_position), mot_comp_id, "axis.%d.in-position", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->error), mot_comp_id, "axis.%d.error", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->phl), mot_comp_id, "axis.%d.pos-hard-limit", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->nhl), mot_comp_id, "axis.%d.neg-hard-limit", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->homed), mot_comp_id, "axis.%d.homed", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->f_errored), mot_comp_id, "axis.%d.f-errored", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->faulted), mot_comp_id, "axis.%d.faulted", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_s32_newf(HAL_OUT, &(addr->home_state), mot_comp_id, "axis.%d.home-state", num);
if (retval != 0) {
return retval;
}
if(num >=3 && num <= 5) {
// for rotaries only...
retval = hal_pin_bit_newf(HAL_OUT, &(addr->unlock), mot_comp_id, "axis.%d.unlock", num);
if (retval != 0) return retval;
retval = hal_pin_bit_newf(HAL_IN, &(addr->is_unlocked), mot_comp_id, "axis.%d.is-unlocked", num);
if (retval != 0) return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
pboard->port[0].mask = portconfig0;
pboard->port[1].mask = portconfig1;
return(0);
}
static int Device_ExportPinsParametersFunctions(board_data_t *this, int componentId, int boardId)
{
int msgLevel, error=0;
// 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.
msgLevel = rtapi_get_msg_level();
rtapi_set_msg_level(RTAPI_MSG_WARN);
// Export digital I/O.
if(!error) error = Device_ExportDigitalInPinsParametersFunctions(this, componentId, boardId);
if(!error) error = Device_ExportDigitalOutPinsParametersFunctions(this, componentId, boardId);
// Restore saved message level.
rtapi_set_msg_level(msgLevel);
return(error);
}
// we check each ports mask (port[portnum].mask) against a mask bit to see which of the 24 points of i/o are inputs (a false bit is an input)
static int export_freqgen(int num, freqgen_t * addr, int step_type)
{
int n, 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 param variable for raw counts */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.rawcounts", num);
retval = hal_param_s32_new(buf, HAL_RO, &(addr->rawcount), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for counts captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.counts", num);
retval = hal_pin_s32_new(buf, HAL_OUT, &(addr->count), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for scaled position captured by update() */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.position-fb", num);
retval = hal_pin_float_new(buf, HAL_OUT, &(addr->pos), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for position scaling */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.position-scale", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->pos_scale), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for frequency command */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.velocity", num);
retval = hal_pin_float_new(buf, HAL_IN, &(addr->vel), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for frequency scaling */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.velocity-scale", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->vel_scale), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for max frequency */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.maxfreq", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->maxfreq), comp_id);
if (retval != 0) {
return retval;
}
/* export param for scaled velocity (frequency in Hz) */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.frequency", num);
retval = hal_param_float_new(buf, HAL_RO, &(addr->freq), comp_id);
if (retval != 0) {
return retval;
}
/* export parameter for max accel/decel */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.maxaccel", num);
retval = hal_param_float_new(buf, HAL_RW, &(addr->maxaccel), comp_id);
if (retval != 0) {
return retval;
}
/* set default parameter values */
addr->pos_scale = 1.0;
addr->vel_scale = 1.0;
/* set maxfreq very high - let update_freq() fix it */
addr->maxfreq = 1e15;
addr->maxaccel = 0.0;
addr->wd.st0.step_type = step_type;
/* init the step generator core to zero output */
addr->accum = 0;
addr->addval = 0;
addr->newaddval = 0;
addr->deltalim = 0;
addr->rawcount = 0;
addr->printed_error = 0;
if (step_type == 0) {
/* setup for stepping type 0 - step/dir */
addr->wd.st0.need_step = 0;
addr->wd.st0.setup_timer = 0;
addr->wd.st0.hold_timer = 0;
addr->wd.st0.space_timer = 0;
addr->wd.st0.len_timer = 0;
/* export parameters for step/dir pulse timing */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dirsetup", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.dir_setup), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dirhold", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.dir_hold), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.steplen", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.step_len), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.stepspace", num);
retval =
hal_param_u32_new(buf, HAL_RW, &(addr->wd.st0.step_space),
comp_id);
if (retval != 0) {
return retval;
}
/* init the parameters */
addr->wd.st0.dir_setup = 1;
addr->wd.st0.dir_hold = 1;
addr->wd.st0.step_len = 1;
addr->wd.st0.step_space = 1;
/* export pins for step and direction */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.step", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[STEP_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[STEP_PIN]) = 0;
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.dir", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[DIR_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[DIR_PIN]) = 0;
} else if (step_type == 1) {
/* setup for stepping type 1 - pseudo-PWM */
/* export pins for up and down */
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.up", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[UP_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[UP_PIN]) = 0;
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.down", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[DOWN_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[DOWN_PIN]) = 0;
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.count", num);
retval =
hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[COUNT_PIN]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[COUNT_PIN]) = 0;
} else {
/* setup for stepping types 2 and higher */
addr->wd.st2.state = 0;
addr->wd.st2.cycle_max = cycle_len_lut[step_type - 2] - 1;
addr->wd.st2.num_phases = num_phases_lut[step_type - 2];
addr->wd.st2.lut = &(master_lut[step_type - 2][0]);
/* export pins for output phases */
for (n = 0; n < addr->wd.st2.num_phases; n++) {
rtapi_snprintf(buf, HAL_NAME_LEN, "freqgen.%d.phase-%c", num,
n + 'A');
retval = hal_pin_bit_new(buf, HAL_OUT, &(addr->phase[n]), comp_id);
if (retval != 0) {
return retval;
}
*(addr->phase[n]) = 0;
}
}
/* set initial pin values */
*(addr->count) = 0;
*(addr->pos) = 0.0;
*(addr->vel) = 0.0;
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
/**
\brief main realtime task */
int rtapi_app_main(void)
{
// zynq and FPGA code revision
int rev, zrev, n;
// save messaging level
static int msg_level;
int retval = 0;
int fdmisc;
// save message level on entering
msg_level = rtapi_get_msg_level();
/* setup messaging level in:
RTAPI_MSG_NONE, RTAPI_MSG_ERR, RTAPI_MSG_WARN,
RTAPI_MSG_INFO, RTAPI_MSG_DBG, RTAPI_MSG_ALL */
rtapi_set_msg_level(RTAPI_MSG_ALL);
// check Zynq revision
if ((zrev = zynq_revision()) < 0) {
// unable to determine zynq revision
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: unable to determine zynq revision");
hal_exit(comp_id);
return -1;
}
// notify zynq revision
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Zynq Revision %d \n", zrev);
// check Zedboard FPGA hardware revision
rev = zb_revision();
// do revision specific configuration
switch (rev) {
case 01:
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Zedboard FPGA Revision 01\n");
break;
default:
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: FPGA revision %d not (yet) supported\n", rev);
return -1;
break;
}
// open and map miscellaneous peripheral for PMOD IO access
fdmisc = open("/dev/mem", O_RDWR);
map_MiscAddress = (unsigned int) mmap(NULL, 100, PROT_READ | PROT_WRITE, MAP_SHARED, fdmisc, (off_t)MISC_ADDR );
if (-1 == map_MiscAddress ) {
fprintf(stderr, "MMap failed to map Miscellaneus peripheral\n");
return 0;
}
printf("Map Misc peripheral: virtual 0x%x real 0x%x \n", map_MiscAddress, MISC_ADDR);
// parse module parameters in order to find the number
// of configured FOC channels and their CAN address/configuration
for(n = 0; n < MAX_FOC_CHAN && FOC_axis[n] != -1 ; n++) {
// check for a valid CAN address
if( (FOC_axis[n] <= 0) || ( FOC_axis[n] > 15) ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: bad CAN address '%i', axis %i", FOC_axis[n], n);
hal_exit(comp_id);
return -1;
}
// check the control mode configuration
if( INVALID == parse_ctrl_type(ctrl_type[n])) {
rtapi_print_msg(RTAPI_MSG_ERR,"HAL_ZED_CAN: ERROR: bad control type '%s' for axis %i ('c','j','v','i','t')",
ctrl_type[n], n);
hal_exit(comp_id);
return -1;
}
// found a correctly configured channel
num_chan++;
// notify
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: FOC axis %d with CAN address %d.",n, FOC_axis[n] );
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: Motor gear %d, Screw gear %d, Screw ratio %d Encoder PPR %d.", Screw_gear[n], Motor_gear[n], Screw_ratio[n], PPR[n]);
}
if( (0 == num_chan) || (8 < num_chan) ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: channels configured incorrectly.");
hal_exit(comp_id);
return -1;
}
// allocate shared memory for FOC_data of each axis
FOC_data_array = hal_malloc(num_chan * sizeof(FOC_data_t));
if ( 0 == FOC_data_array ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
/** \note CAN8/2FOC connection is 1 axis for each CAN channel */
// configure CAN communication
for(n = 0; n < num_chan ; n++) {
if ( 0 != setup_CAN(n) ) {
hal_exit(comp_id);
return -1;
}
}
// try to init the component
comp_id = hal_init("hal_zed_can");
if( comp_id < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: hal_init() failed\n");
hal_exit(comp_id);
return -1;
}
/* Export the variables/parameters for each FOC axis */
for (n = 0; n < num_chan; n++) {
// export pins/params for the n^th component
retval = exportFOCaxis(n, &(FOC_data_array[n]) );
if( retval < 0 ) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: exportFOCaxis() failed");
hal_exit(comp_id);
return -1;
}
}
// export the read_feedback and send_setpoint functions as hal_zed_can.send_setpoint and hal_zed_can.read_feedback in hal
retval = hal_export_funct("hal_zed_can.send_setpoint", send_setpoint, FOC_data_array, 0, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: write funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("hal_zed_can.read_feedback", read_feedback, FOC_data_array, 0, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_ZED_CAN: ERROR: read funct export failed\n");
hal_exit(comp_id);
return -1;
}
// activate periodic communication on all axis
setup_2FOC_periodic();
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: FOC periodic data exchange active.");
// all operations succeded
rtapi_print_msg(RTAPI_MSG_INFO, "HAL_ZED_CAN: driver installed successfully.\n");
hal_ready(comp_id);
// return to previous mesaging level
rtapi_set_msg_level(msg_level);
return 0;
}
static int export_port(int portnum, parport_t * port)
{
int retval, msg;
/* 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);
retval = 0;
/* declare input pins (status port) */
retval += export_input_pin(portnum, 15, port->status_in, 0);
retval += export_input_pin(portnum, 13, port->status_in, 1);
retval += export_input_pin(portnum, 12, port->status_in, 2);
retval += export_input_pin(portnum, 10, port->status_in, 3);
retval += export_input_pin(portnum, 11, port->status_in, 4);
if (port->data_dir != 0) {
/* declare input pins (data port) */
retval += export_input_pin(portnum, 2, port->data_in, 0);
retval += export_input_pin(portnum, 3, port->data_in, 1);
retval += export_input_pin(portnum, 4, port->data_in, 2);
retval += export_input_pin(portnum, 5, port->data_in, 3);
retval += export_input_pin(portnum, 6, port->data_in, 4);
retval += export_input_pin(portnum, 7, port->data_in, 5);
retval += export_input_pin(portnum, 8, port->data_in, 6);
retval += export_input_pin(portnum, 9, port->data_in, 7);
} else {
/* declare output pins (data port) */
retval += export_output_pin(portnum, 2,
port->data_out, port->data_inv, port->data_reset, 0);
retval += export_output_pin(portnum, 3,
port->data_out, port->data_inv, port->data_reset, 1);
retval += export_output_pin(portnum, 4,
port->data_out, port->data_inv, port->data_reset, 2);
retval += export_output_pin(portnum, 5,
port->data_out, port->data_inv, port->data_reset, 3);
retval += export_output_pin(portnum, 6,
port->data_out, port->data_inv, port->data_reset, 4);
retval += export_output_pin(portnum, 7,
port->data_out, port->data_inv, port->data_reset, 5);
retval += export_output_pin(portnum, 8,
port->data_out, port->data_inv, port->data_reset, 6);
retval += export_output_pin(portnum, 9,
port->data_out, port->data_inv, port->data_reset, 7);
retval += hal_param_u32_newf(HAL_RW, &port->reset_time, comp_id,
"parport.%d.reset-time", portnum);
retval += hal_param_u32_newf(HAL_RW, &port->debug1, comp_id,
"parport.%d.debug1", portnum);
retval += hal_param_u32_newf(HAL_RW, &port->debug2, comp_id,
"parport.%d.debug2", portnum);
port->write_time = 0;
}
if(port->use_control_in == 0) {
/* declare output variables (control port) */
retval += export_output_pin(portnum, 1,
port->control_out, port->control_inv, port->control_reset, 0);
retval += export_output_pin(portnum, 14,
port->control_out, port->control_inv, port->control_reset, 1);
retval += export_output_pin(portnum, 16,
port->control_out, port->control_inv, port->control_reset, 2);
retval += export_output_pin(portnum, 17,
port->control_out, port->control_inv, port->control_reset, 3);
} else {
/* declare input variables (control port) */
retval += export_input_pin(portnum, 1, port->control_in, 0);
retval += export_input_pin(portnum, 14, port->control_in, 1);
retval += export_input_pin(portnum, 16, port->control_in, 2);
retval += export_input_pin(portnum, 17, port->control_in, 3);
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return retval;
}
static int export_pwmgen(int num, pwmgen_t * addr, int output_type)
{
int retval, msg;
/* 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 paramameters */
retval = hal_pin_float_newf(HAL_IO, &(addr->scale), comp_id,
"pwmgen.%d.scale", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IO, &(addr->offset), comp_id,
"pwmgen.%d.offset", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_IO, &(addr->dither_pwm), comp_id,
"pwmgen.%d.dither-pwm", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IO, &(addr->pwm_freq), comp_id,
"pwmgen.%d.pwm-freq", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IO, &(addr->min_dc), comp_id,
"pwmgen.%d.min-dc", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_IO, &(addr->max_dc), comp_id,
"pwmgen.%d.max-dc", num);
if (retval != 0) {
return retval;
}
retval = hal_pin_float_newf(HAL_OUT, &(addr->curr_dc), comp_id,
"pwmgen.%d.curr-dc", num);
if (retval != 0) {
return retval;
}
/* export pins */
retval = hal_pin_bit_newf(HAL_IN, &(addr->enable), comp_id,
"pwmgen.%d.enable", num);
if (retval != 0) {
return retval;
}
*(addr->enable) = 0;
retval = hal_pin_float_newf(HAL_IN, &(addr->value), comp_id,
"pwmgen.%d.value", num);
if (retval != 0) {
return retval;
}
*(addr->value) = 0.0;
if (output_type == 2) {
/* export UP/DOWN pins */
retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[UP_PIN]), comp_id,
"pwmgen.%d.up", num);
if (retval != 0) {
return retval;
}
/* init the pin */
*(addr->out[UP_PIN]) = 0;
retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[DOWN_PIN]), comp_id,
"pwmgen.%d.down", num);
if (retval != 0) {
return retval;
}
/* init the pin */
*(addr->out[DOWN_PIN]) = 0;
} else {
/* export PWM pin */
retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[PWM_PIN]), comp_id,
"pwmgen.%d.pwm", num);
if (retval != 0) {
return retval;
}
/* init the pin */
*(addr->out[PWM_PIN]) = 0;
if ( output_type == 1 ) {
/* export DIR pin */
retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[DIR_PIN]), comp_id,
"pwmgen.%d.dir", num);
if (retval != 0) {
return retval;
}
/* init the pin */
*(addr->out[DIR_PIN]) = 0;
}
}
/* set default pin values */
*(addr->scale) = 1.0;
*(addr->offset) = 0.0;
*(addr->dither_pwm) = 0;
*(addr->pwm_freq) = 0;
*(addr->min_dc) = 0.0;
*(addr->max_dc) = 1.0;
*(addr->curr_dc) = 0.0;
/* init other fields */
addr->period = 50000;
addr->high_time = 0;
addr->period_timer = 0;
addr->high_timer = 0;
addr->curr_output = 0;
addr->output_type = output_type;
addr->pwm_mode = PWM_DISABLED;
addr->direction = 0;
addr->old_scale = *(addr->scale) + 1.0;
addr->old_pwm_freq = -1;
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
static int export_sim_enc(sim_enc_t * addr, char *prefix)
{
int retval, msg;
/* 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 param variable for pulses per rev */
retval = hal_pin_u32_newf(HAL_IO, &(addr->ppr), comp_id,
"%s.ppr", prefix);
if (retval != 0) {
return retval;
}
/* export param variable for scaling */
retval = hal_pin_float_newf(HAL_IO, &(addr->scale), comp_id,
"%s.scale", prefix);
if (retval != 0) {
return retval;
}
/* export pin for speed command */
retval = hal_pin_float_newf(HAL_IN, &(addr->speed), comp_id,
"%s.speed", prefix);
if (retval != 0) {
return retval;
}
/* export pins for output phases */
retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseA), comp_id,
"%s.phase-A", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseB), comp_id,
"%s.phase-B", prefix);
if (retval != 0) {
return retval;
}
retval = hal_pin_bit_newf(HAL_OUT, &(addr->phaseZ), comp_id,
"%s.phase-Z", prefix);
if (retval != 0) {
return retval;
}
/* export pin for rawcounts */
retval = hal_pin_s32_newf(HAL_IN, &(addr->rawcounts), comp_id,
"%s.rawcounts", prefix);
if (retval != 0) {
return retval;
}
/* init parameters */
*(addr->ppr) = 100;
*(addr->scale) = 1.0;
/* init internal vars */
addr->old_scale = 0.0;
addr->scale_mult = 1.0;
/* init the state variables */
addr->accum = 0;
addr->addval = 0;
addr->state = 0;
addr->cycle = 0;
/* init the outputs */
*(addr->phaseA) = 0;
*(addr->phaseB) = 0;
*(addr->phaseZ) = 0;
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
static int export_encoder_pair(int num, encoder_pair_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 pins for the quadrature inputs */
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-A", num);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->master_A), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-B", num);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->master_B), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-A", num);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->slave_A), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-B", num);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->slave_B), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for the enable input */
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.enable", num);
retval = hal_pin_bit_new(buf, HAL_IN, &(addr->enable), comp_id);
if (retval != 0) {
return retval;
}
/* export pin for output */
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.error", num);
retval = hal_pin_float_new(buf, HAL_OUT, &(addr->error), comp_id);
if (retval != 0) {
return retval;
}
/* export pins for config info() */
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-ppr", num);
retval = hal_pin_u32_new(buf, HAL_IO, &(addr->master_ppr), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-ppr", num);
retval = hal_pin_u32_new(buf, HAL_IO, &(addr->slave_ppr), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.master-teeth", num);
retval = hal_pin_u32_new(buf, HAL_IO, &(addr->master_teeth), comp_id);
if (retval != 0) {
return retval;
}
rtapi_snprintf(buf, HAL_NAME_LEN, "encoder-ratio.%d.slave-teeth", num);
retval = hal_pin_u32_new(buf, HAL_IO, &(addr->slave_teeth), comp_id);
if (retval != 0) {
return retval;
}
/* restore saved message level */
rtapi_set_msg_level(msg);
return 0;
}
