void ec_fsm_sii_state_start_reading(
ec_fsm_sii_t *fsm /**< finite state machine */
)
{
ec_datagram_t *datagram = fsm->datagram;
// initiate read operation
switch (fsm->mode) {
case EC_FSM_SII_USE_INCREMENT_ADDRESS:
ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x502, 4);
break;
case EC_FSM_SII_USE_CONFIGURED_ADDRESS:
ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 4);
break;
}
EC_WRITE_U8 (datagram->data, 0x80); // two address octets
EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation
EC_WRITE_U16(datagram->data + 2, fsm->word_offset);
#ifdef SII_DEBUG
EC_SLAVE_DBG(fsm->slave, 0, "reading SII data, word %u:\n",
fsm->word_offset);
ec_print_data(datagram->data, 4);
#endif
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_sii_state_read_check;
}
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);
}
/** Initializes a sync manager configuration page.
*
* The referenced memory (\a data) must be at least \a EC_SYNC_SIZE bytes.
*/
void ec_sync_page(
const ec_sync_t *sync, /**< Sync manager. */
uint8_t sync_index, /**< Index of the sync manager. */
uint16_t data_size, /**< Data size. */
const ec_sync_config_t *sync_config, /**< Configuration. */
uint8_t pdo_xfer, /**< Non-zero, if PDOs will be transferred via this
sync manager. */
uint8_t *data /**> Configuration memory. */
)
{
// enable only if (SII enable is set or PDO xfer)
// and size is > 0 and SM is not virtual
uint16_t enable = ((sync->enable & 0x01) || pdo_xfer)
&& data_size
&& ((sync->enable & 0x04) == 0);
uint8_t control = sync->control_register;
if (sync_config) {
switch (sync_config->dir) {
case EC_DIR_OUTPUT:
case EC_DIR_INPUT:
EC_WRITE_BIT(&control, 2,
sync_config->dir == EC_DIR_OUTPUT ? 1 : 0);
EC_WRITE_BIT(&control, 3, 0);
break;
default:
break;
}
switch (sync_config->watchdog_mode) {
case EC_WD_ENABLE:
case EC_WD_DISABLE:
EC_WRITE_BIT(&control, 6,
sync_config->watchdog_mode == EC_WD_ENABLE);
break;
default:
break;
}
}
EC_SLAVE_DBG(sync->slave, 1, "SM%u: Addr 0x%04X, Size %3u,"
" Ctrl 0x%02X, En %u\n",
sync_index, sync->physical_start_address,
data_size, control, enable);
EC_WRITE_U16(data, sync->physical_start_address);
EC_WRITE_U16(data + 2, data_size);
EC_WRITE_U8 (data + 4, control);
EC_WRITE_U8 (data + 5, 0x00); // status byte (read only)
EC_WRITE_U16(data + 6, enable);
}
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
}
/** Initializes an FMMU configuration page.
*
* The referenced memory (\a data) must be at least EC_FMMU_PAGE_SIZE bytes.
*/
void ec_fmmu_config_page(
const ec_fmmu_config_t *fmmu, /**< EtherCAT FMMU configuration. */
const ec_sync_t *sync, /**< Sync manager. */
uint8_t *data /**> Configuration page memory. */
)
{
EC_CONFIG_DBG(fmmu->sc, 1, "FMMU: LogAddr 0x%08X, Size %3u,"
" PhysAddr 0x%04X, SM%u, Dir %s\n",
fmmu->logical_start_address, fmmu->data_size,
sync->physical_start_address, fmmu->sync_index,
fmmu->dir == EC_DIR_INPUT ? "in" : "out");
EC_WRITE_U32(data, fmmu->logical_start_address);
EC_WRITE_U16(data + 4, fmmu->data_size); // size of fmmu
EC_WRITE_U8 (data + 6, 0x00); // logical start bit
EC_WRITE_U8 (data + 7, 0x07); // logical end bit
EC_WRITE_U16(data + 8, sync->physical_start_address);
EC_WRITE_U8 (data + 10, 0x00); // physical start bit
EC_WRITE_U8 (data + 11, fmmu->dir == EC_DIR_INPUT ? 0x01 : 0x02);
EC_WRITE_U16(data + 12, 0x0001); // enable
EC_WRITE_U16(data + 14, 0x0000); // reserved
}
void ec_fsm_sii_state_start_writing(
ec_fsm_sii_t *fsm /**< finite state machine */
)
{
ec_datagram_t *datagram = fsm->datagram;
// initiate write operation
ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 8);
EC_WRITE_U8 (datagram->data, 0x81); /* two address octets
+ enable write access */
EC_WRITE_U8 (datagram->data + 1, 0x02); // request write operation
EC_WRITE_U16(datagram->data + 2, fsm->word_offset);
memset(datagram->data + 4, 0x00, 2);
memcpy(datagram->data + 6, fsm->value, 2);
#ifdef SII_DEBUG
EC_SLAVE_DBG(fsm->slave, 0, "writing SII data:\n");
ec_print_data(datagram->data, 8);
#endif
fsm->retries = EC_FSM_RETRIES;
fsm->state = ec_fsm_sii_state_write_check;
}
uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */
ec_datagram_t *datagram, /**< datagram */
uint8_t type, /**< mailbox protocol */
size_t size /**< size of the data */
)
{
size_t total_size;
int ret;
if (unlikely(!slave->sii.mailbox_protocols)) {
EC_SLAVE_ERR(slave, "Slave does not support mailbox"
" communication!\n");
return ERR_PTR(-EPROTONOSUPPORT);
}
total_size = EC_MBOX_HEADER_SIZE + size;
if (unlikely(total_size > slave->configured_rx_mailbox_size)) {
EC_SLAVE_ERR(slave, "Data size (%zu) does not fit in mailbox (%u)!\n",
total_size, slave->configured_rx_mailbox_size);
return ERR_PTR(-EOVERFLOW);
}
ret = ec_datagram_fpwr(datagram, slave->station_address,
slave->configured_rx_mailbox_offset,
slave->configured_rx_mailbox_size);
if (ret)
return ERR_PTR(ret);
EC_WRITE_U16(datagram->data, size); // mailbox service data length
EC_WRITE_U16(datagram->data + 2, slave->station_address); // station addr.
EC_WRITE_U8 (datagram->data + 4, 0x00); // channel & priority
EC_WRITE_U8 (datagram->data + 5, type); // underlying protocol type
return datagram->data + EC_MBOX_HEADER_SIZE;
}
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 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
#if 1
// 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);
}
/** Add a PDO entry.
*/
void ec_fsm_pdo_entry_conf_state_map_entry(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping 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_WARN(fsm->slave, "Failed to map PDO entry"
" 0x%04X:%02X (%u bit) to position %u.\n",
fsm->entry->index, fsm->entry->subindex,
fsm->entry->bit_length, fsm->entry_pos);
EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
// find next entry
if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(
fsm, &fsm->entry->list))) {
// No more entries to add. Write entry count.
EC_WRITE_U8(fsm->request.data, fsm->entry_pos);
fsm->request.data_size = 1;
ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index, 0);
ecrt_sdo_request_write(&fsm->request);
EC_SLAVE_DBG(fsm->slave, 1, "Setting number of PDO entries to %u.\n",
fsm->entry_pos);
fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
return;
}
// add next entry
fsm->entry_pos++;
ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
}
/** Start PDO mapping.
*/
void ec_fsm_pdo_entry_conf_state_start(
ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
if (ec_sdo_request_alloc(&fsm->request, 4)) {
fsm->state = ec_fsm_pdo_entry_state_error;
return;
}
// set mapped PDO entry count to zero
EC_WRITE_U8(fsm->request.data, 0);
fsm->request.data_size = 1;
ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index, 0);
ecrt_sdo_request_write(&fsm->request);
EC_SLAVE_DBG(fsm->slave, 1, "Setting entry count to zero.\n");
fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;
ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
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;
}
/** Prepare a set IP parameters operation.
*
* \return 0 on success, otherwise a negative error code.
*/
int ec_fsm_eoe_prepare_set(
ec_fsm_eoe_t *fsm, /**< finite state machine */
ec_datagram_t *datagram /**< Datagram to use. */
)
{
uint8_t *data, *cur;
ec_slave_t *slave = fsm->slave;
ec_master_t *master = slave->master;
ec_eoe_request_t *req = fsm->request;
size_t size = 8;
if (req->mac_address_included) {
size += ETH_ALEN;
}
if (req->ip_address_included) {
size += 4;
}
if (req->subnet_mask_included) {
size += 4;
}
if (req->gateway_included) {
size += 4;
}
if (req->dns_included) {
size += 4;
}
if (req->name_included) {
size += EC_MAX_HOSTNAME_SIZE;
}
data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_EOE,
size);
if (IS_ERR(data)) {
return PTR_ERR(data);
}
EC_WRITE_U8(data, EC_EOE_FRAMETYPE_SET_IP_REQ); // Set IP parameter req.
EC_WRITE_U8(data + 1, 0x01); // last fragment, no timestamps
EC_WRITE_U16(data + 2, 0x0000); // fragment no., offset, frame no.
EC_WRITE_U32(data + 4,
((req->mac_address_included != 0) << 0) |
((req->ip_address_included != 0) << 1) |
((req->subnet_mask_included != 0) << 2) |
((req->gateway_included != 0) << 3) |
((req->dns_included != 0) << 4) |
((req->name_included != 0) << 5)
);
cur = data + 8;
if (req->mac_address_included) {
memcpy(cur, req->mac_address, ETH_ALEN);
cur += ETH_ALEN;
}
if (req->ip_address_included) {
memcpy(cur, &req->ip_address, 4);
cur += 4;
}
if (req->subnet_mask_included) {
memcpy(cur, &req->subnet_mask, 4);
cur += 4;
}
if (req->gateway_included) {
memcpy(cur, &req->gateway, 4);
cur += 4;
}
if (req->dns_included) {
memcpy(cur, &req->dns, 4);
cur += 4;
}
if (req->name_included) {
memcpy(cur, req->name, EC_MAX_HOSTNAME_SIZE);
cur += EC_MAX_HOSTNAME_SIZE;
}
if (master->debug_level) {
EC_SLAVE_DBG(slave, 0, "Set IP parameter request:\n");
ec_print_data(data, cur - data);
}
fsm->request->jiffies_sent = jiffies;
return 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++;
}
}
void run(long data)
{
int i;
struct timeval tv;
unsigned int sync_ref_counter = 0;
count2timeval(nano2count(rt_get_real_time_ns()), &tv);
while (1) {
t_last_cycle = get_cycles();
// receive process data
rt_sem_wait(&master_sem);
ecrt_master_receive(master);
ecrt_domain_process(domain1);
rt_sem_signal(&master_sem);
// check process data state (optional)
check_domain1_state();
if (counter) {
counter--;
} else {
u32 c;
counter = FREQUENCY;
// check for master state (optional)
check_master_state();
c = EC_READ_U32(domain1_pd + off_counter_in);
if (counter_value != c) {
counter_value = c;
printk(KERN_INFO PFX "counter=%u\n", counter_value);
}
}
if (blink_counter) {
blink_counter--;
} else {
blink_counter = 9;
// calculate new process data
blink = !blink;
}
// write process data
for (i = 0; i < NUM_DIG_OUT; i++) {
EC_WRITE_U8(domain1_pd + off_dig_out[i], blink ? 0x66 : 0x99);
}
EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02);
rt_sem_wait(&master_sem);
tv.tv_usec += 1000;
if (tv.tv_usec >= 1000000) {
tv.tv_usec -= 1000000;
tv.tv_sec++;
}
ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 9;
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
ecrt_domain_queue(domain1);
ecrt_master_send(master);
rt_sem_signal(&master_sem);
rt_task_wait_period();
}
}
void el60xx_port_run(el60xx_port_t *port, u8 *pd)
{
u16 status = EC_READ_U16(pd + port->off_status);
u8 *rx_data = pd + port->off_rx;
uint8_t tx_accepted_toggle, rx_request_toggle;
switch (port->state) {
case SER_READY:
/* Check, if hardware handshaking has to be configured. */
if (!port->config_error &&
port->requested_rtscts != port->current_rtscts) {
EC_WRITE_U8(ecrt_sdo_request_data(port->rtscts_sdo),
port->requested_rtscts);
ecrt_sdo_request_write(port->rtscts_sdo);
port->state = SER_SET_RTSCTS;
break;
}
/* Check, if the baud rate has to be configured. */
if (!port->config_error &&
port->requested_baud_rate != port->current_baud_rate) {
EC_WRITE_U8(ecrt_sdo_request_data(port->baud_sdo),
port->requested_baud_rate);
ecrt_sdo_request_write(port->baud_sdo);
port->state = SER_SET_BAUD_RATE;
break;
}
/* Check, if the data frame has to be configured. */
if (!port->config_error &&
port->requested_data_frame != port->current_data_frame) {
EC_WRITE_U8(ecrt_sdo_request_data(port->frame_sdo),
port->requested_data_frame);
ecrt_sdo_request_write(port->frame_sdo);
port->state = SER_SET_DATA_FRAME;
break;
}
/* Send data */
tx_accepted_toggle = status & 0x0001;
if (tx_accepted_toggle != port->tx_accepted_toggle) { // ready
port->tx_data_size =
ectty_tx_data(port->tty, port->tx_data, port->max_tx_data_size);
if (port->tx_data_size) {
#if DEBUG
printk(KERN_INFO PFX "%s: Sending %u bytes.\n",
port->name, port->tx_data_size);
#endif
port->tx_request_toggle = !port->tx_request_toggle;
port->tx_accepted_toggle = tx_accepted_toggle;
}
}
/* Receive data */
rx_request_toggle = status & 0x0002;
if (rx_request_toggle != port->rx_request_toggle) {
uint8_t rx_data_size = status >> 8;
port->rx_request_toggle = rx_request_toggle;
#if DEBUG
printk(KERN_INFO PFX "%s: Received %u bytes.\n",
port->name, rx_data_size);
#endif
ectty_rx_data(port->tty, rx_data, rx_data_size);
port->rx_accepted_toggle = !port->rx_accepted_toggle;
}
port->control =
port->tx_request_toggle |
port->rx_accepted_toggle << 1 |
port->tx_data_size << 8;
break;
case SER_REQUEST_INIT:
if (status & (1 << 2)) {
port->control = 0x0000;
port->state = SER_WAIT_FOR_INIT_RESPONSE;
} else {
port->control = 1 << 2; // CW.2, request initialization
}
break;
case SER_WAIT_FOR_INIT_RESPONSE:
if (!(status & (1 << 2))) {
printk(KERN_INFO PFX "%s: Init successful.\n", port->name);
port->tx_accepted_toggle = 1;
port->control = 0x0000;
port->state = SER_READY;
}
break;
case SER_SET_RTSCTS:
switch (ecrt_sdo_request_state(port->rtscts_sdo)) {
case EC_REQUEST_SUCCESS:
printk(KERN_INFO PFX "%s: Accepted RTS/CTS.\n",
port->name);
port->current_rtscts = port->requested_rtscts;
port->state = SER_REQUEST_INIT;
break;
case EC_REQUEST_ERROR:
printk(KERN_ERR PFX "Failed to set RTS/CTS on %s!\n",
port->name);
port->state = SER_REQUEST_INIT;
port->config_error = 1;
break;
default:
break;
}
break;
case SER_SET_BAUD_RATE:
switch (ecrt_sdo_request_state(port->baud_sdo)) {
case EC_REQUEST_SUCCESS:
printk(KERN_INFO PFX "%s: Accepted baud rate.\n",
port->name);
port->current_baud_rate = port->requested_baud_rate;
port->state = SER_REQUEST_INIT;
break;
case EC_REQUEST_ERROR:
printk(KERN_ERR PFX "Failed to set baud rate on %s!\n",
port->name);
port->state = SER_REQUEST_INIT;
port->config_error = 1;
break;
default:
break;
}
break;
case SER_SET_DATA_FRAME:
switch (ecrt_sdo_request_state(port->frame_sdo)) {
case EC_REQUEST_SUCCESS:
printk(KERN_INFO PFX "%s: Accepted data frame.\n",
port->name);
port->current_data_frame = port->requested_data_frame;
port->state = SER_REQUEST_INIT;
break;
case EC_REQUEST_ERROR:
printk(KERN_ERR PFX "Failed to set data frame on %s!\n",
port->name);
port->state = SER_REQUEST_INIT;
port->config_error = 1;
break;
default:
break;
}
break;
}
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);
}
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;
}
