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, AnaInSlavePos, Beckhoff_EL3102))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
#if SDO_ACCESS
fprintf(stderr, "Creating SDO requests...\n");
if (!(sdo = ecrt_slave_config_create_sdo_request(sc_ana_in, 0x3102, 2, 2))) {
fprintf(stderr, "Failed to create SDO request.\n");
return -1;
}
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, el3102_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 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;
}
int __init init_mod(void)
{
int ret = -1, i;
RTIME tick_period, requested_ticks, now;
ec_slave_config_t *sc;
printk(KERN_INFO PFX "Starting...\n");
rt_sem_init(&master_sem, 1);
t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000;
master = ecrt_request_master(0);
if (!master) {
ret = -EBUSY;
printk(KERN_ERR PFX "Requesting master 0 failed!\n");
goto out_return;
}
ecrt_master_callbacks(master, send_callback, receive_callback, master);
printk(KERN_INFO PFX "Registering domain...\n");
if (!(domain1 = ecrt_master_create_domain(master))) {
printk(KERN_ERR PFX "Domain creation failed!\n");
goto out_release_master;
}
// if (!(domain2 = ecrt_master_create_domain(master))) {
// printk(KERN_ERR PFX "Domain2 creation failed!\n");
// goto out_release_master;
// }
printk(KERN_INFO PFX "Configuring PDOs...\n");
if (!(sc = ecrt_master_slave_config(master, yas, yaskawa))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
// ecrt_slave_config_dc(sc, 0x300, 1000000, 0, 0, 0);
if (ecrt_slave_config_sdo8(sc, 0x6060, 0, 8)){
printk(KERN_ERR PFX "Failed to configure SDOs.\n");
goto out_release_master;
}
if (ecrt_slave_config_pdos(sc, EC_END, slave_0_syncs)) {
printk(KERN_ERR PFX "Failed to configure PDOs.\n");
goto out_release_master;
}
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
printk(KERN_ERR PFX "1st motor RX_PDO entry registration failed!\n");
goto out_release_master;
}
printk(KERN_INFO PFX "Activating master...\n");
if (ecrt_master_activate(master)) {
printk(KERN_ERR PFX "Failed to activate master!\n");
goto out_release_master;
}
// Get internal process data for domain
domain1_pd = ecrt_domain_data(domain1);
// domain2_pd = ecrt_domain_data(domain2);
printk(KERN_INFO PFX "Starting cyclic sample thread...\n");
requested_ticks = nano2count(TIMERTICKS);
tick_period = start_rt_timer(requested_ticks);
printk(KERN_INFO PFX "RT timer started with %i/%i ticks.\n",
(int) tick_period, (int) requested_ticks);
if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) {
printk(KERN_ERR PFX "Failed to init RTAI task!\n");
goto out_stop_timer;
}
now = rt_get_time();
if (rt_task_make_periodic(&task, now + tick_period, tick_period)) {
printk(KERN_ERR PFX "Failed to run RTAI task!\n");
goto out_stop_task;
}
printk(KERN_INFO PFX "Initialized.\n");
return 0;
out_stop_task:
rt_task_delete(&task);
out_stop_timer:
stop_rt_timer();
out_release_master:
printk(KERN_ERR PFX "Releasing master...\n");
ecrt_release_master(master);
out_return:
rt_sem_delete(&master_sem);
printk(KERN_ERR PFX "Failed to load. Aborting.\n");
return ret;
}
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 init_master(master_setup_variables_t *master_setup, ctrlproto_slv_handle *slv_handles, unsigned int total_no_of_slaves)
{
int slv;
struct sigaction sa;
struct itimerval tv;
master_setup->master = ecrt_request_master(0);
if (!master_setup->master)
exit(-1);
master_setup->domain = ecrt_master_create_domain(master_setup->master);
if (!master_setup->domain)
exit(-1);
for (slv = 0; slv < total_no_of_slaves; ++slv)
{
if (!( slv_handles[slv].slave_config= ecrt_master_slave_config( //sc_data_in
master_setup->master, slv_handles[slv].slave_alias, slv_handles[slv].slave_pos , slv_handles[slv].slave_vendorid, slv_handles[slv].slave_productid))) {
fprintf(stderr, "Failed to get slave configuration.\n");
exit(-1);
}
//logmsg(1, "Configuring PDOs...\n");
if (ecrt_slave_config_pdos(slv_handles[slv].slave_config, EC_END, slv_handles[slv].__sync_info)) { //slave_0_syncs
fprintf(stderr, "Failed to configure PDOs.\n");
exit(-1);
}
//#if PARAMETER_UPDATE
motor_config_request(slv_handles[slv].slave_config, slv_handles[slv].__request);
//#endif
}
if (ecrt_domain_reg_pdo_entry_list(master_setup->domain, master_setup->domain_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
exit(-1);
}
if (ecrt_master_set_send_interval(master_setup->master, FREQUENCY) != 0) {
fprintf(stderr, "failed to set send interval\n");
exit(-1);
}
logmsg(1, "Activating master...\n");
if (ecrt_master_activate(master_setup->master))
exit(-1);
if (!(master_setup->domain_pd = ecrt_domain_data(master_setup->domain))) {
exit(-1);
}
pid_t pid = getpid();
if (setpriority(PRIO_PROCESS, pid, -19))
fprintf(stderr, "Warning: Failed to set priority: %s\n",
strerror(errno));
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");
exit(-1);
}
//logmsg(1, "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));
exit(-1);
}
logmsg(0, "Started.\n");
}
int main(int argc, char **argv)
{
ec_slave_config_t *sc;
if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
{
perror("mlockall failed");
return -1;
}
master = ecrt_request_master(0);
if (!master)
return -1;
domain_r = ecrt_master_create_domain(master);
if (!domain_r)
return -1;
domain_w = ecrt_master_create_domain(master);
if (!domain_w)
return -1;
if (!(sc = ecrt_master_slave_config(master, pana, panasonic)))
{
fprintf(stderr, "Failed to get slave1 configuration.\n");
return -1;
}
#if SDO_ACCESS
if (ecrt_slave_config_sdo8(sc, 0x6060, 0, 8))
{
return -1;
}
#endif
#if CONFIGURE_PDOS
printf("Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc, EC_END, slave_0_syncs))
{
fprintf(stderr, "Failed to configure 1st PDOs.\n");
return -1;
}
#endif
if (ecrt_domain_reg_pdo_entry_list(domain_r, domain_r_regs))
{
fprintf(stderr, "1st motor RX_PDO entry registration failed!\n");
return -1;
}
if (ecrt_domain_reg_pdo_entry_list(domain_w, domain_w_regs))
{
fprintf(stderr, "1st motor TX_PDO entry registration failed!\n");
return -1;
}
ecrt_slave_config_dc(sc,0x0300,500000,0,0,0);
printf("Activating master...\n");
if (ecrt_master_activate(master))
return -1;
if (!(domain_r_pd = ecrt_domain_data(domain_r)))
{
return -1;
}
if (!(domain_w_pd = ecrt_domain_data(domain_w)))
{
return -1;
}
pid_t pid = getpid();
if (setpriority(PRIO_PROCESS, pid, -20))
fprintf(stderr, "Warning: Failed to set priority: %s\n",
strerror(errno));
signal( SIGINT , endsignal );
printf("Starting cyclic function.\n");
cyclic_task();
ecrt_release_master(master);
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;
}
// 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;
}
int main(int argc, char **argv)
{
cmdline(argc, 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_data_in = ecrt_master_slave_config(
master, SomanetPos, SOMANET_ID))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
#if SDO_ACCESS
fprintf(stderr, "Creating SDO requests...\n");
if (!(sdo = ecrt_slave_config_create_sdo_request(sc_data_in, 0x6041, 0, 1))) {
fprintf(stderr, "Failed to create SDO request.\n");
return -1;
}
ecrt_sdo_request_timeout(sdo, 500); // ms
if (!(request[0] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_POS_VALUE, 0, 4))) {
fprintf(stderr, "Failed to create SDO request for object 0x%4x\n", CAN_OD_POS_VALUE);
return -1;
}
ecrt_sdo_request_timeout(request[0], 500); // ms
if (!(request[1] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_VEL_VALUE, 0, 4))) {
fprintf(stderr, "Failed to create SDO request for object 0x%4x\n", CAN_OD_VEL_VALUE);
return -1;
}
ecrt_sdo_request_timeout(request[1], 500); // ms
if (!(request[2] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_TOR_VALUE, 0, 2))) {
fprintf(stderr, "Failed to create SDO request for object 0x%4x\n", CAN_OD_TOR_VALUE);
return -1;
}
ecrt_sdo_request_timeout(request[2], 500); // ms
/* register sdo download request */
if (!(sdo_download_requests[0] = ecrt_slave_config_create_sdo_request(sc_data_in, CAN_OD_MODES, 0, 4))) {
fprintf(stderr, "Failed to create SDO download request for object 0x%4x\n", CAN_OD_MODES);
return -1;
}
ecrt_sdo_request_timeout(sdo_download_requests[0], 500); // ms
/* set the sdoexample to a specific bit muster */
sdoexample = 0x22442244;
#endif
#if CONFIGURE_PDOS
logmsg(1, "Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc_data_in, EC_END, slave_0_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
if (!(sc = ecrt_master_slave_config(
master, SomanetPos, SOMANET_ID))) {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
#if 0
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
#endif
// Create configuration for bus coupler
sc = ecrt_master_slave_config(master, SomanetPos /*BusCouplerPos*/, SOMANET_ID/*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;
}
logmsg(1, "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;
}
logmsg(1, "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;
}
logmsg(0, "Started.\n");
while (1) {
pause();
#if 0
struct timeval t;
gettimeofday(&t, NULL);
printf("%u.%06u\n", (unsigned)t.tv_sec, (unsigned)t.tv_usec);
#endif
while (sig_alarms != user_alarms) {
cyclic_task();
user_alarms++;
}
}
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;
domain_input = ecrt_master_create_domain(master);
domain_output = ecrt_master_create_domain(master);
if (!domain_input || !domain_output)
{
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 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 1
printf("Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc_ana_in, EC_END, slave_0_syncs)) {
fprintf(stderr, "Failed to configure PDOs.\n");
return -1;
}
#endif
if (ecrt_domain_reg_pdo_entry_list(domain_output, domain_output_regs)) {
fprintf(stderr, "Output PDO entry registration failed!\n");
return -1;
}
if (ecrt_domain_reg_pdo_entry_list(domain_input, domain_input_regs)) {
fprintf(stderr, "Input PDO entry registration failed!\n");
return -1;
}
printf("Activating master...\n");
if (ecrt_master_activate(master))
return -1;
if (!(domain_output_pd = ecrt_domain_data(domain_output))) {
return -1;
}
if (!(domain_input_pd = ecrt_domain_data(domain_input))) {
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 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;
}
int __init init_mod(void)
{
int ret = -1, i;
RTIME tick_period, requested_ticks, now;
ec_slave_config_t *sc;
ec_slave_config_t *sc2;
printk(KERN_INFO PFX "Starting...\n");
rt_sem_init(&master_sem, 1);
t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000;
master = ecrt_request_master(0);
if (!master) {
ret = -EBUSY;
printk(KERN_ERR PFX "Requesting master 0 failed!\n");
goto out_return;
}
ecrt_master_callbacks(master, send_callback, receive_callback, master);
printk(KERN_INFO PFX "Registering domain...\n");
if (!(domain1 = ecrt_master_create_domain(master))) {
printk(KERN_ERR PFX "Domain creation failed!\n");
goto out_release_master;
}
if (!(domain2 = ecrt_master_create_domain(master))) {
printk(KERN_ERR PFX "Domain2 creation failed!\n");
goto out_release_master;
}
printk(KERN_INFO PFX "Configuring PDOs...\n");
// create configuration for reference clock FIXME
if (!(sc = ecrt_master_slave_config(master, yas, yaskawa))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
if (!(sc2 = ecrt_master_slave_config(master, yas2, yaskawa))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
/*
for (i = 0; i < NUM_DIG_OUT; i++) {
if (!(sc = ecrt_master_slave_config(master,
DigOutSlavePos(i), Beckhoff_EL2008))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
if (ecrt_slave_config_pdos(sc, EC_END, el2008_syncs)) {
printk(KERN_ERR PFX "Failed to configure PDOs.\n");
goto out_release_master;
}
off_dig_out[i] = ecrt_slave_config_reg_pdo_entry(sc,
0x7000, 1, domain1, NULL);
if (off_dig_out[i] < 0)
goto out_release_master;
}
*/
/*
if (!(sc = ecrt_master_slave_config(master,
CounterSlavePos, IDS_Counter))) {
printk(KERN_ERR PFX "Failed to get slave configuration.\n");
goto out_release_master;
}
off_counter_in = ecrt_slave_config_reg_pdo_entry(sc,
0x6020, 0x11, domain1, NULL);
if (off_counter_in < 0)
goto out_release_master;
off_counter_out = ecrt_slave_config_reg_pdo_entry(sc,
0x7020, 1, domain1, NULL);
if (off_counter_out < 0)
goto out_release_master;
*/
if (ecrt_slave_config_pdos(sc, EC_END, slave_0_syncs)) {
printk(KERN_ERR PFX "Failed to configure PDOs.\n");
goto out_release_master;
}
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
printk(KERN_ERR PFX "1st motor RX_PDO entry registration failed!\n");
goto out_release_master;
}
if (ecrt_domain_reg_pdo_entry_list(domain2, domain2_regs)) {
printk(KERN_ERR PFX "1st motor RX_PDO entry registration failed!\n");
goto out_release_master;
}
// configure SYNC signals for this slave
//ecrt_slave_config_dc(sc, 0x0700, 1000000, 440000, 0, 0);
ecrt_slave_config_dc(sc, 0x0300, 1000000, 125000, 0, 0);
// ecrt_slave_config_dc(sc2, 0x0300, 1000000, 125000, 0, 0);
printk(KERN_INFO PFX "Activating master...\n");
if (ecrt_master_activate(master)) {
printk(KERN_ERR PFX "Failed to activate master!\n");
goto out_release_master;
}
// Get internal process data for domain
domain1_pd = ecrt_domain_data(domain1);
domain2_pd = ecrt_domain_data(domain2);
printk(KERN_INFO PFX "Starting cyclic sample thread...\n");
requested_ticks = nano2count(TIMERTICKS);
tick_period = start_rt_timer(requested_ticks);
printk(KERN_INFO PFX "RT timer started with %i/%i ticks.\n",
(int) tick_period, (int) requested_ticks);
if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) {
printk(KERN_ERR PFX "Failed to init RTAI task!\n");
goto out_stop_timer;
}
now = rt_get_time();
if (rt_task_make_periodic(&task, now + tick_period, tick_period)) {
printk(KERN_ERR PFX "Failed to run RTAI task!\n");
goto out_stop_task;
}
printk(KERN_INFO PFX "Initialized.\n");
return 0;
out_stop_task:
rt_task_delete(&task);
out_stop_timer:
stop_rt_timer();
out_release_master:
printk(KERN_ERR PFX "Releasing master...\n");
ecrt_release_master(master);
out_return:
rt_sem_delete(&master_sem);
printk(KERN_ERR PFX "Failed to load. Aborting.\n");
return ret;
}
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;
}
