Exemplo n.º 1
0
int __init init_mini_module(void)
{
    int ret = -1;
    ec_slave_config_t *sc;

    printk(KERN_INFO PFX "Starting...\n");

    master = ecrt_request_master(0);
    if (!master) {
        printk(KERN_ERR PFX "Requesting master 0 failed.\n");
        ret = -EBUSY;
        goto out_return;
    }

    sema_init(&master_sem, 1);
    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;
    }

    // Create configuration for bus coupler
    sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100);
    if (!sc) {
        printk(KERN_ERR PFX "Failed to create slave config.\n");
        ret = -ENOMEM;
        goto out_release_master;
    }

    create_serial_devices(master, domain1);

    printk(KERN_INFO PFX "Activating master...\n");
    if (ecrt_master_activate(master)) {
        printk(KERN_ERR PFX "Failed to activate master!\n");
        goto out_free_serial;
    }

    // Get internal process data for domain
    domain1_pd = ecrt_domain_data(domain1);

    printk(KERN_INFO PFX "Starting cyclic sample thread.\n");
    init_timer(&timer);
    timer.function = cyclic_task;
    timer.expires = jiffies + 10;
    add_timer(&timer);

    printk(KERN_INFO PFX "Started.\n");
    return 0;

out_free_serial:
    free_serial_devices();
out_release_master:
    printk(KERN_ERR PFX "Releasing master...\n");
    ecrt_release_master(master);
out_return:
    printk(KERN_ERR PFX "Failed to load. Aborting.\n");
    return ret;
}
Exemplo n.º 2
0
int EtherCatServer::configServer(std::string configFile) {


  // 1. Configure the system

  master = ecrt_request_master(0);
   if (!master)
       return -1;


   // load up the configuration

   slaves = configLoader.loadConfiguration(master, configFile);

   domain1 = ecrt_master_create_domain(master);
   if (!domain1)
       return -1;

  // load and apply configurations
  configLoader.applyConfiguration(master, domain1, &slaves);

  printf("Activating master...\n");
  if (ecrt_master_activate(master))
      return -1;

  if (!(domain1_pd = ecrt_domain_data(domain1))) {
      return -1;
  }

  return 0;

};
Exemplo n.º 3
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, 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;
}
Exemplo n.º 4
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;
}
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;
}
Exemplo n.º 6
0
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;	
}
Exemplo n.º 7
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");

}
Exemplo n.º 8
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;
}
Exemplo n.º 9
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;
}
Exemplo n.º 10
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;
    }

    printk(KERN_INFO PFX "Configuring PDOs...\n");

    // create configuration for reference clock FIXME
    if (!(sc = ecrt_master_slave_config(master, 0, 0, Beckhoff_EK1100))) {
        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;

    // configure SYNC signals for this slave
    ecrt_slave_config_dc(sc, 0x0700, 1000000, 440000, 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);

    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;
}
Exemplo n.º 11
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;
}
Exemplo n.º 12
0
int uei_ethercat_initialize (void)
{

    uint8_t *data;

    ec_pdo_entry_reg_t rw_pdos[] = {
//		{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CI,  current_ci_off, NULL},
//		{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CP,  current_cp_off, NULL},
//    	{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_OFFSET,  current_offset_off, NULL},
    	{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_VAL,  current_val_off, NULL},
//    	{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_POSITION,  motor_pos_off, NULL},
    	{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATE,  state_off, NULL},
//    	{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATUS,  status_off, NULL},
//    	{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_TEMP,  drive_temp_off, NULL},
//    	{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_FAULT,  latched_fault_off, NULL},
//    	{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_STATUS,  latched_status_off, NULL},
//    	{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE,  motor_temp_v_off, NULL},
//    	{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS,  motor_enc_wrap_off, NULL},
    	{0, 0,      0x00,         0x00, 0x0, 0x0,           NULL, NULL}};
//    ec_pdo_entry_reg_t pv_pdos[] = {
//		{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_CI,  current_ci_off+1, NULL},
//		{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_CP,  current_cp_off+1, NULL},
//		{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_OFFSET,  current_offset_off+1, NULL},
//    	{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_VAL,  current_val_off+1, NULL},
//    	{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_STATUS,  status_off+1, NULL},
//    	{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_TEMP,  drive_temp_off+1, NULL},
//    	{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_LATCHED_DRIVE_FAULT,  latched_fault_off+1, NULL},
//    	{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_LATCHED_DRIVE_STATUS,  latched_status_off+1, NULL},
//    	{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_POSITION,  motor_pos_off+1, NULL},
//    	{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE,  motor_temp_v_off+1, NULL},
//    	{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS,  motor_enc_wrap_off+1, NULL},
//    	{PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_STATE,  state_off+1, NULL},
//    	{0, 0,      0x00,         0x00, 0x0, 0x0,           NULL, NULL}};
//    ec_pdo_entry_reg_t el_pdos[] = {
//		{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CI,  current_ci_off+2, NULL},
//		{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CP,  current_cp_off+2, NULL},
//		{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_OFFSET,  current_offset_off+2, NULL},
//    	{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_VAL,  current_val_off+2, NULL},
//    	{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATUS,  status_off+2, NULL},
//    	{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_TEMP,  drive_temp_off+2, NULL},
//    	{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_FAULT,  latched_fault_off+2, NULL},
//    	{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_STATUS,  latched_status_off+2, NULL},
//    	{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_POSITION,  motor_pos_off+2, NULL},
//    	{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE,  motor_temp_v_off+2, NULL},
//    	{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS,  motor_enc_wrap_off+2, NULL},
//    	{EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATE,  state_off+2, NULL},
//    	{0, 0,      0x00,         0x00, 0x0, 0x0,           NULL, NULL}};

    master = ecrt_request_master(0);
    if (!master){
        printf("Could not request master!\n");
        return -1;
    }

    domain = ecrt_master_create_domain(master);
    if (!domain) {
        printf("Could not create domain!\n");
        return -1;
    }

    printf("Created Domain\n");

    if (!(rx_controller = ecrt_master_slave_config(master,RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE))) {
        fprintf(stderr, "Failed to get slave configuration for Reaction Wheel controller!\n");
        return -1;
    }
//    if (!(pv_controller = ecrt_master_slave_config(master,PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE))) {
//        fprintf(stderr, "Failed to get slave configuration for Pivot Motor controller!\n");
//        return -1;
//    }
//    if (!(el_controller = ecrt_master_slave_config(master,EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE))) {
//        fprintf(stderr, "Failed to get slave configuration for Elevation Motor controller!\n");
//        return -1;
//    }

	if (ecrt_slave_config_pdos(rx_controller, EC_END, copley_pdo_syncs)) {
		perror("ecrt_slave_config_pdos() failed for RX controller.");
		ecrt_release_master(master);
		return 3;
	}
//	if (ecrt_slave_config_pdos(pv_controller, 1, copley_pdo_syncs)) {
//		perror("ecrt_slave_config_pdos() failed for Pivot controller.");
//		ecrt_release_master(master);
//		return 3;
//	}
//	if (ecrt_slave_config_pdos(el_controller, 1, copley_pdo_syncs)) {
//		perror("ecrt_slave_config_pdos() failed for Elevation controller.");
//		ecrt_release_master(master);
//		return 3;
//	}

	/// Register the PDO list and variable mappings
//	if (ecrt_domain_reg_pdo_entry_list(domain, rw_pdos)) {
//		perror("ecrt_domain_reg_pdo_entry_list() failed for reaction wheel!");
//		ecrt_release_master(master);
//		return -1;
//	}
//	if (ecrt_domain_reg_pdo_entry_list(domain, pv_pdos)) {
//		perror("ecrt_domain_reg_pdo_entry_list() failed for pivot motor!");
//		ecrt_release_master(master);
//		return -1;
//	}
//	if (ecrt_domain_reg_pdo_entry_list(domain, el_pdos)) {
//		perror("ecrt_domain_reg_pdo_entry_list() failed for Elevation motor!");
//		ecrt_release_master(master);
//		return -1;
//	}
    state_off[0] = ecrt_slave_config_reg_pdo_entry(rx_controller,
                ECAT_DRIVE_STATE, domain, NULL);
    current_val_off[0] = ecrt_slave_config_reg_pdo_entry(rx_controller,
    		ECAT_CURRENT_LOOP_VAL, domain, NULL);
    // configure SYNC signals for this slave
	ecrt_slave_config_dc(rx_controller, 0, 1000000000ll / 100, 4400000, 0, 0);

    printf("Set Master/Slave Configuration\n");

    if (ecrt_master_activate(master) < 0) {
        printf("Could not activate master!\n");
        return -1;
    }

    if (!(data = ecrt_domain_data(domain))) {
    	perror("ecrt_domain_data() failed!");
    	ecrt_release_master(master);
    	return -1;
    }

    ethercat_set_offsets(&rx_controller_state, data, 0);
//    ethercat_set_offsets(&pv_controller_state, data, 1);
//    ethercat_set_offsets(&el_controller_state, data, 2);

    check_domain1_state();
    check_master_state();

    printf("Data: %p\t Current: %p\t State: %p\n", data, rx_controller_state.current_val, rx_controller_state.amp_state);
    ecrt_master_receive(master);
    ecrt_domain_process(domain);

    EC_WRITE_S16(data + current_val_off[0], 0);
    EC_WRITE_U16(data + state_off[0], ECAT_STATE_DISABLED);

    ecrt_domain_queue(domain);
    ecrt_master_send(master);
    check_domain1_state();
    check_master_state();
    return 0;
}
Exemplo n.º 13
0
Arquivo: main.c Projeto: Beckhoff/CCAT
int main(int argc, char **argv)
{
    ec_slave_config_t *sc;
    struct sigaction sa;
    struct itimerval tv;
    uint16_t i;

    master = ecrt_request_master(0);
    if (!master)
        return -1;

    domain1 = ecrt_master_create_domain(master);
    if (!domain1)
        return -1;

    printf("Configuring PDOs...\n");
    Init_EL100x(&off_dig_in[0], 0, Beckhoff_EL1004, el1004_syncs);
    Init_EL100x(&off_dig_in[1], 1, Beckhoff_EL1004, el1004_syncs);
    Init_EL100x(&off_dig_out[0], 2, Beckhoff_EL2004, el2004_syncs);
    Init_EL100x(&off_dig_out[1], 3, Beckhoff_EL2004, el2004_syncs);

    // Create configuration for bus coupler
    sc = ecrt_master_slave_config(master, BusCouplerPos, Beckhoff_EK1100);
    if (!sc)
        return -1;
    fprintf(stderr, "EK1100 configured.\n");

    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;
}
Exemplo n.º 14
0
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;
}