Esempio n. 1
0
/** Initializes a sync manager configuration page.
 *
 * The referenced memory (\a data) must be at least \a EC_SYNC_SIZE bytes.
 */
void ec_sync_page(
        const ec_sync_t *sync, /**< Sync manager. */
        uint8_t sync_index, /**< Index of the sync manager. */
        uint16_t data_size, /**< Data size. */
        const ec_sync_config_t *sync_config, /**< Configuration. */
        uint8_t pdo_xfer, /**< Non-zero, if PDOs will be transferred via this
                            sync manager. */
        uint8_t *data /**> Configuration memory. */
        )
{
    // enable only if (SII enable is set or PDO xfer)
    // and size is > 0 and SM is not virtual
    uint16_t enable = ((sync->enable & 0x01) || pdo_xfer)
                        && data_size
                        && ((sync->enable & 0x04) == 0);
    uint8_t control = sync->control_register;

    if (sync_config) {

        switch (sync_config->dir) {
            case EC_DIR_OUTPUT:
            case EC_DIR_INPUT:
                EC_WRITE_BIT(&control, 2,
                        sync_config->dir == EC_DIR_OUTPUT ? 1 : 0);
                EC_WRITE_BIT(&control, 3, 0);
                break;
            default:
                break;
        }

        switch (sync_config->watchdog_mode) {
            case EC_WD_ENABLE:
            case EC_WD_DISABLE:
                EC_WRITE_BIT(&control, 6,
                        sync_config->watchdog_mode == EC_WD_ENABLE);
                break;
            default:
                break;
        }
    }

    EC_SLAVE_DBG(sync->slave, 1, "SM%u: Addr 0x%04X, Size %3u,"
            " Ctrl 0x%02X, En %u\n",
            sync_index, sync->physical_start_address,
            data_size, control, enable);

    EC_WRITE_U16(data,     sync->physical_start_address);
    EC_WRITE_U16(data + 2, data_size);
    EC_WRITE_U8 (data + 4, control);
    EC_WRITE_U8 (data + 5, 0x00); // status byte (read only)
    EC_WRITE_U16(data + 6, enable);
}
Esempio n. 2
0
/** Sends a file or the next fragment.
 *
 * \return Zero on success, otherwise a negative error code.
 */
int ec_foe_prepare_data_send(
        ec_fsm_foe_t *fsm, /**< Finite state machine. */
        ec_datagram_t *datagram /**< Datagram to use. */
        )
{
    size_t remaining_size, current_size;
    uint8_t *data;

    remaining_size = fsm->tx_buffer_size - fsm->tx_buffer_offset;

    if (remaining_size < fsm->slave->configured_tx_mailbox_size
            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE) {
        current_size = remaining_size;
        fsm->tx_last_packet = 1;
    } else {
        current_size = fsm->slave->configured_tx_mailbox_size
            - EC_MBOX_HEADER_SIZE - EC_FOE_HEADER_SIZE;
    }

    data = ec_slave_mbox_prepare_send(fsm->slave,
            datagram, EC_MBOX_TYPE_FILEACCESS,
            current_size + EC_FOE_HEADER_SIZE);
    if (IS_ERR(data)) {
        return -1;
    }

    EC_WRITE_U16(data, EC_FOE_OPCODE_DATA);    // OpCode = DataBlock req.
    EC_WRITE_U32(data + 2, fsm->tx_packet_no); // PacketNo, Password

    memcpy(data + EC_FOE_HEADER_SIZE,
            fsm->tx_buffer + fsm->tx_buffer_offset, current_size);
    fsm->tx_current_size = current_size;

    return 0;
}
Esempio n. 3
0
void ec_fsm_sii_state_start_reading(
        ec_fsm_sii_t *fsm /**< finite state machine */
        )
{
    ec_datagram_t *datagram = fsm->datagram;

    // initiate read operation
    switch (fsm->mode) {
        case EC_FSM_SII_USE_INCREMENT_ADDRESS:
            ec_datagram_apwr(datagram, fsm->slave->ring_position, 0x502, 4);
            break;
        case EC_FSM_SII_USE_CONFIGURED_ADDRESS:
            ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 4);
            break;
    }

    EC_WRITE_U8 (datagram->data,     0x80); // two address octets
    EC_WRITE_U8 (datagram->data + 1, 0x01); // request read operation
    EC_WRITE_U16(datagram->data + 2, fsm->word_offset);

#ifdef SII_DEBUG
    EC_SLAVE_DBG(fsm->slave, 0, "reading SII data, word %u:\n",
            fsm->word_offset);
    ec_print_data(datagram->data, 4);
#endif

    fsm->retries = EC_FSM_RETRIES;
    fsm->state = ec_fsm_sii_state_read_check;
}
Esempio n. 4
0
/** Prepare a write request (WRQ) with filename
 *
 * \return Zero on success, otherwise a negative error code.
 */
int ec_foe_prepare_wrq_send(
        ec_fsm_foe_t *fsm, /**< Finite state machine. */
        ec_datagram_t *datagram /**< Datagram to use. */
        )
{
    size_t current_size;
    uint8_t *data;

    fsm->tx_buffer_offset = 0;
    fsm->tx_current_size = 0;
    fsm->tx_packet_no = 0;
    fsm->tx_last_packet = 0;

    current_size = fsm->tx_filename_len;

    data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
            EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
    if (IS_ERR(data)) {
        return -1;
    }

    EC_WRITE_U16( data, EC_FOE_OPCODE_WRQ); // fsm write request
    EC_WRITE_U32( data + 2, fsm->tx_packet_no );

    memcpy(data + EC_FOE_HEADER_SIZE, fsm->tx_filename, current_size);

    return 0;
}
Esempio n. 5
0
/** Prepare a read request (RRQ) with filename
 *
 * \return Zero on success, otherwise a negative error code.
 */
int ec_foe_prepare_rrq_send(
        ec_fsm_foe_t *fsm, /**< Finite state machine. */
        ec_datagram_t *datagram /**< Datagram to use. */
        )
{
    size_t current_size;
    uint8_t *data;

    current_size = fsm->rx_filename_len;

    data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
            EC_MBOX_TYPE_FILEACCESS, current_size + EC_FOE_HEADER_SIZE);
    if (IS_ERR(data)) {
        return -1;
    }

    EC_WRITE_U16(data, EC_FOE_OPCODE_RRQ); // fsm read request
    EC_WRITE_U32(data + 2, 0x00000000); // no passwd
    memcpy(data + EC_FOE_HEADER_SIZE, fsm->rx_filename, current_size);

    if (fsm->slave->master->debug_level) {
        EC_SLAVE_DBG(fsm->slave, 1, "FoE Read Request:\n");
        ec_print_data(data, current_size + EC_FOE_HEADER_SIZE);
    }

    return 0;
}
Esempio n. 6
0
/** Initializes an FMMU configuration page.
 *
 * The referenced memory (\a data) must be at least EC_FMMU_PAGE_SIZE bytes.
 */
void ec_fmmu_config_page(
        const ec_fmmu_config_t *fmmu, /**< EtherCAT FMMU configuration. */
        const ec_sync_t *sync, /**< Sync manager. */
        uint8_t *data /**> Configuration page memory. */
        )
{
    EC_CONFIG_DBG(fmmu->sc, 1, "FMMU: LogAddr 0x%08X, Size %3u,"
            " PhysAddr 0x%04X, SM%u, Dir %s\n",
            fmmu->logical_start_address, fmmu->data_size,
            sync->physical_start_address, fmmu->sync_index,
            fmmu->dir == EC_DIR_INPUT ? "in" : "out");

    EC_WRITE_U32(data,      fmmu->logical_start_address);
    EC_WRITE_U16(data + 4,  fmmu->data_size); // size of fmmu
    EC_WRITE_U8 (data + 6,  0x00); // logical start bit
    EC_WRITE_U8 (data + 7,  0x07); // logical end bit
    EC_WRITE_U16(data + 8,  sync->physical_start_address);
    EC_WRITE_U8 (data + 10, 0x00); // physical start bit
    EC_WRITE_U8 (data + 11, fmmu->dir == EC_DIR_INPUT ? 0x01 : 0x02);
    EC_WRITE_U16(data + 12, 0x0001); // enable
    EC_WRITE_U16(data + 14, 0x0000); // reserved
}
Esempio n. 7
0
uint8_t *ec_slave_mbox_prepare_send(const ec_slave_t *slave, /**< slave */
                                    ec_datagram_t *datagram, /**< datagram */
                                    uint8_t type, /**< mailbox protocol */
                                    size_t size /**< size of the data */
                                    )
{
    size_t total_size;
    int ret;

    if (unlikely(!slave->sii.mailbox_protocols)) {
        EC_SLAVE_ERR(slave, "Slave does not support mailbox"
                " communication!\n");
        return ERR_PTR(-EPROTONOSUPPORT);
    }

    total_size = EC_MBOX_HEADER_SIZE + size;

    if (unlikely(total_size > slave->configured_rx_mailbox_size)) {
        EC_SLAVE_ERR(slave, "Data size (%zu) does not fit in mailbox (%u)!\n",
                total_size, slave->configured_rx_mailbox_size);
        return ERR_PTR(-EOVERFLOW);
    }

    ret = ec_datagram_fpwr(datagram, slave->station_address,
            slave->configured_rx_mailbox_offset,
            slave->configured_rx_mailbox_size);
    if (ret)
        return ERR_PTR(ret);

    EC_WRITE_U16(datagram->data,     size); // mailbox service data length
    EC_WRITE_U16(datagram->data + 2, slave->station_address); // station addr.
    EC_WRITE_U8 (datagram->data + 4, 0x00); // channel & priority
    EC_WRITE_U8 (datagram->data + 5, type); // underlying protocol type

    return datagram->data + EC_MBOX_HEADER_SIZE;
}
Esempio n. 8
0
void uei_ethercat_cleanup(void)
{
    ecrt_master_receive(master);
    ecrt_domain_process(domain);
    EC_WRITE_S16(rx_controller_state.current_val, 0);
    EC_WRITE_U16(rx_controller_state.amp_state, ECAT_STATE_DISABLED);

//    EC_WRITE_S16(pv_controller_state.current_val, 0);
//    EC_WRITE_U16(pv_controller_state.amp_state, ECAT_STATE_DISABLED);
//
//    EC_WRITE_S16(el_controller_state.current_val, 0);
//    EC_WRITE_U16(el_controller_state.amp_state, ECAT_STATE_DISABLED);

    ecrt_domain_queue(domain);
    ecrt_master_send(master);

    ecrt_release_master(master);
}
Esempio n. 9
0
/** Prepare to send an acknowledge.
 *
 * \return Zero on success, otherwise a negative error code.
 */
int ec_foe_prepare_send_ack(
        ec_fsm_foe_t *fsm, /**< FoE statemachine. */
        ec_datagram_t *datagram /**< Datagram to use. */
        )
{
    uint8_t *data;

    data = ec_slave_mbox_prepare_send(fsm->slave, datagram,
            EC_MBOX_TYPE_FILEACCESS, EC_FOE_HEADER_SIZE);
    if (IS_ERR(data)) {
        return -1;
    }

    EC_WRITE_U16(data, EC_FOE_OPCODE_ACK);
    EC_WRITE_U32(data + 2, fsm->rx_expected_packet_no);

    return 0;
}
Esempio n. 10
0
void ec_fsm_sii_state_start_writing(
        ec_fsm_sii_t *fsm /**< finite state machine */
        )
{
    ec_datagram_t *datagram = fsm->datagram;

    // initiate write operation
    ec_datagram_fpwr(datagram, fsm->slave->station_address, 0x502, 8);
    EC_WRITE_U8 (datagram->data,     0x81); /* two address octets
                                               + enable write access */
    EC_WRITE_U8 (datagram->data + 1, 0x02); // request write operation
    EC_WRITE_U16(datagram->data + 2, fsm->word_offset);
    memset(datagram->data + 4, 0x00, 2);
    memcpy(datagram->data + 6, fsm->value, 2);

#ifdef SII_DEBUG
    EC_SLAVE_DBG(fsm->slave, 0, "writing SII data:\n");
    ec_print_data(datagram->data, 8);
#endif

    fsm->retries = EC_FSM_RETRIES;
    fsm->state = ec_fsm_sii_state_write_check;
}
Esempio n. 11
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;
}
Esempio n. 12
0
/** Prepare a set IP parameters operation.
 *
 * \return 0 on success, otherwise a negative error code.
 */
int ec_fsm_eoe_prepare_set(
        ec_fsm_eoe_t *fsm, /**< finite state machine */
        ec_datagram_t *datagram /**< Datagram to use. */
        )
{
    uint8_t *data, *cur;
    ec_slave_t *slave = fsm->slave;
    ec_master_t *master = slave->master;
    ec_eoe_request_t *req = fsm->request;
    size_t size = 8;

    if (req->mac_address_included) {
        size += ETH_ALEN;
    }

    if (req->ip_address_included) {
        size += 4;
    }

    if (req->subnet_mask_included) {
        size += 4;
    }

    if (req->gateway_included) {
        size += 4;
    }

    if (req->dns_included) {
        size += 4;
    }

    if (req->name_included) {
        size += EC_MAX_HOSTNAME_SIZE;
    }

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_EOE,
            size);
    if (IS_ERR(data)) {
        return PTR_ERR(data);
    }

    EC_WRITE_U8(data, EC_EOE_FRAMETYPE_SET_IP_REQ); // Set IP parameter req.
    EC_WRITE_U8(data + 1, 0x01); // last fragment, no timestamps
    EC_WRITE_U16(data + 2, 0x0000); // fragment no., offset, frame no.

    EC_WRITE_U32(data + 4,
            ((req->mac_address_included != 0) << 0) |
            ((req->ip_address_included != 0) << 1) |
            ((req->subnet_mask_included != 0) << 2) |
            ((req->gateway_included != 0) << 3) |
            ((req->dns_included != 0) << 4) |
            ((req->name_included != 0) << 5)
            );

    cur = data + 8;

    if (req->mac_address_included) {
        memcpy(cur, req->mac_address, ETH_ALEN);
        cur += ETH_ALEN;
    }

    if (req->ip_address_included) {
        memcpy(cur, &req->ip_address, 4);
        cur += 4;
    }

    if (req->subnet_mask_included) {
        memcpy(cur, &req->subnet_mask, 4);
        cur += 4;
    }

    if (req->gateway_included) {
        memcpy(cur, &req->gateway, 4);
        cur += 4;
    }

    if (req->dns_included) {
        memcpy(cur, &req->dns, 4);
        cur += 4;
    }

    if (req->name_included) {
        memcpy(cur, req->name, EC_MAX_HOSTNAME_SIZE);
        cur += EC_MAX_HOSTNAME_SIZE;
    }

    if (master->debug_level) {
        EC_SLAVE_DBG(slave, 0, "Set IP parameter request:\n");
        ec_print_data(data, cur - data);
    }

    fsm->request->jiffies_sent = jiffies;

    return 0;
}
void run(long data)
{
    int i;
    struct timeval tv;
    unsigned int sync_ref_counter = 0;

    count2timeval(nano2count(rt_get_real_time_ns()), &tv);


//    while (deactive!=20) {
    while (1) {
        t_last_cycle = get_cycles();



        // receive process data
        rt_sem_wait(&master_sem);
        ecrt_master_receive(master);
        ecrt_domain_process(domain1);
        rt_sem_signal(&master_sem);

        // check process data state (optional)
        //check_domain1_state();
				
		inpu[0]=EC_READ_U16(domain1_pd + status_word);
		inpu[1]=EC_READ_U32(domain1_pd + pos_act);	
		
//		if(servooff==1){//servo off
	//	if(stop==1){
			
	//		if( ( inpu[0] == 0x1637 )  &&  ( inpu[2] == 0x1637 ) ){
	//		EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
          //  		EC_WRITE_U16(domain1_pd+ctrl_word2, 0x0006 );
	//		}
	//		else if( ( inpu[0] == 0x0650 )  &&  ( inpu[2] == 0x0650 ) ){
	//		printk(KERN_INFO PFX "want to PREOP");
	//		deactive++;
	//		}
		
	//	}	

		if( (inpu[0]&0x004f) == 0x0040 ){
		EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
		}
		else if( (inpu[0]&0x006f) == 0x0021){
		EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 );
		}
		else if( (inpu[0]&0x006f) == 0x0023){
		EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f);
		EC_WRITE_S32(domain1_pd+tar_pos, 0);
		EC_WRITE_S32(domain1_pd+max_torq, 0xf00);
		}
		else if( (inpu[0]&0x006f) == 0x0027){
                                EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f);
				EC_WRITE_S32(domain1_pd+tar_pos      , value );            //for mode 8 no sin
					
					if(value==180000){
					speedup=0;
					speeddown=1;
					//printk(KERN_INFO PFX "top");					
					value=value-1;
					}
					else if(speeddown==1 && value!=0){
					value=value-1;
					//printk(KERN_INFO PFX "slow down");
					}
					else if(speeddown==1 && value==0){
					speedup=0;
					speeddown=0;
				//	stop=1;
					//printk(KERN_INFO PFX "stop");
					}
					else if(!stop){
					speedup=1;
					speeddown=0;
					value=value+1;
					//printk(KERN_INFO PFX "fast up ");

					}
				
				
//				change++;
				
//			}
//			else
//			change = 0;
		}
		
		
        rt_sem_wait(&master_sem);

        tv.tv_usec += 1000;
        if (tv.tv_usec >= 1000000)  {
            tv.tv_usec -= 1000000;
            tv.tv_sec++;
        }
        ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));

        if (sync_ref_counter) {
            sync_ref_counter--;
        } else {
            sync_ref_counter = 1; //original = 9
            ecrt_master_sync_reference_clock(master);
        }
		
        ecrt_master_sync_slave_clocks(master);
        ecrt_domain_queue(domain1);
        ecrt_master_send(master);
        rt_sem_signal(&master_sem);

        rt_task_wait_period();

	



    }
}
Esempio n. 14
0
void cyclic_task()
{
    struct timespec wakeupTime, time;
    // get current time
    clock_gettime(CLOCK_TO_USE, &wakeupTime);

	while(1) 
	{

		if(deactive==1)
		{
			break;
		}

		wakeupTime = timespec_add(wakeupTime, cycletime);
     		clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);

		
		// writter_receive(master);
 	   	ecrt_master_receive(master);
   		ecrt_domain_process(domain_r);
   		ecrt_domain_process(domain_w);

                temp[0]=EC_READ_U16(domain_w_pd + status_word);
                temp[1]=EC_READ_S32(domain_w_pd + mode_display);

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


		// write process data

                if(servo_flag==1)
		{
			//servo off
                	EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0006 );
                }

                else if( (temp[0]&0x004f) == 0x0040  )
		{
                	EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0006 );
                }

                else if( (temp[0]&0x006f) == 0x0021)
		{
                	EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0007 );
                }
                
		else if( (temp[0]&0x006f) == 0x0023)
		{
                	EC_WRITE_U16(domain_r_pd+ctrl_word, 0x000f );
                	EC_WRITE_S32(domain_r_pd+tar_pos,0);
                	EC_WRITE_S32(domain_r_pd+tar_vel, 0xffff);
                	EC_WRITE_S32(domain_r_pd+max_torq, 0xf00);

                }
		
		//operation enabled
                else if( (temp[0]&0x006f) == 0x0027)
		{
                        EC_WRITE_S32(domain_r_pd+tar_pos, (move_value+=2000) );
                        EC_WRITE_U16(domain_r_pd+ctrl_word, 0x001f);

                }

		clock_gettime(CLOCK_TO_USE, &time);
		ecrt_master_application_time(master, TIMESPEC2NS(time));

		if (sync_ref_counter) 
		{
			sync_ref_counter--;
		} 
		else 
		{
			sync_ref_counter = 1; // sync every cycle
			ecrt_master_sync_reference_clock(master);
		}

		ecrt_master_sync_slave_clocks(master);

		
		
		// send process data
		ecrt_domain_queue(domain_r);
		ecrt_domain_queue(domain_w);
		
		ecrt_master_send(master);

		

	}
}
void run(long data)
{
    int i;
    struct timeval tv;
    unsigned int sync_ref_counter = 0;

    count2timeval(nano2count(rt_get_real_time_ns()), &tv);

	
//    while (deactive!=20) {
    while (1) {
        t_last_cycle = get_cycles();
/*
	if ( (inpu[0]==0x0650) &&(stop==1)   )
	break;
*/



        // receive process data
        rt_sem_wait(&master_sem);
        ecrt_master_receive(master);
        ecrt_domain_process(domain1);
	ecrt_domain_process(domain2);
        rt_sem_signal(&master_sem);

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

		
		
		inpu[0]=EC_READ_U16(domain2_pd + status_word);
		inpu[1]=EC_READ_U32(domain2_pd + actual_pos);	
		/*
        if (counter) {
            counter--;
        } else {
            u32 c;

            counter = FREQUENCY;

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

            c = EC_READ_U32(domain1_pd + off_counter_in);
            if (counter_value != c) {
                counter_value = c;
                printk(KERN_INFO PFX "counter=%u\n", counter_value);
            }
        }
		*/
		
		/*
        if (blink_counter) {
            blink_counter--;
        } else {
            blink_counter = 9;

            // calculate new process data
            blink = !blink;
        }
		*/
		
		
        // write process data
		/*
        for (i = 0; i < NUM_DIG_OUT; i++) {
            EC_WRITE_U8(domain1_pd + off_dig_out[i], blink ? 0x66 : 0x99);
        }

        EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02);
		*/
		
		
		
		
		
		
		
		
//		if(servooff==1){//servo off
		if(stop==1){
			
			if ( inpu[0] == 0x1637 ) {
			EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
			}
			else if( inpu[0] == 0x0650  ){
			//++deactive ;
			//EC_WRITE_U16(domain1_pd+alstat, 0x0002 );
			printk(KERN_INFO PFX "want to PREOP");
			deactive++;
			}
			/*
			else{
			EC_WRITE_U16(domain1_pd+alstat, 0x0002 );			
			printk(KERN_INFO PFX "want to PREOP");
			break;
			}
			*/
		
		}	
		else if( (inpu[0]&0x0040) == 0x0040){
		EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
		}
		else if( (inpu[0]&0x006f) == 0x0021 ){
		EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 );
		}
		else if( (inpu[0]&0x027f) == 0x0233){
		EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f);
		EC_WRITE_S32(domain1_pd+interpolateddata, 0);
		//EC_WRITE_S32(domain1_pd+tar_velo, 0xffffff);
		EC_WRITE_S32(domain1_pd+max_torq, 0xf00);
		EC_WRITE_S32(domain1_pd+modeofoper, 8);
		
		}
		else if( (inpu[0]&0x027f) == 0x0237){
			//if(change >= 0 && change<2  ){
			if( change<1  ){	
				
				//start=1;

				//if(i==0){
				//EC_WRITE_S32(domain1_pd+interpolateddata	, 0 );
				//EC_WRITE_S32(domain1_pd+interpolateddata2	, 0 );
				//EC_WRITE_S32(domain1_pd+target_pos	, 0 );
				//EC_WRITE_S32(domain1_pd+target_pos2	, 0 );
				//EC_WRITE_S32(domain1_pd+tar_velo	, 0 );	
				//EC_WRITE_S32(domain1_pd+tar_velo2	, 0 );
				//}
				//else {
				//EC_WRITE_S32(domain1_pd+interpolateddata	, (sin(i)*180000) ); 	//for mode 7
				//EC_WRITE_S32(domain1_pd+interpolateddata2	, (sin(i)*180000) );
				//EC_WRITE_S32(domain1_pd+target_pos	, (sin(i)*180000) );		//for mode 8 with sin
				//EC_WRITE_S32(domain1_pd+target_pos2	, (sin(i)*180000) );
				EC_WRITE_S32(domain1_pd+target_pos      , inpu[7] );            //for mode 8 no sin
				//EC_WRITE_S32(domain1_pd+tar_velo	, 500000 );		//for mode 9
				//EC_WRITE_S32(domain1_pd+tar_velo2	, 500000 );
					//if(1){
					
					
					if(inpu[7]==1800000){
					speedup=0;
					speeddown=1;
					//printk(KERN_INFO PFX "top");					
					inpu[7]=inpu[7]-200;
					}
					else if(speeddown==1 && inpu[7]!=0){
					inpu[7]=inpu[7]-200;
					//printk(KERN_INFO PFX "slow down");
					}
					else if(speeddown==1 && inpu[7]==0){
					speedup=0;
					speeddown=0;
					stop=1;
					//printk(KERN_INFO PFX "stop");
					}
					else if(!stop){
					speedup=1;
					speeddown=0;
					inpu[7]=inpu[7]+2000;
					//printk(KERN_INFO PFX "fast up ");

					}
				
/*
					if(speedup==1)
						inpu[7]+=500;
					else if(speeddown==1)
						inpu[7]-=1000;
					else{
						inpu[7]=0;
						servooff=1;
					}
*/
					//EC_WRITE_S32(domain1_pd+tar_velo	, inpu[7] );		//for mode 9
					//EC_WRITE_S32(domain1_pd+tar_velo2	, inpu[7] );
					//}
					//else{
					//EC_WRITE_S32(domain1_pd+tar_velo	, inpu[7] );		//for mode 9
					//EC_WRITE_S32(domain1_pd+tar_velo2	, inpu[7] );
					//}
				
				//}
				
				EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f);
				change++;
				
				/*
				if(datacount<10001){
				data[datacount][0]=(sin(i)*360000);
				data[datacount][1]=inpu[1];
				data[datacount][2]=inpu[3];
				data[datacount][3]=(inpu[1] - inpu[3]);
				datacount++;
				}
				*/
			}
			else
			change = 0;
		}
		
		
		
		
		
		
		
	//printk(KERN_INFO PFX "pos1=%d   pos2=%d   inpu7=%d\n",inpu[1],inpu[3],inpu[7]);

		
		
		
        rt_sem_wait(&master_sem);

        tv.tv_usec += 1000;
        if (tv.tv_usec >= 1000000)  {
            tv.tv_usec -= 1000000;
            tv.tv_sec++;
        }
        ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));

        if (sync_ref_counter) {
            sync_ref_counter--;
        } else {
            sync_ref_counter = 1; //original = 9
            ecrt_master_sync_reference_clock(master);
        }
		
        ecrt_master_sync_slave_clocks(master);
        ecrt_domain_queue(domain1);
	ecrt_domain_queue(domain2);
        ecrt_master_send(master);
        rt_sem_signal(&master_sem);

        rt_task_wait_period();

	



    }
}
Esempio n. 16
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;
}
Esempio n. 17
0
void pdo_handle_ecat(master_setup_variables_t *master_setup,
        ctrlproto_slv_handle *slv_handles,
        unsigned int total_no_of_slaves)
{
	int slv;

	if(sig_alarms == user_alarms) pause();
	while (sig_alarms != user_alarms)
	{
		/* sync the dc clock of the slaves */
		//	ecrt_master_sync_slave_clocks(master);

		// receive process data
		ecrt_master_receive(master_setup->master);
		ecrt_domain_process(master_setup->domain);

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

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

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


		for(slv=0;slv<total_no_of_slaves;++slv)
		{
			/* Read process data */
			slv_handles[slv].motorctrl_status_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[0]);
			slv_handles[slv].operation_mode_disp = EC_READ_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[1]);
			slv_handles[slv].position_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[2]);
			slv_handles[slv].speed_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[3]);
			slv_handles[slv].torque_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[4]);
		}

/*		printf("\n%x", 	slv_handles[slv].motorctrl_status_in);
		printf("\n%x",  slv_handles[slv].operation_mode_disp);
		printf("\n%x",  slv_handles[slv].position_in);
		printf("\n%x",  slv_handles[slv].speed_in);
		printf("\n%x",  slv_handles[slv].torque_in);
*/

		for(slv=0;slv<total_no_of_slaves;++slv)
		{
			EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[0], (slv_handles[slv].motorctrl_out)&0xffff);
			EC_WRITE_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[1], (slv_handles[slv].operation_mode)&0xff);
			EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[2], (slv_handles[slv].torque_setpoint)&0xffff);
			EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[3], slv_handles[slv].position_setpoint);
			EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[4], slv_handles[slv].speed_setpoint);
		}

		// send process data
		ecrt_domain_queue(master_setup->domain);
		ecrt_master_send(master_setup->master);

		//Check for master und domain state
		ecrt_master_state(master_setup->master, &master_setup->master_state);
		ecrt_domain_state(master_setup->domain, &master_setup->domain_state);

		if (master_setup->domain_state.wc_state == EC_WC_COMPLETE && !master_setup->op_flag)
		{
			//printf("System up!\n");
			master_setup->op_flag = 1;
		}
		else
		{
			if(master_setup->domain_state.wc_state != EC_WC_COMPLETE && master_setup->op_flag)
			{
				//printf("System down!\n");
				master_setup->op_flag = 0;
			}
		}

		user_alarms++;
	}
}
Esempio n. 18
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);
}