void disable_operation()
{
bool isOperationDisabled=false;
// while(!isOperationDisabled)
for(int i=0;i<1;++i)
{
EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x0000);// p85, use state transition 9
switch (ecrt_sdo_request_state(sdo_controlword_write)) {
case EC_REQUEST_UNUSED: // request was not used yet
// ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
fprintf(stderr, "Request to Write,But Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "sdo_controlword_write write value 0x03");
ecrt_sdo_request_write(sdo_controlword_write);
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO state!\n");
break;
}
// read state ,need switch_on_disabled
ecrt_sdo_request_read(sdo_statusword_read); // trigger read
switch (ecrt_sdo_request_state(sdo_statusword_read)) {
case EC_REQUEST_UNUSED: // request was not used yet
ecrt_sdo_request_read(sdo_statusword_read); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
fprintf(stderr, "Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "statusword value: 0x%04X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)));
if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x004F ^ 0x0040 == 0) // p91
{
isOperationDisabled = true;
printf("motor state is in switch_on_disabled,exit safely \n");
}
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO!\n");
break;
}
if(isOperationDisabled)
{
break;
}
else
{
printf("bad exit!\n");
}
}//while
}
/** Read number of mapped PDO entries.
*/
void ec_fsm_pdo_entry_read_state_count(
ec_fsm_pdo_entry_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
return;
}
if (!ec_fsm_coe_success(fsm->fsm_coe)) {
EC_SLAVE_ERR(fsm->slave,
"Failed to read number of mapped PDO entries.\n");
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
if (fsm->request.data_size != sizeof(uint8_t)) {
EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu at uploading"
" SDO 0x%04X:%02X.\n",
fsm->request.data_size, fsm->request.index,
fsm->request.subindex);
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
fsm->entry_count = EC_READ_U8(fsm->request.data);
EC_SLAVE_DBG(fsm->slave, 1, "%u PDO entries mapped.\n", fsm->entry_count);
// read first PDO entry
fsm->entry_pos = 1;
ec_fsm_pdo_entry_read_action_next(fsm, datagram);
}
void cyclic_task()
{
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain1);
// check process data state (optional)
check_domain1_state();
if (counter) {
counter--;
} else { // do this at 1 Hz
counter = FREQUENCY;
// calculate new process data
blink = !blink;
// check for master state (optional)
check_master_state();
// check for islave configuration state(s) (optional)
check_slave_config_states();
#if SDO_ACCESS
// read process data SDO
read_sdo();
#endif
}
#if CONFIGURE_PDOS
// read process data
printf("AnaIn: state %u value %u\n",
EC_READ_U8(domain1_pd + off_ana_in_status),
EC_READ_U8(domain1_pd + off_ana_in_value));
#endif
#if 0
// write process data
// EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
#endif
// send process data
ecrt_domain_queue(domain1);
ecrt_master_send(master);
}
void cyclic_task()
{
static unsigned int counter = 10;
static uint8_t outputValue = 0;
static int numAsyncCycles = 0;
uint8_t inputValue = 0;
static uint8_t error = 0;
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain1);
// check process data state (optional)
check_domain1_state();
inputValue = EC_READ_U8(domain1_pd + off_dig_in[1]) & 0x0F;
if(inputValue != outputValue) {
numAsyncCycles++;
} else {
numAsyncCycles = 0;
}
if(numAsyncCycles > 2) {
if(error != 0xff) {
error++;
}
}
if (counter) {
counter--;
} else {
counter = 5; //update delay
// calculate new process data
outputValue = (outputValue + 1) & 0x0F;
// check for master state (optional)
check_master_state();
// check for islave configuration state(s) (optional)
// check_slave_config_states();
}
// write process data
EC_WRITE_U8(domain1_pd + off_dig_out[0], outputValue);
EC_WRITE_U8(domain1_pd + off_dig_out[1], error);
// send process data
ecrt_domain_queue(domain1);
ecrt_master_send(master);
}
/** Processes received mailbox data.
*
* \return Pointer to the received data, or ERR_PTR() code.
*/
uint8_t *ec_slave_mbox_fetch(const ec_slave_t *slave, /**< slave */
const ec_datagram_t *datagram, /**< datagram */
uint8_t *type, /**< expected mailbox protocol */
size_t *size /**< size of the received data */
)
{
size_t data_size;
data_size = EC_READ_U16(datagram->data);
if (data_size + EC_MBOX_HEADER_SIZE > slave->configured_tx_mailbox_size) {
EC_SLAVE_ERR(slave, "Corrupt mailbox response received!\n");
ec_print_data(datagram->data, slave->configured_tx_mailbox_size);
return ERR_PTR(-EPROTO);
}
*type = EC_READ_U8(datagram->data + 5) & 0x0F;
*size = data_size;
if (*type == 0x00) {
const ec_code_msg_t *mbox_msg;
uint16_t code = EC_READ_U16(datagram->data + 8);
EC_SLAVE_ERR(slave, "Mailbox error response received - ");
for (mbox_msg = mbox_error_messages; mbox_msg->code; mbox_msg++) {
if (mbox_msg->code != code)
continue;
printk("Code 0x%04X: \"%s\".\n",
mbox_msg->code, mbox_msg->message);
break;
}
if (!mbox_msg->code)
printk("Unknown error reply code 0x%04X.\n", code);
if (slave->master->debug_level)
ec_print_data(datagram->data + EC_MBOX_HEADER_SIZE, data_size);
return ERR_PTR(-EPROTO);
}
return datagram->data + EC_MBOX_HEADER_SIZE;
}
void read_sdo(void)
{
switch (ecrt_sdo_request_state(sdo_operation_mode_display)) {
case EC_REQUEST_UNUSED: // request was not used yet
ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
fprintf(stderr, "Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "SDO value: 0x%04X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)));
ecrt_sdo_request_read(sdo_operation_mode_display); // trigger next read
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO!\n");
ecrt_sdo_request_read(sdo_operation_mode_display); // retry reading
break;
}
}
int lcec_el7342_init(int comp_id, struct lcec_slave *slave, ec_pdo_entry_reg_t *pdo_entry_regs) {
lcec_master_t *master = slave->master;
lcec_el7342_data_t *hal_data;
int i;
lcec_el7342_chan_t *chan;
uint8_t info1_select, info2_select;
int err;
// initialize callbacks
slave->proc_read = lcec_el7342_read;
slave->proc_write = lcec_el7342_write;
// alloc hal memory
if ((hal_data = hal_malloc(sizeof(lcec_el7342_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_el7342_data_t));
slave->hal_data = hal_data;
// initialize sync info
slave->sync_info = lcec_el7342_syncs;
// initialize global data
hal_data->last_operational = 0;
// initialize pins
for (i=0; i<LCEC_EL7342_CHANS; i++) {
chan = &hal_data->chans[i];
// read sdos
// Info1 selector
if ((chan->sdo_info1_select = lcec_read_sdo(slave, 0x8022 + (i << 4), 0x11, 1)) == NULL) {
return -EIO;
}
info1_select = EC_READ_U8(ecrt_sdo_request_data(chan->sdo_info1_select));
// Info2 selector
if ((chan->sdo_info2_select = lcec_read_sdo(slave, 0x8022 + (i << 4), 0x19, 1)) == NULL) {
return -EIO;
}
info2_select = EC_READ_U8(ecrt_sdo_request_data(chan->sdo_info2_select));
// initialize POD entries
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x02, &chan->latch_ext_valid_pdo_os, &chan->latch_ext_valid_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x03, &chan->set_count_done_pdo_os, &chan->set_count_done_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x04, &chan->count_overflow_pdo_os, &chan->count_overflow_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x05, &chan->count_underflow_pdo_os, &chan->count_underflow_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x08, &chan->expol_stall_pdo_os, &chan->expol_stall_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x09, &chan->ina_pdo_os, &chan->ina_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x0a, &chan->inb_pdo_os, &chan->inb_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x0d, &chan->inext_pdo_os, &chan->inext_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1c32 , 0x20, &chan->sync_err_pdo_os, &chan->sync_err_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1800 + (i * 3), 0x09, &chan->tx_toggle_pdo_os, &chan->tx_toggle_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x11, &chan->count_pdo_os, NULL);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6000 + (i << 4), 0x12, &chan->latch_pdo_os, NULL);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x02, &chan->ena_latch_ext_pos_pdo_os, &chan->ena_latch_ext_pos_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x03, &chan->set_count_pdo_os, &chan->set_count_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x04, &chan->ena_latch_ext_neg_pdo_os, &chan->ena_latch_ext_neg_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7000 + (i << 4), 0x11, &chan->set_count_val_pdo_os, NULL);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x01, &chan->dcm_ready_to_enable_pdo_os, &chan->dcm_ready_to_enable_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x02, &chan->dcm_ready_pdo_os, &chan->dcm_ready_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x03, &chan->dcm_warning_pdo_os, &chan->dcm_warning_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x04, &chan->dcm_error_pdo_os, &chan->dcm_error_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x05, &chan->dcm_move_pos_pdo_os, &chan->dcm_move_pos_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x06, &chan->dcm_move_neg_pdo_os, &chan->dcm_move_neg_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x07, &chan->dcm_torque_reduced_pdo_os, &chan->dcm_torque_reduced_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x0c, &chan->dcm_din1_pdo_os, &chan->dcm_din1_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x0d, &chan->dcm_din2_pdo_os, &chan->dcm_din2_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1c32 , 0x20, &chan->dcm_sync_err_pdo_os, &chan->dcm_sync_err_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x1806 + (i << 1), 0x09, &chan->dcm_tx_toggle_pdo_os, &chan->dcm_tx_toggle_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x11, &chan->dcm_info1_pdo_os, NULL);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x6020 + (i << 4), 0x12, &chan->dcm_info2_pdo_os, NULL);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x01, &chan->dcm_ena_pdo_os, &chan->dcm_ena_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x02, &chan->dcm_reset_pdo_os, &chan->dcm_reset_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x03, &chan->dcm_reduce_torque_pdo_os, &chan->dcm_reduce_torque_pdo_bp);
LCEC_PDO_INIT(pdo_entry_regs, slave->index, slave->vid, slave->pid, 0x7020 + (i << 4), 0x21, &chan->dcm_velo_pdo_os, NULL);
// export encoder pins
if ((err = hal_pin_bit_newf(HAL_IN, &(chan->reset), comp_id, "%s.%s.%s.enc-%d-reset", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->ina), comp_id, "%s.%s.%s.enc-%d-ina", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-ina failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->inb), comp_id, "%s.%s.%s.enc-%d-inb", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-inb failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->inext), comp_id, "%s.%s.%s.enc-%d-inext", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-inext failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->sync_err), comp_id, "%s.%s.%s.enc-%d-sync-error", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-sync-error failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->expol_stall), comp_id, "%s.%s.%s.enc-%d-expol-stall", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-expol-stall failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->tx_toggle), comp_id, "%s.%s.%s.enc-%d-tx-toggle", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-tx-toggle failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->count_overflow), comp_id, "%s.%s.%s.enc-%d-count-overflow", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-count-overflow failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->count_underflow), comp_id, "%s.%s.%s.enc-%d-count-underflow", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-count-underflow failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->latch_ext_valid), comp_id, "%s.%s.%s.enc-%d-latch-ext-valid", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-latch-ext-valid failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IO, &(chan->set_raw_count), comp_id, "%s.%s.%s.enc-%d-set-raw-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-set-raw-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IO, &(chan->ena_latch_ext_pos), comp_id, "%s.%s.%s.enc-%d-index-ext-pos-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-ndex-ext-pos-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IO, &(chan->ena_latch_ext_neg), comp_id, "%s.%s.%s.enc-%d-index-ext-neg-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-ndex-ext-neg-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_s32_newf(HAL_IN, &(chan->set_raw_count_val), comp_id, "%s.%s.%s.enc-%d-set-raw-count-val", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-set-raw-count-val failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->raw_count), comp_id, "%s.%s.%s.enc-%d-raw-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-raw-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->count), comp_id, "%s.%s.%s.enc-%d-count", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-count failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->raw_latch), comp_id, "%s.%s.%s.enc-%d-raw-latch", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-raw-latch failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(chan->pos), comp_id, "%s.%s.%s.enc-%d-pos", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-pos failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_float_newf(HAL_IO, &(chan->pos_scale), comp_id, "%s.%s.%s.enc-%d-pos-scale", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-scale failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
// export servo pins
if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_scale), comp_id, "%s.%s.%s.srv-%d-scale", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-scale failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_offset), comp_id, "%s.%s.%s.srv-%d-offset", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-offset failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_min_dc), comp_id, "%s.%s.%s.srv-%d-min-dc", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-min-dc failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_float_newf(HAL_IO, &(chan->dcm_max_dc), comp_id, "%s.%s.%s.srv-%d-max-dc", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-max-dc failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_float_newf(HAL_OUT, &(chan->dcm_curr_dc), comp_id, "%s.%s.%s.srv-%d-curr-dc", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-curr-dc failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_enable), comp_id, "%s.%s.%s.srv-%d-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-enable failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_absmode), comp_id, "%s.%s.%s.srv-%d-absmode", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-absmode failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_float_newf(HAL_IN, &(chan->dcm_value), comp_id, "%s.%s.%s.srv-%d-cmd", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-value failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->dcm_raw_val), comp_id, "%s.%s.%s.srv-%d-raw-cmd", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-raw-cmd failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_reset), comp_id, "%s.%s.%s.srv-%d-reset", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-reset failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_IN, &(chan->dcm_reduce_torque), comp_id, "%s.%s.%s.srv-%d-reduce-torque", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-reduce-torque failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_ready_to_enable), comp_id, "%s.%s.%s.srv-%d-ready-to-enable", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-ready-to-enable%d- failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_ready), comp_id, "%s.%s.%s.srv-%d-ready", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-ready%d- failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_warning), comp_id, "%s.%s.%s.srv-%d-warning", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-warning failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_error), comp_id, "%s.%s.%s.srv-%d-error", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-error failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_move_pos), comp_id, "%s.%s.%s.srv-%d-move-pos", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-move-pos failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_move_neg), comp_id, "%s.%s.%s.srv-%d-move-neg", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-move-neg failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_torque_reduced), comp_id, "%s.%s.%s.srv-%d-torque-reduced", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-torque-reduced failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_din1), comp_id, "%s.%s.%s.srv-%d-din1", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-din1 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_din2), comp_id, "%s.%s.%s.srv-%d-din2", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-din2 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_sync_err), comp_id, "%s.%s.%s.srv-%d-sync-error", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-sync-error failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_bit_newf(HAL_OUT, &(chan->dcm_tx_toggle), comp_id, "%s.%s.%s.srv-%d-tx-toggle", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-tx-toggle failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->dcm_raw_info1), comp_id, "%s.%s.%s.srv-%d-raw-info1", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-raw-info1 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_s32_newf(HAL_OUT, &(chan->dcm_raw_info2), comp_id, "%s.%s.%s.srv-%d-raw-info2", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-raw-info2 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_u32_newf(HAL_OUT, &(chan->dcm_sel_info1), comp_id, "%s.%s.%s.srv-%d-sel-info1", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-sel-info1 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if ((err = hal_pin_u32_newf(HAL_OUT, &(chan->dcm_sel_info2), comp_id, "%s.%s.%s.srv-%d-sel-info2", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.srv-%d-sel-info2 failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
if (info1_select == INFO_SEL_MOTOR_VELO || info2_select == INFO_SEL_MOTOR_VELO) {
if ((err = hal_pin_float_newf(HAL_OUT, &(chan->dcm_velo_fb), comp_id, "%s.%s.%s.srv-%d-velo-fb", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-velo-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
}
if (info1_select == INFO_SEL_MOTOR_CURR || info2_select == INFO_SEL_MOTOR_CURR) {
if ((err = hal_pin_float_newf(HAL_OUT, &(chan->dcm_current_fb), comp_id, "%s.%s.%s.srv-%d-current-fb", LCEC_MODULE_NAME, master->name, slave->name, i)) != 0) {
rtapi_print_msg(RTAPI_MSG_ERR, LCEC_MSG_PFX "exporting pin %s.%s.%s.enc-%d-current-fb failed\n", LCEC_MODULE_NAME, master->name, slave->name, i);
return err;
}
}
// initialize pins
*(chan->reset) = 0;
*(chan->ina) = 0;
*(chan->inb) = 0;
*(chan->inext) = 0;
*(chan->sync_err) = 0;
*(chan->expol_stall) = 0;
*(chan->tx_toggle) = 0;
*(chan->count_overflow) = 0;
*(chan->count_underflow) = 0;
*(chan->latch_ext_valid) = 0;
*(chan->ena_latch_ext_pos) = 0;
*(chan->ena_latch_ext_neg) = 0;
*(chan->set_raw_count) = 0;
*(chan->set_raw_count_val) = 0;
*(chan->raw_count) = 0;
*(chan->raw_latch) = 0;
*(chan->count) = 0;
*(chan->pos) = 0;
*(chan->pos_scale) = 1.0;
*(chan->dcm_scale) = 1.0;
*(chan->dcm_offset) = 0.0;
*(chan->dcm_min_dc) = -1.0;
*(chan->dcm_max_dc) = 1.0;
*(chan->dcm_curr_dc) = 0.0;
*(chan->dcm_enable) = 0;
*(chan->dcm_absmode) = 0;
*(chan->dcm_value) = 0.0;
*(chan->dcm_raw_val) = 0;
*(chan->dcm_reset) = 0;
*(chan->dcm_reduce_torque) = 0;
*(chan->dcm_ready_to_enable) = 0;
*(chan->dcm_ready) = 0;
*(chan->dcm_warning) = 0;
*(chan->dcm_error) = 0;
*(chan->dcm_move_pos) = 0;
*(chan->dcm_move_neg) = 0;
*(chan->dcm_torque_reduced) = 0;
*(chan->dcm_din1) = 0;
*(chan->dcm_din2) = 0;
*(chan->dcm_sync_err) = 0;
*(chan->dcm_tx_toggle) = 0;
*(chan->dcm_raw_info1) = 0;
*(chan->dcm_raw_info2) = 0;
*(chan->dcm_sel_info1) = info1_select;
*(chan->dcm_sel_info2) = info2_select;
if (chan->dcm_velo_fb != NULL) {
*(chan->dcm_velo_fb) = 0.0;
}
if (chan->dcm_current_fb != NULL) {
*(chan->dcm_current_fb) = 0.0;
}
// initialize variables
chan->enc_do_init = 1;
chan->enc_last_count = 0;
chan->enc_old_scale = *(chan->pos_scale) + 1.0;
chan->enc_scale_recip = 1.0;
chan->dcm_old_scale = *(chan->dcm_scale) + 1.0;
chan->dcm_scale_recip = 1.0;
}
return 0;
}
int main(int argc, char **argv)
{
// ec_slave_config_t *sc;
struct sigaction sa;
struct itimerval tv;
master = ecrt_request_master(0);
if (!master)
return -1;
domain1 = ecrt_master_create_domain(master);
if (!domain1)
{
return -1;
}
if (!(sc_ana_in = ecrt_master_slave_config(master, AliasAndPositon, VendorID_ProductCode)))
{
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
#if SDO_ACCESS
fprintf(stderr, "Creating operation mode read SDO requests...\n");
if (!(sdo_operation_mode_display = ecrt_slave_config_create_sdo_request(sc_ana_in, MODES_OF_OPERATION_DISPLAY, 0, 1))) // uint8 data size 1
{
fprintf(stderr, "Failed to create SDO modes_of_operation_display 0x6061 request.\n");
return -1;
}
fprintf(stderr, "Creating operation mode write SDO requests...\n");
if (!(sdo_operation_mode_write = ecrt_slave_config_create_sdo_request(sc_ana_in, MODES_OF_OPERATION, 0, 1))) // uint8 data size 1
{
fprintf(stderr, "Failed to create SDO MODES_OF_OPERATION request.\n");
return -1;
}
fprintf(stderr, "Creating controlword write SDO requests...\n");
if (!(sdo_controlword_write = ecrt_slave_config_create_sdo_request(sc_ana_in, CONTROLWORD, 0, 2))) // uint16 data size 2
{
fprintf(stderr, "Failed to create SDO CONTROLWORD request.\n");
return -1;
}
fprintf(stderr, "Creating statusword read SDO requests...\n");
if (!(sdo_statusword_read = ecrt_slave_config_create_sdo_request(sc_ana_in, STATUSWORD, 0, 2))) // uint16 data size 2
{
fprintf(stderr, "Failed to create SDO STATUSWORD request.\n");
return -1;
}
//EC_WRITE_U16(ecrt_sdo_request_data(sdo), 0xFFFF);
ecrt_sdo_request_timeout(sdo_operation_mode_display, 500); // ms
ecrt_sdo_request_timeout(sdo_operation_mode_write, 500); // ms
ecrt_sdo_request_timeout(sdo_controlword_write, 500); // ms
ecrt_sdo_request_timeout(sdo_statusword_read, 500); // ms
#endif
#if CONFIGURE_PDOS
printf("Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc_ana_in, EC_END, duetfl80_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
// if (!(sc = ecrt_master_slave_config(
// master, AnaOutSlavePos, Beckhoff_EL4102))) {
// fprintf(stderr, "Failed to get slave configuration.\n");
// return -1;
// }
// if (ecrt_slave_config_pdos(sc, EC_END, el4102_syncs)) {
// fprintf(stderr, "Failed to configure PDOs.\n");
// return -1;
// }
// if (!(sc = ecrt_master_slave_config(
// master, DigOutSlavePos, Beckhoff_EL2032))) {
// fprintf(stderr, "Failed to get slave configuration.\n");
// return -1;
// }
// if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) {
// fprintf(stderr, "Failed to configure PDOs.\n");
// return -1;
// }
// // Create configuration for bus coupler
// sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100);
// if (!sc)
// return -1;
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
#endif
printf("Activating master...\n");
if (ecrt_master_activate(master))
return -1;
#if 1
if (!(domain1_pd = ecrt_domain_data(domain1))) {
return -1;
}
#endif
#if PRIORITY
pid_t pid = getpid();
if (setpriority(PRIO_PROCESS, pid, -19))
fprintf(stderr, "Warning: Failed to set priority: %s\n",
strerror(errno));
#endif
printf("Starting timer...\n");
tv.it_interval.tv_sec = 0;
tv.it_interval.tv_usec = 1000000 / FREQUENCY;
tv.it_value.tv_sec = 0;
tv.it_value.tv_usec = 1000;
if (setitimer(ITIMER_REAL, &tv, NULL)) {
fprintf(stderr, "Failed to start timer: %s\n", strerror(errno));
return 1;
}
// handle ctrl+c ,important , do not remove
sa.sa_handler = signal_handler;
sigemptyset(&sa.sa_mask);
sa.sa_flags = 0;
if (signal(SIGINT, my_sig_handler) == SIG_ERR)
{
printf("\ncan't catch SIGUSR1\n");
return -1;
}
if (sigaction(SIGALRM, &sa, 0)) {
fprintf(stderr, "Failed to install signal handler!\n");
return -1;
}
// 1. check operation mode
bool getModeOk=false;
uint8_t mode_value;
int i=0;
while(1)
{
// printf("i=%d\n",i);
i++;
// receive process data
ecrt_master_receive(master);
// ecrt_domain_process(domain1);
// check process data state (optional)
// check_domain1_state();
// check for master state (optional)
check_master_state();
// check for islave configuration state(s) (optional)
check_slave_config_states();
// read_sdo();
if(i==1)
{
ecrt_sdo_request_read(sdo_operation_mode_display); // trigger read
}
switch (ecrt_sdo_request_state(sdo_operation_mode_display)) {
case EC_REQUEST_UNUSED: // request was not used yet
printf("request was not used yet\n");
break;
case EC_REQUEST_BUSY:
// printf( "Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "sdo_operation_mode_display value: 0x%02X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)));
getModeOk = true;
mode_value = EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display));
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO!\n");
break;
}
if(getModeOk)
{
printf("get mode value <%02x>\n",mode_value);
break;
}
// ecrt_domain_queue(domain1);
ecrt_master_send(master);
// pause();
// sleep(1);
// cyclic_task();
}
printf("check mode done\n");
if(getModeOk == false)
{
exit(-1);
}
// exit(0);
// 2. set operation mode to velocity mode
printf("setting mode to velocity_mode...\n");
bool isWriteModeOk=false;
while(1)
{
ecrt_master_receive(master);
EC_WRITE_U8(ecrt_sdo_request_data(sdo_operation_mode_write), 0x03);
ecrt_sdo_request_write(sdo_operation_mode_write);
switch (ecrt_sdo_request_state(sdo_operation_mode_write)) {
case EC_REQUEST_UNUSED: // request was not used yet
// ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
// printf("Request to Write,But Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "sdo_operation_mode_write write value 0x03 ok\n");
// ecrt_sdo_request_write(sdo_operation_mode_write);
isWriteModeOk = true;
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO state!\n");
break;
}
if(isWriteModeOk)
{
break;
}
ecrt_master_send(master);
}
// 3. check operation mode, after write
getModeOk=false;
// for(int i=0;i<10;++i)
i=0;
while(1)
{
i++;
ecrt_master_receive(master);
if(i==1)
{
ecrt_sdo_request_read(sdo_operation_mode_display); // trigger read
}
switch (ecrt_sdo_request_state(sdo_operation_mode_display)) {
case EC_REQUEST_UNUSED: // request was not used yet
ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
// fprintf(stderr, "Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
printf("sdo_operation_mode_display value: 0x%02X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)));
if(EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)) ^ 0x03 == 0)
{
printf("mode is in velocity_mode \n");
getModeOk = true;
}
else
{
printf("mode not in velocity_mode \n");
exit(-1);
}
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO!\n");
break;
}
if(getModeOk)
{
break;
}
// if(i<10)
{
ecrt_master_send(master);
}
// sleep(1);
}
// 4. read target velocity
while(1)
{
i++;
ecrt_master_receive(master);
if(i==1)
{
ecrt_sdo_request_read(sdo_operation_mode_display); // trigger read
}
switch (ecrt_sdo_request_state(sdo_operation_mode_display)) {
case EC_REQUEST_UNUSED: // request was not used yet
ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
// fprintf(stderr, "Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
printf("sdo_operation_mode_display value: 0x%02X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)));
if(EC_READ_U8(ecrt_sdo_request_data(sdo_operation_mode_display)) ^ 0x03 == 0)
{
printf("mode is in velocity_mode \n");
getModeOk = true;
}
else
{
printf("mode not in velocity_mode \n");
exit(-1);
}
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO!\n");
break;
}
if(getModeOk)
{
break;
}
ecrt_master_send(master);
}
// 5. set target velocity to zero
// 6. read statusword
printf("6.read statusword...\n");
bool isOperationDisabled=false;
// while(!isOperationDisabled)
while(1)
{
ecrt_master_receive(master);
EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x0080);// reset from fault
ecrt_sdo_request_write(sdo_controlword_write);
switch (ecrt_sdo_request_state(sdo_controlword_write)) {
case EC_REQUEST_UNUSED: // request was not used yet
// ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
printf("Request to Write,But Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "sdo_controlword_write write value 0x0080\n");
ecrt_sdo_request_write(sdo_controlword_write);
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO state!\n");
break;
}
ecrt_master_send(master);
// read state ,need switch_on_disabled
ecrt_sdo_request_read(sdo_statusword_read); // trigger read
switch (ecrt_sdo_request_state(sdo_statusword_read)) {
case EC_REQUEST_UNUSED: // request was not used yet
ecrt_sdo_request_read(sdo_statusword_read); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
printf("Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "statusword value: 0x%04X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)));
if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x004F ^ 0x0040 == 0) // p91
{
isOperationDisabled = true;
printf("motor state is in switch_on_disabled \n");
}
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO!\n");
break;
}
if(isOperationDisabled)
{
printf("good \n");
break;
}
ecrt_master_send(master);
}
// 7. set controlword to ready_to_switch_on
printf("7. set controlword , ready_to_switch_on...\n");
isOperationDisabled = false;
while(1)
{
ecrt_master_receive(master);
EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x0006);// reset from fault
ecrt_sdo_request_write(sdo_controlword_write);
switch (ecrt_sdo_request_state(sdo_controlword_write)) {
case EC_REQUEST_UNUSED: // request was not used yet
// ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
// printf("Request to Write,But Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "sdo_controlword_write write value 0x0006\n");
ecrt_sdo_request_write(sdo_controlword_write);
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO state!\n");
break;
}
ecrt_master_send(master);
// read state ,need switch_on_disabled
ecrt_sdo_request_read(sdo_statusword_read); // trigger read
switch (ecrt_sdo_request_state(sdo_statusword_read)) {
case EC_REQUEST_UNUSED: // request was not used yet
ecrt_sdo_request_read(sdo_statusword_read); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
// printf("Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "statusword value: 0x%04X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)));
if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F ^ 0x0021 == 0) // p91
{
isOperationDisabled = true;
printf("motor state is in ready_to_switch_on \n");
}
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO!\n");
break;
}
if(isOperationDisabled)
{
printf("good2 \n");
break;
}
ecrt_master_send(master);
}
// 8. set controlword , enable operation
printf("8. set controlword , enable operation...\n");
isOperationDisabled = false;
while(1)
{
ecrt_master_receive(master);
EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x000F);// reset from fault
ecrt_sdo_request_write(sdo_controlword_write);
switch (ecrt_sdo_request_state(sdo_controlword_write)) {
case EC_REQUEST_UNUSED: // request was not used yet
// ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
// printf("Request to Write,But Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "sdo_controlword_write write value 0x000F\n");
ecrt_sdo_request_write(sdo_controlword_write);
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO state!\n");
break;
}
ecrt_master_send(master);
// read state ,need switch_on_disabled
ecrt_sdo_request_read(sdo_statusword_read); // trigger read
switch (ecrt_sdo_request_state(sdo_statusword_read)) {
case EC_REQUEST_UNUSED: // request was not used yet
ecrt_sdo_request_read(sdo_statusword_read); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
// printf("Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "statusword value: 0x%04X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)));
fprintf(stderr, "statusword value aa: 0x%04X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F);
fprintf(stderr, "statusword value aaa: 0x%04X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F^ 0x0027);
if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x006F == 0x0027) // p91
{
isOperationDisabled = true;
printf("motor state is in operation_on \n");
}
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO!\n");
break;
}
if(isOperationDisabled)
{
printf("good2 \n");
break;
}
ecrt_master_send(master);
}
// 8. set target velocity 100r/min
// sa.sa_handler = signal_handler;
// sigemptyset(&sa.sa_mask);
// sa.sa_flags = 0;
// if (sigaction(SIGALRM, &sa, 0)) {
// fprintf(stderr, "Failed to install signal handler!\n");
// return -1;
// }
// printf("Started.\n");
// while (1) {
// pause();
//#if 0
// struct timeval t;
// gettimeofday(&t, NULL);
// printf("%u.%06u\n", t.tv_sec, t.tv_usec);
//#endif
// while (sig_alarms != user_alarms) {
// cyclic_task();
// user_alarms++;
// }
// }
return 0;
}
void cyclic_task()
{
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain_output);
ecrt_domain_process(domain_input);
// check process data state (optional)
check_domain1_state();
if (counter) {
counter--;
} else { // do this at 1 Hz
counter = FREQUENCY;
// check for master state (optional)
check_master_state();
// check for islave configuration state(s) (optional)
check_slave_config_states();
// read process data
// {0x6040, 0x00, 16}, /* Controlword */
// {0x6060, 0x00, 8}, /* Mode_of_Operation */
// {0x6098, 0x00, 8}, /* Homing_Method */
// {0x607a, 0x00, 32}, /* Target_Position */
// {0x60ff, 0x00, 32}, /* Target_Velocity */
// {0x6071, 0x00, 16}, /* Target_Torque */
// {0x6041, 0x00, 16}, /* Statusword */
// {0x6064, 0x00, 32}, /* Position_Actual_Value */
// {0x6061, 0x00, 8}, /* Modes_Of_Operation_Display */
// {0x1001, 0x00, 8}, /* Error_Register */
// {0x606c, 0x00, 32}, /* Velocity_Actual_Value */
// {0x6077, 0x00, 16}, /* Torque_Actual_Value */
// printf("pdo value: %02x offset %u\n",
// EC_READ_U16(domain1_pd + off_0x6040),off_0x6040);
// printf("pdo value: %02x offset %u\n",
// EC_READ_U16(domain1_pd + off_0x1001),off_0x1001);
printf("pdo value: %04x offset %u\n",
EC_READ_U16(domain_input_pd + off_0x6041),off_0x6041);
printf("pdo value 6061asfsadf: %04x offset %u\n",
EC_READ_U8(domain_input_pd + off_0x6061),off_0x6061);
printf("pd: %u \n",*domain_input_pd);
// EC_READ_U8(domain1_pd + off_ana_in_value));
#if SDO_ACCESS
// read process data SDO
read_sdo();
#endif
}
// send process data
ecrt_domain_queue(domain_output);
ecrt_domain_queue(domain_input);
ecrt_master_send(master);
#if 0
// write process data
// EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
#endif
}
int ec_slave_mbox_check(const ec_datagram_t *datagram /**< datagram */)
{
return EC_READ_U8(datagram->data + 5) & 8 ? 1 : 0;
}
/** Start reading data.
*/
void ec_fsm_foe_state_data_read(
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
size_t rec_size;
uint8_t *data, opCode, packet_no, mbox_prot;
ec_slave_t *slave = fsm->slave;
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
#endif
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE DATA READ datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE DATA READ failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
ec_foe_set_rx_error(fsm, FOE_MBOX_FETCH_ERROR);
return;
}
if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE
EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
mbox_prot);
ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
return;
}
opCode = EC_READ_U8(data);
if (opCode == EC_FOE_OPCODE_BUSY) {
if (ec_foe_prepare_send_ack(fsm, datagram)) {
ec_foe_set_rx_error(fsm, FOE_PROT_ERROR);
}
return;
}
if (opCode == EC_FOE_OPCODE_ERR) {
fsm->request->error_code = EC_READ_U32(data + 2);
EC_SLAVE_ERR(slave, "Received FoE Error Request (code 0x%08x).\n",
fsm->request->error_code);
if (rec_size > 6) {
uint8_t text[256];
strncpy(text, data + 6, min(rec_size - 6, sizeof(text)));
EC_SLAVE_ERR(slave, "FoE Error Text: %s\n", text);
}
ec_foe_set_rx_error(fsm, FOE_OPCODE_ERROR);
return;
}
if (opCode != EC_FOE_OPCODE_DATA) {
EC_SLAVE_ERR(slave, "Received OPCODE %x, expected %x.\n",
opCode, EC_FOE_OPCODE_DATA);
fsm->request->error_code = 0x00000000;
ec_foe_set_rx_error(fsm, FOE_OPCODE_ERROR);
return;
}
packet_no = EC_READ_U16(data + 2);
if (packet_no != fsm->rx_expected_packet_no) {
EC_SLAVE_ERR(slave, "Received unexpected packet number.\n");
ec_foe_set_rx_error(fsm, FOE_PACKETNO_ERROR);
return;
}
rec_size -= EC_FOE_HEADER_SIZE;
if (fsm->rx_buffer_size >= fsm->rx_buffer_offset + rec_size) {
memcpy(fsm->rx_buffer + fsm->rx_buffer_offset,
data + EC_FOE_HEADER_SIZE, rec_size);
fsm->rx_buffer_offset += rec_size;
}
fsm->rx_last_packet =
(rec_size + EC_MBOX_HEADER_SIZE + EC_FOE_HEADER_SIZE
!= slave->configured_rx_mailbox_size);
if (fsm->rx_last_packet ||
(slave->configured_rx_mailbox_size - EC_MBOX_HEADER_SIZE
- EC_FOE_HEADER_SIZE + fsm->rx_buffer_offset)
<= fsm->rx_buffer_size) {
// either it was the last packet or a new packet will fit into the
// delivered buffer
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "last_packet=true\n");
#endif
if (ec_foe_prepare_send_ack(fsm, datagram)) {
ec_foe_set_rx_error(fsm, FOE_RX_DATA_ACK_ERROR);
return;
}
fsm->state = ec_fsm_foe_state_sent_ack;
}
else {
// no more data fits into the delivered buffer
// ... wait for new read request
EC_SLAVE_ERR(slave, "Data do not fit in receive buffer!\n");
printk(" rx_buffer_size = %d\n", fsm->rx_buffer_size);
printk("rx_buffer_offset = %d\n", fsm->rx_buffer_offset);
printk(" rec_size = %zd\n", rec_size);
printk(" rx_mailbox_size = %d\n", slave->configured_rx_mailbox_size);
printk(" rx_last_packet = %d\n", fsm->rx_last_packet);
fsm->request->result = FOE_READY;
}
}
/** EoE state: SET IP RESPONSE.
*/
void ec_fsm_eoe_set_ip_response(
ec_fsm_eoe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
uint8_t *data, mbox_prot, frame_type;
size_t rec_size;
ec_eoe_request_t *req = fsm->request;
if (fsm->datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.
return;
}
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Failed to receive EoE read response datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Reception of EoE read response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
fsm->state = ec_fsm_eoe_error;
return;
}
if (master->debug_level) {
EC_SLAVE_DBG(slave, 0, "Set IP parameter response:\n");
ec_print_data(data, rec_size);
}
if (mbox_prot != EC_MBOX_TYPE_EOE) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
mbox_prot);
return;
}
if (rec_size < 4) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_ERR(slave, "Received currupted EoE set IP parameter response"
" (%zu bytes)!\n", rec_size);
ec_print_data(data, rec_size);
return;
}
frame_type = EC_READ_U8(data) & 0x0f;
if (frame_type != EC_EOE_FRAMETYPE_SET_IP_RES) {
EC_SLAVE_ERR(slave, "Received no set IP parameter response"
" (frame type %x).\n", frame_type);
ec_print_data(data, rec_size);
fsm->state = ec_fsm_eoe_error;
return;
}
req->result = EC_READ_U16(data + 2);
if (req->result) {
fsm->state = ec_fsm_eoe_error;
EC_SLAVE_DBG(slave, 1, "EoE set IP parameters failed with result code"
" 0x%04X.\n", req->result);
} else {
fsm->state = ec_fsm_eoe_end; // success
}
}
/**
SII state: READ FETCH.
Fetches the result of an SII-read datagram.
*/
void ec_fsm_sii_state_read_fetch(
ec_fsm_sii_t *fsm /**< finite state machine */
)
{
ec_datagram_t *datagram = fsm->datagram;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_sii_state_error;
EC_SLAVE_ERR(fsm->slave,
"Failed to receive SII check/fetch datagram: ");
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_sii_state_error;
EC_SLAVE_ERR(fsm->slave,
"Reception of SII check/fetch datagram failed: ");
ec_datagram_print_wc_error(datagram);
return;
}
#ifdef SII_DEBUG
EC_SLAVE_DBG(fsm->slave, 0, "checking SII read state:\n");
ec_print_data(datagram->data, 10);
#endif
if (EC_READ_U8(datagram->data + 1) & 0x20) {
EC_SLAVE_ERR(fsm->slave, "Error on last command while"
" reading from SII word 0x%04x.\n", fsm->word_offset);
fsm->state = ec_fsm_sii_state_error;
return;
}
// check "busy bit"
if (EC_READ_U8(datagram->data + 1) & 0x81) { /* busy bit or
read operation busy */
// still busy... timeout?
unsigned long diff_ms =
(datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms >= SII_TIMEOUT) {
if (fsm->check_once_more) {
fsm->check_once_more = 0;
} else {
EC_SLAVE_ERR(fsm->slave, "SII: Read timeout.\n");
fsm->state = ec_fsm_sii_state_error;
return;
}
}
// issue check/fetch datagram again
fsm->retries = EC_FSM_RETRIES;
return;
}
// SII value received.
memcpy(fsm->value, datagram->data + 6, 4);
fsm->state = ec_fsm_sii_state_end;
}
/** Acknowledge a read operation.
*/
void ec_fsm_foe_state_ack_read(
ec_fsm_foe_t *fsm, /**< FoE statemachine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
ec_slave_t *slave = fsm->slave;
uint8_t *data, mbox_prot;
uint8_t opCode;
size_t rec_size;
#ifdef DEBUG_FOE
EC_SLAVE_DBG(fsm->slave, 0, "%s()\n", __func__);
#endif
if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) {
ec_foe_set_rx_error(fsm, FOE_RECEIVE_ERROR);
EC_SLAVE_ERR(slave, "Failed to receive FoE ack response datagram: ");
ec_datagram_print_state(fsm->datagram);
return;
}
if (fsm->datagram->working_counter != 1) {
ec_foe_set_rx_error(fsm, FOE_WC_ERROR);
EC_SLAVE_ERR(slave, "Reception of FoE ack response failed: ");
ec_datagram_print_wc_error(fsm->datagram);
return;
}
data = ec_slave_mbox_fetch(slave, fsm->datagram, &mbox_prot, &rec_size);
if (IS_ERR(data)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
}
if (mbox_prot != EC_MBOX_TYPE_FILEACCESS) { // FoE
ec_foe_set_tx_error(fsm, FOE_MBOX_PROT_ERROR);
EC_SLAVE_ERR(slave, "Received mailbox protocol 0x%02X as response.\n",
mbox_prot);
return;
}
opCode = EC_READ_U8(data);
if (opCode == EC_FOE_OPCODE_BUSY) {
// slave not ready
if (ec_foe_prepare_data_send(fsm, datagram)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
EC_SLAVE_ERR(slave, "Slave is busy.\n");
return;
}
fsm->state = ec_fsm_foe_state_data_sent;
return;
}
if (opCode == EC_FOE_OPCODE_ACK) {
fsm->tx_packet_no++;
fsm->tx_buffer_offset += fsm->tx_current_size;
if (fsm->tx_last_packet) {
fsm->state = ec_fsm_foe_end;
return;
}
if (ec_foe_prepare_data_send(fsm, datagram)) {
ec_foe_set_tx_error(fsm, FOE_PROT_ERROR);
return;
}
fsm->state = ec_fsm_foe_state_data_sent;
return;
}
ec_foe_set_tx_error(fsm, FOE_ACK_ERROR);
}
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 pdo_handle_ecat(master_setup_variables_t *master_setup,
ctrlproto_slv_handle *slv_handles,
unsigned int total_no_of_slaves)
{
int slv;
if(sig_alarms == user_alarms) pause();
while (sig_alarms != user_alarms)
{
/* sync the dc clock of the slaves */
// ecrt_master_sync_slave_clocks(master);
// receive process data
ecrt_master_receive(master_setup->master);
ecrt_domain_process(master_setup->domain);
// check process data state (optional)
//check_domain1_state(master_setup);
// check for master state (optional)
//check_master_state(master_setup);
// check for islave configuration state(s) (optional)
// check_slave_config_states();
for(slv=0;slv<total_no_of_slaves;++slv)
{
/* Read process data */
slv_handles[slv].motorctrl_status_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[0]);
slv_handles[slv].operation_mode_disp = EC_READ_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[1]);
slv_handles[slv].position_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[2]);
slv_handles[slv].speed_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[3]);
slv_handles[slv].torque_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[4]);
}
/* printf("\n%x", slv_handles[slv].motorctrl_status_in);
printf("\n%x", slv_handles[slv].operation_mode_disp);
printf("\n%x", slv_handles[slv].position_in);
printf("\n%x", slv_handles[slv].speed_in);
printf("\n%x", slv_handles[slv].torque_in);
*/
for(slv=0;slv<total_no_of_slaves;++slv)
{
EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[0], (slv_handles[slv].motorctrl_out)&0xffff);
EC_WRITE_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[1], (slv_handles[slv].operation_mode)&0xff);
EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[2], (slv_handles[slv].torque_setpoint)&0xffff);
EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[3], slv_handles[slv].position_setpoint);
EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[4], slv_handles[slv].speed_setpoint);
}
// send process data
ecrt_domain_queue(master_setup->domain);
ecrt_master_send(master_setup->master);
//Check for master und domain state
ecrt_master_state(master_setup->master, &master_setup->master_state);
ecrt_domain_state(master_setup->domain, &master_setup->domain_state);
if (master_setup->domain_state.wc_state == EC_WC_COMPLETE && !master_setup->op_flag)
{
//printf("System up!\n");
master_setup->op_flag = 1;
}
else
{
if(master_setup->domain_state.wc_state != EC_WC_COMPLETE && master_setup->op_flag)
{
//printf("System down!\n");
master_setup->op_flag = 0;
}
}
user_alarms++;
}
}
int main(int argc, char **argv)
{
// ec_slave_config_t *sc;
struct sigaction sa;
struct itimerval tv;
master = ecrt_request_master(0);
if (!master)
return -1;
domain1 = ecrt_master_create_domain(master);
if (!domain1)
{
return -1;
}
if (!(sc_ana_in = ecrt_master_slave_config(master, AliasAndPositon, VendorID_ProductCode)))
{
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
// printf("sync mgr 0 config: %d \n",sc_ana_in->sync_configs[0].dir);
// printf("sync mgr 1 config: %d \n",sc_ana_in->sync_configs[1].dir);
#if SDO_ACCESS
fprintf(stderr, "Creating SDO requests...\n");
if (!(sdo = ecrt_slave_config_create_sdo_request(sc_ana_in, 0x6061, 0, 1))) // data size 1 ?
{
fprintf(stderr, "Failed to create SDO modes_of_operation_display 0x6061 request.\n");
return -1;
}
//EC_WRITE_U16(ecrt_sdo_request_data(sdo), 0xFFFF);
ecrt_sdo_request_timeout(sdo, 500); // ms
#endif
#if CONFIGURE_PDOS
printf("Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc_ana_in, EC_END, duetfl80_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
// if (!(sc = ecrt_master_slave_config(
// master, AnaOutSlavePos, Beckhoff_EL4102))) {
// fprintf(stderr, "Failed to get slave configuration.\n");
// return -1;
// }
// if (ecrt_slave_config_pdos(sc, EC_END, el4102_syncs)) {
// fprintf(stderr, "Failed to configure PDOs.\n");
// return -1;
// }
// if (!(sc = ecrt_master_slave_config(
// master, DigOutSlavePos, Beckhoff_EL2032))) {
// fprintf(stderr, "Failed to get slave configuration.\n");
// return -1;
// }
// if (ecrt_slave_config_pdos(sc, EC_END, el2004_syncs)) {
// fprintf(stderr, "Failed to configure PDOs.\n");
// return -1;
// }
#endif
// // Create configuration for bus coupler
// sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100);
// if (!sc)
// return -1;
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
printf("Activating master...\n");
if (ecrt_master_activate(master))
return -1;
if (!(domain1_pd = ecrt_domain_data(domain1))) {
return -1;
}
#if PRIORITY
pid_t pid = getpid();
if (setpriority(PRIO_PROCESS, pid, -19))
fprintf(stderr, "Warning: Failed to set priority: %s\n",
strerror(errno));
#endif
sa.sa_handler = signal_handler;
sigemptyset(&sa.sa_mask);
sa.sa_flags = 0;
if (sigaction(SIGALRM, &sa, 0)) {
fprintf(stderr, "Failed to install signal handler!\n");
return -1;
}
printf("Starting timer...\n");
// tv.it_interval.tv_sec = 0;
// tv.it_interval.tv_usec = 1000000 / FREQUENCY;
// tv.it_value.tv_sec = 0;
// tv.it_value.tv_usec = 1000;
// if (setitimer(ITIMER_REAL, &tv, NULL)) {
// fprintf(stderr, "Failed to start timer: %s\n", strerror(errno));
// return 1;
// }
printf("Started.\n");
while (1) {
// pause();
#if 1
// for(int i=0;i<10;++i)
// {
// my_cyclic_task();
// receive process data
ecrt_master_receive(master);
// ecrt_domain_process(domain1);
// check process data state (optional)
// check_domain1_state();
// check for master state (optional)
check_master_state();
// check for islave configuration state(s) (optional)
check_slave_config_states();
// read process data SDO
switch (ecrt_sdo_request_state(sdo)) {
case EC_REQUEST_UNUSED: // request was not used yet
printf("request was not used yet\n");
ecrt_sdo_request_read(sdo); // trigger first read
// ecrt_sdo_request_write(sdo);
break;
case EC_REQUEST_BUSY:
// fprintf(stderr, "Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
printf( "SDO value: 0x%04X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo)));
ecrt_sdo_request_read(sdo); // trigger next read
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO!\n");
ecrt_sdo_request_read(sdo); // retry reading
break;
}
// read_sdo();
// switch (ecrt_sdo_request_state(sdo)) {
// case EC_REQUEST_UNUSED: // request was not used yet
// printf("request was not used yet\n");
// ecrt_sdo_request_read(sdo); // trigger first read
// break;
// case EC_REQUEST_BUSY:
// fprintf(stderr, "Still busy...\n");
// break;
// case EC_REQUEST_SUCCESS:
// fprintf(stderr, "sdo value: 0x%04X\n",
// EC_READ_U8(ecrt_sdo_request_data(sdo)));
//// getModeOk = true;
// break;
// case EC_REQUEST_ERROR:
// fprintf(stderr, "Failed to read SDO!\n");
// break;
// }
// send process data
// ecrt_domain_queue(domain1);
// ecrt_sdo_request_read(sdo); // trigger next read
ecrt_master_send(master);
ecrt_master_receive(master);
// sleep(1);
// }
#else
// 1. check operation mode
bool getModeOk=false;
for(int i=0;i<10;++i)
{
printf("i=%d\n",i);
// receive process data
ecrt_master_receive(master);
// ecrt_domain_process(domain1);
// check process data state (optional)
// check_domain1_state();
// check for master state (optional)
check_master_state();
// check for islave configuration state(s) (optional)
check_slave_config_states();
// read_sdo();
// ecrt_sdo_request_read(sdo); // trigger read
switch (ecrt_sdo_request_state(sdo)) {
case EC_REQUEST_UNUSED: // request was not used yet
printf("request was not used yet\n");
ecrt_sdo_request_read(sdo); // trigger first read
break;
case EC_REQUEST_BUSY:
fprintf(stderr, "Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
fprintf(stderr, "sdo value: 0x%04X\n",
EC_READ_U8(ecrt_sdo_request_data(sdo)));
getModeOk = true;
break;
case EC_REQUEST_ERROR:
fprintf(stderr, "Failed to read SDO!\n");
break;
}
if(getModeOk)
{
break;
}
// ecrt_domain_queue(domain1);
ecrt_master_send(master);
// sleep(1);
// cyclic_task();
}
if(getModeOk == false)
{
// exit(-1);
}
#endif
}
return 0;
}
void cyclic_task()
{
/* sync the dc clock of the slaves */
ecrt_master_sync_slave_clocks(master);
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain1);
// check process data state (optional)
check_domain1_state();
if (counter) {
counter--;
} else { // do this at 1 Hz
counter = FREQUENCY;
// calculate new process data
blink = !blink;
// check for master state (optional)
check_master_state();
// check for islave configuration state(s) (optional)
check_slave_config_states();
#if SDO_ACCESS
// read process data SDO
read_sdo(sdo);
read_sdo(request[0]);
read_sdo(request[1]);
read_sdo(request[2]);
write_sdo(sdo_download_requests[0], sdoexample); /* SDO download value to the node */
#endif
}
/* Read process data */
unsigned int sn_status = EC_READ_U16(domain1_pd + off_pdo1_in);
unsigned int sn_modes = EC_READ_U8(domain1_pd + off_pdo2_in);
unsigned int sn_position = EC_READ_U32(domain1_pd + off_pdo3_in);
unsigned int sn_velocity = EC_READ_U32(domain1_pd + off_pdo4_in);
unsigned int sn_torque = EC_READ_U16(domain1_pd + off_pdo5_in);
logmsg(2, "[REC] 0x%4x 0x%4x 0x%8x 0x%8x 0x%4x\n",
sn_status, sn_modes,
sn_position, sn_velocity, sn_torque);
#if 0
// read process data
printf("AnaIn: state %u value %u\n",
EC_READ_U8(domain1_pd + off_ana_in_status),
EC_READ_U16(domain1_pd + off_ana_in_value));
#endif
// write process data
//EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
#ifdef CIA402
#define STATUSW1 0x88AA
#define STATUSW2 0xAA88
#define OPMODES1 0xf1
#define OPMODES2 0x1f
#define TORVAL1 0xabab
#define TORVAL2 0xbaba
#define VELVAL1 0x2d2d4d4d
#define VELVAL2 0xd4d4d2d2
#define POSVAL1 0xe4e4e2e2
#define POSVAL2 0x2e2e4e4e
EC_WRITE_U16(domain1_pd + off_pdo1_out, (blink ? STATUSW1 : STATUSW2)&0xffff);
EC_WRITE_U8(domain1_pd + off_pdo2_out, (blink ? OPMODES1 : OPMODES2)&0xff);
EC_WRITE_U16(domain1_pd + off_pdo3_out, (blink ? TORVAL1 : TORVAL2)&0xffff);
EC_WRITE_U32(domain1_pd + off_pdo4_out, blink ? POSVAL1 : POSVAL2);
EC_WRITE_U32(domain1_pd + off_pdo5_out, blink ? VELVAL1 : VELVAL2);
#else
#define TESTWORD1 0xdead
#define TESTWORD2 0xbeef
#define TESTWORD3 0xfefe
#define TESTWORD4 0xa5a5
EC_WRITE_U16(domain1_pd + off_pdo1_out, blink ? TESTWORD1 : TESTWORD2);
EC_WRITE_U16(domain1_pd + off_pdo2_out, blink ? TESTWORD3 : TESTWORD4);
#endif
// send process data
ecrt_domain_queue(domain1);
ecrt_master_send(master);
//printf("Wrote %x to slave\n", blink ? TESTWORD1 : TESTWORD2);
}
void ec_fsm_sii_state_write_check2(
ec_fsm_sii_t *fsm /**< finite state machine */
)
{
ec_datagram_t *datagram = fsm->datagram;
unsigned long diff_ms;
if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
return;
if (datagram->state != EC_DATAGRAM_RECEIVED) {
fsm->state = ec_fsm_sii_state_error;
EC_SLAVE_ERR(fsm->slave,
"Failed to receive SII write check datagram: ");
ec_datagram_print_state(datagram);
return;
}
if (datagram->working_counter != 1) {
fsm->state = ec_fsm_sii_state_error;
EC_SLAVE_ERR(fsm->slave,
"Reception of SII write check datagram failed: ");
ec_datagram_print_wc_error(datagram);
return;
}
#ifdef SII_DEBUG
EC_SLAVE_DBG(fsm->slave, 0, "checking SII write state:\n");
ec_print_data(datagram->data, 2);
#endif
if (EC_READ_U8(datagram->data + 1) & 0x20) {
EC_SLAVE_ERR(fsm->slave, "SII: Error on last SII command!\n");
fsm->state = ec_fsm_sii_state_error;
return;
}
/* FIXME: some slaves never answer with the busy flag set...
* wait a few ms for the write operation to complete. */
diff_ms = (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;
if (diff_ms < SII_INHIBIT) {
#ifdef SII_DEBUG
EC_SLAVE_DBG(fsm->slave, 0, "too early.\n");
#endif
// issue check datagram again
fsm->retries = EC_FSM_RETRIES;
return;
}
if (EC_READ_U8(datagram->data + 1) & 0x82) { /* busy bit or
write operation busy bit */
// still busy... timeout?
if (diff_ms >= SII_TIMEOUT) {
if (fsm->check_once_more) {
fsm->check_once_more = 0;
} else {
EC_SLAVE_ERR(fsm->slave, "SII: Write timeout.\n");
fsm->state = ec_fsm_sii_state_error;
return;
}
}
// issue check datagram again
fsm->retries = EC_FSM_RETRIES;
return;
}
if (EC_READ_U8(datagram->data + 1) & 0x40) {
EC_SLAVE_ERR(fsm->slave, "SII: Write operation failed!\n");
fsm->state = ec_fsm_sii_state_error;
return;
}
// success
fsm->state = ec_fsm_sii_state_end;
}