예제 #1
0
int write_sdo(ec_sdo_request_t *req, unsigned data)
{
	EC_WRITE_S32(ecrt_sdo_request_data(req), data&0xffffffff);

	switch (ecrt_sdo_request_state(req)) {
		case EC_REQUEST_UNUSED: // request was not used yet
			ecrt_sdo_request_write(req); // trigger first read
			break;
		case EC_REQUEST_BUSY:
			//fprintf(stderr, "SDO write still busy...\n");
			//logmsg(1, "SDO value written: \n",data );
			pause();
			break;
		case EC_REQUEST_SUCCESS:
			//logmsg(1, "SDO value written: 0x%X\n", data);
			pause();
			ecrt_sdo_request_read(req); // trigger next read
			return 1;
			break;
		case EC_REQUEST_ERROR:
			fprintf(stderr, "Failed to write SDO!\n");
			ecrt_sdo_request_write(req); // retry writing
			return 0;
			break;
	}
}
예제 #2
0
void disable_operation()
{
    bool isOperationDisabled=false;
//    while(!isOperationDisabled)
    for(int i=0;i<1;++i)
    {
        EC_WRITE_U8(ecrt_sdo_request_data(sdo_controlword_write), 0x0000);// p85, use state transition 9
        switch (ecrt_sdo_request_state(sdo_controlword_write)) {
            case EC_REQUEST_UNUSED: // request was not used yet
    //            ecrt_sdo_request_read(sdo_operation_mode_display); // trigger first read
    //            ecrt_sdo_request_write(sdo);
                break;
            case EC_REQUEST_BUSY:
                fprintf(stderr, "Request to Write,But Still busy...\n");
                break;
            case EC_REQUEST_SUCCESS:
                fprintf(stderr, "sdo_controlword_write write value 0x03");
                ecrt_sdo_request_write(sdo_controlword_write);
                break;
            case EC_REQUEST_ERROR:
                fprintf(stderr, "Failed to read SDO state!\n");
                break;
        }
        // read state ,need switch_on_disabled
        ecrt_sdo_request_read(sdo_statusword_read); // trigger read
        switch (ecrt_sdo_request_state(sdo_statusword_read)) {
            case EC_REQUEST_UNUSED: // request was not used yet
                ecrt_sdo_request_read(sdo_statusword_read); // trigger first read
    //            ecrt_sdo_request_write(sdo);
                break;
            case EC_REQUEST_BUSY:
                fprintf(stderr, "Still busy...\n");
                break;
            case EC_REQUEST_SUCCESS:
                fprintf(stderr, "statusword value: 0x%04X\n",
                        EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)));
                if(EC_READ_U8(ecrt_sdo_request_data(sdo_statusword_read)) & 0x004F ^ 0x0040 == 0) // p91
                {
                    isOperationDisabled = true;
                    printf("motor state is in switch_on_disabled,exit safely \n");
                }
                break;
            case EC_REQUEST_ERROR:
                fprintf(stderr, "Failed to read SDO!\n");
                break;
        }
        if(isOperationDisabled)
        {
            break;
        }
        else
        {
            printf("bad exit!\n");
        }
    }//while
}
예제 #3
0
/** Add a PDO entry.
 */
void ec_fsm_pdo_entry_conf_state_map_entry(
        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
        ec_datagram_t *datagram /**< Datagram to use. */
        )
{
    if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) {
        return;
    }

    if (!ec_fsm_coe_success(fsm->fsm_coe)) {
        EC_SLAVE_WARN(fsm->slave, "Failed to map PDO entry"
                " 0x%04X:%02X (%u bit) to position %u.\n",
                fsm->entry->index, fsm->entry->subindex,
                fsm->entry->bit_length, fsm->entry_pos);
        EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_entry_print(fsm);
        fsm->state = ec_fsm_pdo_entry_state_error;
        return;
    }

    // find next entry
    if (!(fsm->entry = ec_fsm_pdo_entry_conf_next_entry(
                    fsm, &fsm->entry->list))) {

        // No more entries to add. Write entry count.
        EC_WRITE_U8(fsm->request.data, fsm->entry_pos);
        fsm->request.data_size = 1;
        ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index, 0);
        ecrt_sdo_request_write(&fsm->request);

        EC_SLAVE_DBG(fsm->slave, 1, "Setting number of PDO entries to %u.\n",
                fsm->entry_pos);

        fsm->state = ec_fsm_pdo_entry_conf_state_set_entry_count;
        ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
        ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
        return;
    }

    // add next entry
    fsm->entry_pos++;
    ec_fsm_pdo_entry_conf_action_map(fsm, datagram);
}
예제 #4
0
int read_sdo(ec_sdo_request_t *req)
{
	int sdo_read_value;
    switch (ecrt_sdo_request_state(req)) {
        case EC_REQUEST_UNUSED: // request was not used yet
            ecrt_sdo_request_read(req); // trigger first read
            break;
        case EC_REQUEST_BUSY:
            //fprintf(stderr, "SDO still busy...\n");
            break;
        case EC_REQUEST_SUCCESS:
        	sdo_read_value = EC_READ_S32(ecrt_sdo_request_data(req));
            //logmsg(1, "SDO value read: 0x%X\n", sdo_read_value);
            ecrt_sdo_request_write(req); // trigger next write
            break;
        case EC_REQUEST_ERROR:
            //fprintf(stderr, "Failed to read SDO!\n");
            ecrt_sdo_request_read(req); // retry reading
            break;
    }
    return sdo_read_value;
}
예제 #5
0
/** Start PDO mapping.
 */
void ec_fsm_pdo_entry_conf_state_start(
        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
        ec_datagram_t *datagram /**< Datagram to use. */
        )
{
    if (ec_sdo_request_alloc(&fsm->request, 4)) {
        fsm->state = ec_fsm_pdo_entry_state_error;
        return;
    }

    // set mapped PDO entry count to zero
    EC_WRITE_U8(fsm->request.data, 0);
    fsm->request.data_size = 1;
    ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index, 0);
    ecrt_sdo_request_write(&fsm->request);

    EC_SLAVE_DBG(fsm->slave, 1, "Setting entry count to zero.\n");

    fsm->state = ec_fsm_pdo_entry_conf_state_zero_entry_count;
    ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
    ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
예제 #6
0
/** Starts to add a PDO entry.
 */
void ec_fsm_pdo_entry_conf_action_map(
        ec_fsm_pdo_entry_t *fsm, /**< PDO mapping state machine. */
        ec_datagram_t *datagram /**< Datagram to use. */
        )
{
    uint32_t value;

    EC_SLAVE_DBG(fsm->slave, 1, "Mapping PDO entry 0x%04X:%02X (%u bit)"
            " at position %u.\n",
            fsm->entry->index, fsm->entry->subindex,
            fsm->entry->bit_length, fsm->entry_pos);

    value = fsm->entry->index << 16
        | fsm->entry->subindex << 8 | fsm->entry->bit_length;
    EC_WRITE_U32(fsm->request.data, value);
    fsm->request.data_size = 4;
    ecrt_sdo_request_index(&fsm->request, fsm->source_pdo->index,
            fsm->entry_pos);
    ecrt_sdo_request_write(&fsm->request);

    fsm->state = ec_fsm_pdo_entry_conf_state_map_entry;
    ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request);
    ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately
}
예제 #7
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;
}
예제 #8
0
void el60xx_port_run(el60xx_port_t *port, u8 *pd)
{
    u16 status = EC_READ_U16(pd + port->off_status);
    u8 *rx_data = pd + port->off_rx;
    uint8_t tx_accepted_toggle, rx_request_toggle;

    switch (port->state) {
        case SER_READY:

            /* Check, if hardware handshaking has to be configured. */
            if (!port->config_error &&
                    port->requested_rtscts != port->current_rtscts) {
                EC_WRITE_U8(ecrt_sdo_request_data(port->rtscts_sdo),
                        port->requested_rtscts);
                ecrt_sdo_request_write(port->rtscts_sdo);
                port->state = SER_SET_RTSCTS;
                break;
            }

            /* Check, if the baud rate has to be configured. */
            if (!port->config_error &&
                    port->requested_baud_rate != port->current_baud_rate) {
                EC_WRITE_U8(ecrt_sdo_request_data(port->baud_sdo),
                        port->requested_baud_rate);
                ecrt_sdo_request_write(port->baud_sdo);
                port->state = SER_SET_BAUD_RATE;
                break;
            }

            /* Check, if the data frame has to be configured. */
            if (!port->config_error &&
                    port->requested_data_frame != port->current_data_frame) {
                EC_WRITE_U8(ecrt_sdo_request_data(port->frame_sdo),
                        port->requested_data_frame);
                ecrt_sdo_request_write(port->frame_sdo);
                port->state = SER_SET_DATA_FRAME;
                break;
            }

            /* Send data */

            tx_accepted_toggle = status & 0x0001;
            if (tx_accepted_toggle != port->tx_accepted_toggle) { // ready
                port->tx_data_size =
                    ectty_tx_data(port->tty, port->tx_data, port->max_tx_data_size);
                if (port->tx_data_size) {
#if DEBUG
                    printk(KERN_INFO PFX "%s: Sending %u bytes.\n",
                            port->name, port->tx_data_size);
#endif
                    port->tx_request_toggle = !port->tx_request_toggle;
                    port->tx_accepted_toggle = tx_accepted_toggle;
                }
            }

            /* Receive data */

            rx_request_toggle = status & 0x0002;
            if (rx_request_toggle != port->rx_request_toggle) {
                uint8_t rx_data_size = status >> 8;
                port->rx_request_toggle = rx_request_toggle;
#if DEBUG
                printk(KERN_INFO PFX "%s: Received %u bytes.\n",
                        port->name, rx_data_size);
#endif
                ectty_rx_data(port->tty, rx_data, rx_data_size);
                port->rx_accepted_toggle = !port->rx_accepted_toggle;
            }

            port->control =
                port->tx_request_toggle |
                port->rx_accepted_toggle << 1 |
                port->tx_data_size << 8;
            break;

        case SER_REQUEST_INIT:
            if (status & (1 << 2)) {
                port->control = 0x0000;
                port->state = SER_WAIT_FOR_INIT_RESPONSE;
            } else {
                port->control = 1 << 2; // CW.2, request initialization
            }
            break;

        case SER_WAIT_FOR_INIT_RESPONSE:
            if (!(status & (1 << 2))) {
                printk(KERN_INFO PFX "%s: Init successful.\n", port->name);
                port->tx_accepted_toggle = 1;
                port->control = 0x0000;
                port->state = SER_READY;
            }
            break;

        case SER_SET_RTSCTS:
            switch (ecrt_sdo_request_state(port->rtscts_sdo)) {
                case EC_REQUEST_SUCCESS:
                    printk(KERN_INFO PFX "%s: Accepted RTS/CTS.\n",
                            port->name);
                    port->current_rtscts = port->requested_rtscts;
                    port->state = SER_REQUEST_INIT;
                    break;
                case EC_REQUEST_ERROR:
                    printk(KERN_ERR PFX "Failed to set RTS/CTS on %s!\n",
                            port->name);
                    port->state = SER_REQUEST_INIT;
                    port->config_error = 1;
                    break;
                default:
                    break;
            }
            break;

        case SER_SET_BAUD_RATE:
            switch (ecrt_sdo_request_state(port->baud_sdo)) {
                case EC_REQUEST_SUCCESS:
                    printk(KERN_INFO PFX "%s: Accepted baud rate.\n",
                            port->name);
                    port->current_baud_rate = port->requested_baud_rate;
                    port->state = SER_REQUEST_INIT;
                    break;
                case EC_REQUEST_ERROR:
                    printk(KERN_ERR PFX "Failed to set baud rate on %s!\n",
                            port->name);
                    port->state = SER_REQUEST_INIT;
                    port->config_error = 1;
                    break;
                default:
                    break;
            }
            break;

        case SER_SET_DATA_FRAME:
            switch (ecrt_sdo_request_state(port->frame_sdo)) {
                case EC_REQUEST_SUCCESS:
                    printk(KERN_INFO PFX "%s: Accepted data frame.\n",
                            port->name);
                    port->current_data_frame = port->requested_data_frame;
                    port->state = SER_REQUEST_INIT;
                    break;
                case EC_REQUEST_ERROR:
                    printk(KERN_ERR PFX "Failed to set data frame on %s!\n",
                            port->name);
                    port->state = SER_REQUEST_INIT;
                    port->config_error = 1;
                    break;
                default:
                    break;
            }
            break;
    }