int read_sdo(ec_sdo_request_t *req)
{
int sdo_read_value;
switch (ecrt_sdo_request_state(req)) {
case EC_REQUEST_UNUSED: // request was not used yet
ecrt_sdo_request_read(req); // trigger first read
break;
case EC_REQUEST_BUSY:
//fprintf(stderr, "SDO still busy...\n");
break;
case EC_REQUEST_SUCCESS:
sdo_read_value = EC_READ_S32(ecrt_sdo_request_data(req));
//logmsg(1, "SDO value read: 0x%X\n", sdo_read_value);
ecrt_sdo_request_write(req); // trigger next write
break;
case EC_REQUEST_ERROR:
//fprintf(stderr, "Failed to read SDO!\n");
ecrt_sdo_request_read(req); // retry reading
break;
}
return sdo_read_value;
}
void cyclic_task()
{
struct timespec wakeupTime, time;
// get current time
clock_gettime(CLOCK_TO_USE, &wakeupTime);
while(1)
{
if(deactive==1)
{
break;
}
wakeupTime = timespec_add(wakeupTime, cycletime);
clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
// writter_receive(master);
ecrt_master_receive(master);
ecrt_domain_process(domain_r);
ecrt_domain_process(domain_w);
temp[0]=EC_READ_U16(domain_w_pd + status_word);
temp[1]=EC_READ_S32(domain_w_pd + mode_display);
if (counter)
{
counter--;
}
else
{ // do this at 1 Hz
counter = FREQUENCY;
check_master_state();
blink = !blink;
}
// write process data
if(servo_flag==1)
{
//servo off
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0006 );
}
else if( (temp[0]&0x004f) == 0x0040 )
{
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0006 );
}
else if( (temp[0]&0x006f) == 0x0021)
{
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0007 );
}
else if( (temp[0]&0x006f) == 0x0023)
{
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x000f );
EC_WRITE_S32(domain_r_pd+tar_pos,0);
EC_WRITE_S32(domain_r_pd+tar_vel, 0xffff);
EC_WRITE_S32(domain_r_pd+max_torq, 0xf00);
}
//operation enabled
else if( (temp[0]&0x006f) == 0x0027)
{
EC_WRITE_S32(domain_r_pd+tar_pos, (move_value+=2000) );
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x001f);
}
clock_gettime(CLOCK_TO_USE, &time);
ecrt_master_application_time(master, TIMESPEC2NS(time));
if (sync_ref_counter)
{
sync_ref_counter--;
}
else
{
sync_ref_counter = 1; // sync every cycle
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
// send process data
ecrt_domain_queue(domain_r);
ecrt_domain_queue(domain_w);
ecrt_master_send(master);
}
}
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;
}
void lcec_stmds5k_read(struct lcec_slave *slave, long period) {
lcec_master_t *master = slave->master;
lcec_stmds5k_data_t *hal_data = (lcec_stmds5k_data_t *) slave->hal_data;
uint8_t *pd = master->process_data;
int32_t index_tmp;
int32_t pos_cnt, pos_cnt_diff;
long long net_count;
uint8_t dev_state;
uint16_t speed_state;
int16_t speed_raw, torque_raw;
double torque;
// wait for slave to be operational
if (!slave->state.operational) {
return;
}
// check for change in scale value
lcec_stmds5k_check_scales(hal_data);
// read device state
dev_state = EC_READ_U8(&pd[hal_data->dev_state_pdo_os]);
*(hal_data->ready) = (dev_state >> 0) & 0x01;
*(hal_data->error) = (dev_state >> 1) & 0x01;
*(hal_data->loc_ena) = (dev_state >> 6) & 0x01;
*(hal_data->toggle) = (dev_state >> 7) & 0x01;
// read speed state
speed_state = EC_READ_U16(&pd[hal_data->speed_state_pdo_os]);
*(hal_data->stopped) = (speed_state >> 0) & 0x01;
*(hal_data->at_speed) = (speed_state >> 1) & 0x01;
*(hal_data->overload) = (speed_state >> 2) & 0x01;
// read current speed
speed_raw = EC_READ_S16(&pd[hal_data->speed_mot_pdo_os]);
*(hal_data->vel_fb_rpm) = hal_data->speed_max_rpm * (double)speed_raw * STMDS5K_PCT_REG_DIV;
*(hal_data->vel_fb) = *(hal_data->vel_fb_rpm) * STMDS5K_RPM_DIV * hal_data->pos_scale_rcpt;
// read torque
// E02 : torque motor filterd (x 0,1 Nm)
torque_raw = EC_READ_S16(&pd[hal_data->torque_mot_pdo_os]);
torque = (double)torque_raw * STMDS5K_TORQUE_DIV;
*(hal_data->torque_fb) = torque;
torque = fabs(torque);
*(hal_data->torque_fb_abs) = torque;
*(hal_data->torque_fb_pct) = torque * hal_data->torque_reference_rcpt * 100.0;
// update raw position counter
pos_cnt = EC_READ_S32(&pd[hal_data->pos_mot_pdo_os]);
*(hal_data->enc_raw) = pos_cnt;
*(hal_data->on_home_neg) = (pos_cnt <= hal_data->home_raw);
*(hal_data->on_home_pos) = (pos_cnt >= hal_data->home_raw);
pos_cnt <<= 8;
pos_cnt_diff = pos_cnt - hal_data->last_pos_cnt;
hal_data->last_pos_cnt = pos_cnt;
hal_data->pos_cnt += pos_cnt_diff;
// check for index edge
if (*(hal_data->index_ena)) {
index_tmp = (hal_data->pos_cnt >> 32) & 0xffffffff;
if (hal_data->do_init || !hal_data->last_index_ena) {
hal_data->index_ref = index_tmp;
} else if (index_tmp > hal_data->index_ref) {
hal_data->index_cnt = (long long)index_tmp << 32;
*(hal_data->index_ena) = 0;
} else if (index_tmp < hal_data->index_ref) {
hal_data->index_cnt = (long long)hal_data->index_ref << 32;
*(hal_data->index_ena) = 0;
}
}
void lcec_el5152_read(struct lcec_slave *slave, long period) {
lcec_master_t *master = slave->master;
lcec_el5152_data_t *hal_data = (lcec_el5152_data_t *) slave->hal_data;
uint8_t *pd = master->process_data;
int i, idx_flag;
lcec_el5152_chan_t *chan;
int32_t idx_count, raw_count, raw_delta;
uint32_t raw_period;
// wait for slave to be operational
if (!slave->state.operational) {
hal_data->last_operational = 0;
return;
}
// check inputs
for (i=0; i<LCEC_EL5152_CHANS; i++) {
chan = &hal_data->chans[i];
// check for change in scale value
if (*(chan->pos_scale) != chan->old_scale) {
// scale value has changed, test and update it
if ((*(chan->pos_scale) < 1e-20) && (*(chan->pos_scale) > -1e-20)) {
// value too small, divide by zero is a bad thing
*(chan->pos_scale) = 1.0;
}
// save new scale to detect future changes
chan->old_scale = *(chan->pos_scale);
// we actually want the reciprocal
chan->scale = 1.0 / *(chan->pos_scale);
}
// get bit states
*(chan->ina) = EC_READ_BIT(&pd[chan->ina_pdo_os], chan->ina_pdo_bp);
*(chan->inb) = EC_READ_BIT(&pd[chan->inb_pdo_os], chan->inb_pdo_bp);
*(chan->expol_stall) = EC_READ_BIT(&pd[chan->expol_stall_pdo_os], chan->expol_stall_pdo_bp);
*(chan->tx_toggle) = EC_READ_BIT(&pd[chan->tx_toggle_pdo_os], chan->tx_toggle_pdo_bp);
// read raw values
raw_count = EC_READ_S32(&pd[chan->count_pdo_os]);
raw_period = EC_READ_U32(&pd[chan->period_pdo_os]);
// check for operational change of slave
if (!hal_data->last_operational) {
chan->last_count = raw_count;
}
// check for counter set done
if (EC_READ_BIT(&pd[chan->set_count_done_pdo_os], chan->set_count_done_pdo_bp)) {
chan->last_count = raw_count;
*(chan->set_raw_count) = 0;
}
// update raw values
if (! *(chan->set_raw_count)) {
*(chan->raw_count) = raw_count;
*(chan->raw_period) = raw_period;
}
// check for index edge
idx_flag = 0;
idx_count = 0;
if (*(chan->index) && !chan->last_index) {
idx_count = raw_count;
idx_flag = 1;
}
chan->last_index = *(chan->index);
// handle initialization
if (chan->do_init || *(chan->reset)) {
chan->do_init = 0;
chan->last_count = raw_count;
*(chan->count) = 0;
idx_flag = 0;
}
// handle index
if (idx_flag && *(chan->index_ena)) {
chan->last_count = idx_count;
*(chan->count) = 0;
*(chan->index_ena) = 0;
}
// compute net counts
raw_delta = raw_count - chan->last_count;
chan->last_count = raw_count;
*(chan->count) += raw_delta;
// scale count to make floating point position
*(chan->pos) = *(chan->count) * chan->scale;
// scale period
*(chan->period) = ((double) (*(chan->raw_period))) * LCEC_EL5152_PERIOD_SCALE;
}
hal_data->last_operational = 1;
}
int main(int argc, char **argv)
{
// Создаем мастер-объект
gkMaster = ecrt_request_master(0);
if (gkMaster) {
fprintf(stdout, "1. Master created.\n");
} else {
fprintf(stderr, "Unable to get requested master.\n");
return -1;
}
// Создаем объект для обмена PDO в циклическом режиме.
gkDomain1 = ecrt_master_create_domain(gkMaster);
if (gkDomain1) {
fprintf(stdout, "2. Process data domain created.\n");
} else {
fprintf(stderr, "Unable to create process data domain.\n");
return -1;
}
// Создаем объект конфигурации подчиненного.
ec_slave_config_t* sc = ecrt_master_slave_config(gkMaster, 0, gkDriveNum, 0x00007595, 0x00000000);
if (sc) {
fprintf(stdout, "3. Slave configuration object created.\n");
} else {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
// Конфигурируем PDO подчиненного
// TxPDO
ec_pdo_entry_info_t l7na_tx_channel1[] = {
{0x6041, 0, 16}, // Statusword
{0x6061, 0, 8}, // The Modes of Operation Display
{0x6062, 0, 32}, // The Position Demand Value
{0x6064, 0, 32}, // The Position Actual Value
{0x606B, 0, 32}, // The Velocity Demand Value
{0x6081, 0, 32}, // The Profile Velocity
{0x606C, 0, 32}, // The Actual Velocity Value
{0x607A, 0, 32}, // The Target Position
{0x6077, 0, 16}, // Actual torque value
// {0x200F, 0, 16}, // Position Scale Denominator
};
ec_pdo_info_t l7na_tx_pdos[] = {
{0x1A00, 9, l7na_tx_channel1}
};
// RxPDO
ec_pdo_entry_info_t l7na_rx_channel1[] = {
{0x6040, 0, 16}, // Controlword
{0x6060, 0, 8}, // Modes of Operation
{0x607A, 0, 32}, // The Target Position
{0x606C, 0, 32}, // The Velocity Demand value
{0x6081, 0, 32}, // The Profile Velocity
{0x60FF, 0, 32}, // The Target Velocity (in Profile Velocity (Pv) mode and Cyclic Synchronous Velocity (Csv) modes)
{0x6071, 0, 16}, // The Target Torque
};
ec_pdo_info_t l7na_rx_pdos[] = {
{0x1600, 7, l7na_rx_channel1}
};
// Конфигурация SyncManagers 2 (FMMU0) и 3 (FMMU1)
// { sync_mgr_idx, sync_mgr_direction, pdo_num, pdo_ptr, watch_dog_mode }
// { 0xFF - end marker}
ec_sync_info_t l7na_syncs[] = {
{2, EC_DIR_OUTPUT, 1, l7na_rx_pdos, EC_WD_DISABLE},
{3, EC_DIR_INPUT, 1, l7na_tx_pdos, EC_WD_DISABLE},
{0xFF}
};
if (ecrt_slave_config_pdos(sc, EC_END, l7na_syncs)) {
fprintf(stderr, "Failed to configure slave pdo.\n");
return -1;
}
fprintf(stdout, "4. Configuring slave PDOs and sync managers done.\n");
// Регистируем PDO в домене
if (ecrt_domain_reg_pdo_entry_list(gkDomain1, gkDomain1Regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
fprintf(stdout, "5. PDO entries registered in domain.\n");
if (ecrt_master_activate(gkMaster)) {
fprintf(stderr,"Master activation failed.\n");
return -1;
}
fprintf(stdout, "6. Master activated.\n");
if (!(gkDomain1PD = ecrt_domain_data(gkDomain1))) {
fprintf(stderr,"Domain data initialization failed.\n");
return -1;
}
fprintf(stdout, "7. Domain data registered.\n");
//goto end;
check_master_state();
check_domain1_state();
int32_t op_flag = 0, ipos = 0;
uint16_t istatus = 0;
//ждать режим OP
for(uint32_t j = 0; ; j++) {
ecrt_master_receive(gkMaster); //RECEIVE A FRAME
ecrt_domain_process(gkDomain1); //DETERMINE THE DATAGRAM STATES
// check_slave_config_states();
if (! op_flag) {
check_domain1_state();
}
if (gkDomain1State.wc_state == EC_WC_COMPLETE && !op_flag) {
printf("Domain is up at %d cycles.\n", j);
op_flag = 1;
}
ipos = EC_READ_U32(gkDomain1PD + gkOffIPos); //READ DATA 0x6064 position
istatus = EC_READ_U16(gkDomain1PD + gkOffIStatus); //READ DATA 0x6041 status
// send process data
ecrt_domain_queue(gkDomain1); //MARK THE DOMAIN DATA AS READY FOR EXCHANGE
ecrt_master_send(gkMaster); //SEND ALL QUEUED DATAGRAMS
usleep(1000); //WAIT 1mS
if (op_flag) {
printf("1-Position: %d Status: 0x%x\n", ipos, istatus);
break;
}
}
fprintf(stdout, "8. Got OP state.\n");
if(argc > 1) {
//перейти в позицию
const int cmdpos = atoi(argv[1]);
printf("cmd pos: %d\n", cmdpos);
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0xF); //0x6040 ControlWord
EC_WRITE_U8(gkDomain1PD + gkOffOMode, 1); // 0x6060 Profile position mode // 3 - for velocity mode, 1- for position mode
EC_WRITE_S32(gkDomain1PD + gkOffPVel, 1000000); // 0x60ff profile velocity // gkOffTVel - for velocity mode
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
//wait
for (uint32_t i = 0; i < 200; ++i) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
}
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
/* comment 2 lines for velocity mode */
EC_WRITE_S32(gkDomain1PD + gkOffOPos, cmdpos);
EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0x11F);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
//wait
for (uint32_t i = 0; i < 200; ++i) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
}
/* ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
EC_WRITE_S32(gkDomain1PD + gkOffOPos, cmdpos);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);*/
//wait
/* for (uint32_t i = 0; i < 1000; ++i) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
}
*/
timespec tbegin, tend;
::clock_gettime(CLOCK_MONOTONIC, &tbegin);
printf("Time begin: %lds/%ldns\n", tbegin.tv_sec, tbegin.tv_nsec);
const uint32_t kIterationMax = 500000;
uint32_t change_count = 0;
bool target_reached = false;
for (uint32_t j = 0; ; j++) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
int32_t ipos_new = EC_READ_S32(gkDomain1PD + gkOffIPos); //READ DATA 0x6064 position
uint16_t istatus_new = EC_READ_U16(gkDomain1PD + gkOffIStatus); //READ DATA 0x6041 status
int32_t imode = EC_READ_S8(gkDomain1PD + gkOffIMode);
int32_t ipvel = EC_READ_S32(gkDomain1PD + gkOffPVel);
int32_t idvel = EC_READ_S32(gkDomain1PD + gkOffDVel);
int32_t iavel = EC_READ_S32(gkDomain1PD + gkOffIVel);
int32_t idpos = EC_READ_S32(gkDomain1PD + gkOffDPos);
int32_t itpos = EC_READ_S32(gkDomain1PD + gkOffOPos);
int32_t icontrol = EC_READ_U16(gkDomain1PD + gkOffOControl);
int16_t iatorq = EC_READ_S16(gkDomain1PD + gkOffITorq);
// int32_t ipdenom = EC_READ_S16(gkDomain1PD + gkOffPDenom);
if (ipos_new != ipos) {
ipos = ipos_new;
change_count++;
printf("Position: %d Status: 0x%x Mode: %d ATorq: %d PVel: %d DVel: %d AVel: %d DPos: %d TPos: %d OControl: 0x%x\n", ipos, istatus, imode, iatorq, ipvel, idvel, iavel, idpos, itpos, icontrol);
}
// position mode
if(! target_reached && ((istatus_new >> 10) & 0x1)) {
::clock_gettime(CLOCK_MONOTONIC, &tend);
printf("Target reached. Pos: %d Status: 0x%x TEnd=%lds/%ldns\n", ipos, istatus, tend.tv_sec, tend.tv_nsec);
target_reached = true;
//break;
}
/* Velocity mode */
if (j == kIterationMax) {
/* clock_gettime(CLOCK_MONOTONIC, &tend);
printf("Iterations=%d, change_count=%d. time_end=%lds/%ldns Stopping...\n", j, change_count, tend.tv_sec, tend.tv_nsec);
EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0x6);
break;
*/
}
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(100); //WAIT 1mS
}
}
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
printf("...Done. Releasing the master!\n");
// Освобождаем мастер-объект
ecrt_release_master(gkMaster);
return 0;
}
