Exemplo n.º 1
0
void cyclic_task()
{
    // receive process data
    ecrt_master_receive(master);
    ecrt_domain_process(domain1);

    // check process data state (optional)
    check_domain1_state();

    if (counter) {
        counter--;
    } else { // do this at 1 Hz
        counter = FREQUENCY;

        // calculate new process data
        blink = !blink;

        // check for master state (optional)
        check_master_state();

        // check for islave configuration state(s) (optional)
        check_slave_config_states();

#if SDO_ACCESS
        // read process data SDO
        read_sdo();
#endif

    }

#if 0
    // read process data
    printf("AnaIn: state %u value %u\n",
           EC_READ_U8(domain1_pd + off_ana_in_status),
           EC_READ_U16(domain1_pd + off_ana_in_value));
#endif

#if 1
    // write process data
    EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
#endif

    // send process data
    ecrt_domain_queue(domain1);
    ecrt_master_send(master);
}
Exemplo n.º 2
0
void 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
        read_sdo();

    // send process data
    ecrt_domain_queue(domain1);
    ecrt_master_send(master);
}
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, 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;
}
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;
    }
//    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.º 5
0
void cyclic_task()
{
	/* sync the dc clock of the slaves */
	ecrt_master_sync_slave_clocks(master);

	// receive process data
	ecrt_master_receive(master);
	ecrt_domain_process(domain1);

	// check process data state (optional)
	check_domain1_state();

	if (counter) {
		counter--;
	} else { // do this at 1 Hz
		counter = FREQUENCY;

		// calculate new process data
		blink = !blink;

		// check for master state (optional)
		check_master_state();

		// check for islave configuration state(s) (optional)
		check_slave_config_states();

#if SDO_ACCESS
		// read process data SDO
		read_sdo(sdo);
		read_sdo(request[0]);
		read_sdo(request[1]);
		read_sdo(request[2]);

		write_sdo(sdo_download_requests[0], sdoexample); /* SDO download value to the node */
#endif
	}

	/* Read process data */
	unsigned int sn_status = EC_READ_U16(domain1_pd + off_pdo1_in);
	unsigned int sn_modes = EC_READ_U8(domain1_pd + off_pdo2_in);
	unsigned int sn_position = EC_READ_U32(domain1_pd + off_pdo3_in);
	unsigned int sn_velocity = EC_READ_U32(domain1_pd + off_pdo4_in);
	unsigned int sn_torque = EC_READ_U16(domain1_pd + off_pdo5_in);

	logmsg(2, "[REC] 0x%4x 0x%4x 0x%8x 0x%8x 0x%4x\n",
			sn_status, sn_modes,
			sn_position, sn_velocity, sn_torque);

#if 0
    // read process data
    printf("AnaIn: state %u value %u\n",
            EC_READ_U8(domain1_pd + off_ana_in_status),
            EC_READ_U16(domain1_pd + off_ana_in_value));
#endif

    // write process data
    //EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
    
#ifdef CIA402
#define STATUSW1   0x88AA
#define STATUSW2   0xAA88
#define OPMODES1   0xf1
#define OPMODES2   0x1f
#define TORVAL1    0xabab
#define TORVAL2    0xbaba
#define VELVAL1    0x2d2d4d4d
#define VELVAL2    0xd4d4d2d2
#define POSVAL1    0xe4e4e2e2
#define POSVAL2    0x2e2e4e4e

	EC_WRITE_U16(domain1_pd + off_pdo1_out, (blink ? STATUSW1 : STATUSW2)&0xffff);
	EC_WRITE_U8(domain1_pd + off_pdo2_out, (blink ? OPMODES1 : OPMODES2)&0xff);
	EC_WRITE_U16(domain1_pd + off_pdo3_out, (blink ? TORVAL1 : TORVAL2)&0xffff);
	EC_WRITE_U32(domain1_pd + off_pdo4_out, blink ? POSVAL1 : POSVAL2);
	EC_WRITE_U32(domain1_pd + off_pdo5_out, blink ? VELVAL1 : VELVAL2);
#else
#define TESTWORD1   0xdead
#define TESTWORD2   0xbeef
#define TESTWORD3   0xfefe
#define TESTWORD4   0xa5a5

	EC_WRITE_U16(domain1_pd + off_pdo1_out, blink ? TESTWORD1 : TESTWORD2);
	EC_WRITE_U16(domain1_pd + off_pdo2_out, blink ? TESTWORD3 : TESTWORD4);
#endif

	// send process data
	ecrt_domain_queue(domain1);
	ecrt_master_send(master);
	//printf("Wrote %x to slave\n",  blink ? TESTWORD1 : TESTWORD2);
}
Exemplo n.º 6
0
void cyclic_task()
{
    // receive process data
    ecrt_master_receive(master);
    ecrt_domain_process(domain_output);
    ecrt_domain_process(domain_input);


    // check process data state (optional)
    check_domain1_state();

    if (counter) {
        counter--;
    } else { // do this at 1 Hz

        counter = FREQUENCY;

        // check for master state (optional)
        check_master_state();

        // check for islave configuration state(s) (optional)
        check_slave_config_states();


    // read process data
//        {0x6040, 0x00, 16}, /* Controlword */
//        {0x6060, 0x00, 8}, /* Mode_of_Operation */
//        {0x6098, 0x00, 8}, /* Homing_Method */
//        {0x607a, 0x00, 32}, /* Target_Position */
//        {0x60ff, 0x00, 32}, /* Target_Velocity */
//        {0x6071, 0x00, 16}, /* Target_Torque */
//        {0x6041, 0x00, 16}, /* Statusword */
//        {0x6064, 0x00, 32}, /* Position_Actual_Value */
//        {0x6061, 0x00, 8}, /* Modes_Of_Operation_Display */
//        {0x1001, 0x00, 8}, /* Error_Register */
//        {0x606c, 0x00, 32}, /* Velocity_Actual_Value */
//        {0x6077, 0x00, 16}, /* Torque_Actual_Value */
//        printf("pdo value: %02x offset %u\n",
//                EC_READ_U16(domain1_pd + off_0x6040),off_0x6040);
//        printf("pdo value: %02x offset %u\n",
//                EC_READ_U16(domain1_pd + off_0x1001),off_0x1001);
    printf("pdo value: %04x offset %u\n",
            EC_READ_U16(domain_input_pd + off_0x6041),off_0x6041);
    printf("pdo value 6061asfsadf: %04x offset %u\n",
            EC_READ_U8(domain_input_pd + off_0x6061),off_0x6061);
    printf("pd: %u \n",*domain_input_pd);
//            EC_READ_U8(domain1_pd + off_ana_in_value));

#if SDO_ACCESS
        // read process data SDO
        read_sdo();
#endif


    }


    // send process data
    ecrt_domain_queue(domain_output);
    ecrt_domain_queue(domain_input);
    ecrt_master_send(master);
#if 0
    // write process data
//    EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
#endif


}