static void* run_master(void * arg) {
ni = init_ec((char*)arg);
init_slave_db();
if(ni != 0) {
printf("Attach netif \n\n");
EtherCAT_DataLinkLayer::instance()->attach(ni);
printf("Master initializing \n\n");
EtherCAT_Master * EM = EtherCAT_Master::instance();
printf("Getting slave handler\n");
EtherCAT_SlaveHandler * sh_ek1100 = EM->get_slave_handler(0x03e9);
//sleep(1);
printf("Setting EK1100 to OP STATE\n");
if(sh_ek1100->to_state(EC_OP_STATE))
printf("EK1100 succesfully set in OP STATE\n");
else
printf("\nfailed to set EK1100 in OP STATE\n");
printf("Getting slave handler\n");
EtherCAT_SlaveHandler * sh_el4102 = EM->get_slave_handler(0x03ea);
printf("Setting EL4102 to OP STATE\n");
if(sh_el4102->to_state(EC_OP_STATE))
printf("EL4102 succesfully set in OP STATE\n");
else
printf("\nfailed to set EL4102 in OP STATE!!\n");
printf("AL initializing \n\n");
EtherCAT_AL * AL = EtherCAT_AL::instance();
///Set Channel 1 to 5V
unsigned char msg[2] = {0xff, 0x3f};
if(AL->isReady()) {
printf("Test: Set Channel 1 to 5V: \n\n");
int count = 0;
while(count<100000) {
EM->txandrx_PD(sizeof(msg),msg);
count++;
}
printf("Test done.\n");
}
close_socket(ni);
}
}
void EthercatDemoIO::update() {
EtherCAT_Master *EM = EtherCAT_Master::instance();
//printf("Got master\n");
unsigned char msg[10] = {0}; //Data to Send
int samples = (int)(1000.0 / this->engine()->getActivity()->getPeriod() / sinus_freq);
if(cnt > samples)
cnt = 0;
time = M_PI * 2 * cnt / samples;
voltage_f = (sin(time) * 5 + 5) * 32767 / 10;
voltage_i = (int) voltage_f;
msg[0] = voltage_i; msg[1] = voltage_i>>8;
//msg[4] = cnt_dig & 0x01;
//printf("mdg[4] = %d\n",msg[4]);
//printf("samples = %d cnt = %d time = %f voltage = %f msg[0] =0x%x msg[1] = 0cx%x \n", samples,cnt,time, voltage_f,msg[0], msg[1]); // Digital Output Channel 1
cnt++; cnt_dig++;
EM->txandrx_PD(sizeof(msg),msg);
int volt = msg[6];
volt = volt<<8;
volt = volt | msg[5];
//printf("voltage at channel0: 0x%x 0x%x %d\n",msg[5],msg[6], volt);
}
void init(char *interface)
{
struct netif *ni;
// Initialize network interface
if ((ni = init_ec(interface)) == NULL)
{
fprintf(stderr, "Unable to initialize interface: %s", interface);
exit(-1);
}
// Initialize Application Layer (AL)
EtherCAT_DataLinkLayer::instance()->attach(ni);
EtherCAT_AL *al;
if ((al = EtherCAT_AL::instance()) == NULL)
{
fprintf(stderr, "Unable to initialize Application Layer (AL): %08x", (int)al);
exit(-1);
}
uint32_t num_slaves = al->get_num_slaves();
if (num_slaves == 0)
{
fprintf(stderr, "Unable to locate any slaves");
exit(-1);
}
// Initialize Master
EtherCAT_Master *em;
if ((em = EtherCAT_Master::instance()) == NULL)
{
fprintf(stderr, "Unable to initialize EtherCAT_Master: %08x", int(em));
exit(-1);
}
static int startAddress = 0x00010000;
for (unsigned int slave = 0; slave < num_slaves; ++slave)
{
EC_FixedStationAddress fsa(slave + 1);
EtherCAT_SlaveHandler *sh = em->get_slave_handler(fsa);
if (sh == NULL)
{
fprintf(stderr, "Unable to get slave handler #%d", slave);
exit(-1);
}
if (sh->get_product_code() == WG05::PRODUCT_CODE)
{
printf("found a WG05 at #%d\n", slave);
WG05 *dev = new WG05();
dev->configure(startAddress, sh);
devices.push_back(dev);
}
else if (sh->get_product_code() == WG06::PRODUCT_CODE)
{
printf("found a WG06 at #%d\n", slave);
WG06 *dev = new WG06();
dev->configure(startAddress, sh);
devices.push_back(dev);
}
else
{
printf("found a %08x at #%d\n", sh->get_product_code(), slave);
devices.push_back(NULL);
}
}
BOOST_FOREACH(WG0X *device, devices)
{
Actuator a;
if (!device) continue;
if (!device->sh_->to_state(EC_OP_STATE))
{
fprintf(stderr, "Unable set device %d into OP_STATE", device->sh_->get_ring_position());
}
device->initialize(&a, true);
}
