You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
296 lines
7.4 KiB
296 lines
7.4 KiB
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include "slaveconfig.h"
|
|
#include "ethercat.h"
|
|
|
|
#define EC_TIMEOUTMON 100
|
|
|
|
char IOmap[4096];
|
|
volatile int wkc;
|
|
OSAL_THREAD_HANDLE thread1;
|
|
int expectedWKC;
|
|
boolean needlf;
|
|
volatile int wkc;
|
|
uint8 currentgroup = 0;
|
|
|
|
|
|
/*!
|
|
* @brief Set the given Slave to given State
|
|
* @return 1 if No Error | 0 if Error
|
|
* */
|
|
uint8_t setSlaveState(uint8_t slave_index, uint16_t ethercat_state )
|
|
{
|
|
//printf("SET STATE: %s - Slave[%d] \n",ec_slave[slave_index].name, slave_index);
|
|
ec_slave[slave_index].state = ethercat_state;// ec_slave[0] refers to a special virtual slave that represents all slaves
|
|
ec_writestate(slave_index); // Same as above
|
|
ec_statecheck(slave_index, ethercat_state, EC_TIMEOUTSTATE);
|
|
|
|
if (ec_slave[slave_index].state == ethercat_state) {
|
|
{
|
|
// printf(" |- ");
|
|
// printState(ec_slave[slave_index].state);
|
|
}
|
|
return 1;
|
|
} else {
|
|
// printf(" |-> Failed\n");
|
|
return 0;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
void searchForSlave()
|
|
{
|
|
for (int i = 1; i <= ec_slavecount; i++) {
|
|
if (strcmp(ec_slave[i].name, "EL2008") == 0) {
|
|
el2008_slave = i;
|
|
printf("Found EL2008 at slave %d.\n", i);
|
|
}
|
|
}
|
|
|
|
if (el2008_slave == -1) {
|
|
printf("EL2008 not found on the network.\n");
|
|
}
|
|
}
|
|
*/
|
|
|
|
|
|
void print_device_info() {
|
|
printf("--------------------------------------------------\n");
|
|
printf("EtherCAT Device Info Panel\n");
|
|
printf("--------------------------------------------------\n");
|
|
|
|
for (int i = 1; i <= ec_slavecount; i++) {
|
|
printf("Slave %d: %s\n", i, ec_slave[i].name);
|
|
printf(" Vendor ID: 0x%08X\n", ec_slave[i].eep_man);
|
|
printf(" Product Code: 0x%08X\n", ec_slave[i].eep_id);
|
|
printf(" Revision: 0x%08X\n", ec_slave[i].eep_rev);
|
|
printf(" ");
|
|
printState(ec_slave[i].state);
|
|
printf(" AL Status Code: 0x%04X\n", ec_slave[i].ALstatuscode);
|
|
|
|
// Print Process Data (PDOs)
|
|
printf(" Process Data:\n");
|
|
printf(" Inputs (%d bytes): ", ec_slave[i].Ibytes);
|
|
for (uint32_t j = 0; j < ec_slave[i].Ibytes; j++) {
|
|
printf("%02X ", ec_slave[i].inputs[j]);
|
|
}
|
|
printf("\n");
|
|
|
|
printf(" Outputs (%d bytes): ", ec_slave[i].Obytes);
|
|
for (uint32_t j = 0; j < ec_slave[i].Obytes; j++) {
|
|
printf("%02X ", ec_slave[i].outputs[j]);
|
|
}
|
|
printf("\n");
|
|
|
|
// Read available SDOs
|
|
printf(" SDO List:\n");
|
|
for (int sdo_index = 0x1000; sdo_index <= 0x1FFF; sdo_index++) {
|
|
uint32_t sdo_value;
|
|
int size = sizeof(sdo_value);
|
|
if (ec_SDOread(i, sdo_index, 0x00, FALSE, &size, &sdo_value, EC_TIMEOUTSAFE) > 0) {
|
|
printf(" Index 0x%04X: Value 0x%08X\n", sdo_index, sdo_value);
|
|
}
|
|
}
|
|
printf("--------------------------------------------------\n");
|
|
}
|
|
}
|
|
|
|
|
|
void setPin1Out()
|
|
{
|
|
uint8_t *outputs = (uint8_t *)ec_slave[3].outputs;
|
|
|
|
// Print current output state before modification
|
|
printf("Current output state: 0x%02X\n", outputs[0]);
|
|
|
|
// Set bit 0 (Channel 1 ON)
|
|
outputs[0] |= 1;
|
|
|
|
// Send process data to the slave
|
|
ec_send_processdata();
|
|
|
|
wkc = ec_receive_processdata(EC_TIMEOUTRET);
|
|
|
|
if (wkc >= expectedWKC) {
|
|
printf("Process data exchanged successfully.\n");
|
|
} else {
|
|
printf("Process data exchange failed. WKC: %d, expectedWKC: %d\n", wkc, expectedWKC);
|
|
}
|
|
|
|
// Print final output state
|
|
printf("Final output state: 0x%02X\n", outputs[0]);
|
|
|
|
// Print slave state and AL status code
|
|
printState(ec_slave[3].state);
|
|
}
|
|
|
|
|
|
/*!
|
|
* @brief Initialising Distributed Clocks. you should initialize Distributed Clocks (DC)
|
|
* after all slaves are configured and mapped,
|
|
* but before transitioning them to OPERATIONAL (EC_STATE_OPERATIONAL).
|
|
* @return 1 if No Error | 0 if Error
|
|
* */
|
|
uint8_t initDisributedClocks()
|
|
{
|
|
printf("Init Distributed Clocks\n");
|
|
if (ec_configdc()) {
|
|
printf(" |-> Succeeded \n");
|
|
return 1;
|
|
}else
|
|
{
|
|
printf(" |-> Failed \n");
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
|
|
/*!
|
|
* @brief map process data objects (PDOs) of all detected EtherCAT slaves into a single memory buffer (IOmap).
|
|
* and groupe them into group[0] This will be important for the Slaves state intitalisation phase.
|
|
* @return 1 if No Error | 0 if Error
|
|
* */
|
|
uint8_t mapSlavesGrouped()
|
|
{
|
|
printf("Mapping Slaves\n");
|
|
if(ec_config_map(&IOmap)){
|
|
printf(" |-> Succeeded \n");
|
|
for (uint8_t i = 1; i <= ec_slavecount; i++) {
|
|
printf(" |-> (%s) - ",ec_slave[i].name);
|
|
printState(ec_slave[i].state);
|
|
}
|
|
return 1;
|
|
}else
|
|
{
|
|
printf(" |-> Failed \n");
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* @brief Enumerate and init all slaves.>
|
|
* @return 1 if No Error | 0 if Error
|
|
* */
|
|
uint8_t enumerateSlaves()
|
|
{
|
|
printf("Detection and enumeration \n");
|
|
if (ec_config_init(FALSE) > 0) {
|
|
printf(" |-> Succeeded \n");
|
|
printf(" |-> Slave Count: %d \n",ec_slavecount);
|
|
return 1;
|
|
}else
|
|
{
|
|
printf(" |-> Failed \n");
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* @brief Initialising network atapter for communication with slaves
|
|
* @param char *Ifname is the interface name
|
|
* @return 1 if No Error | 0 if Error
|
|
* */
|
|
uint8_t initNetworkAdapterSingle(char *ifname)
|
|
{
|
|
printf("Init Network Interface < %s >\n", ifname);
|
|
if (ec_init(ifname)) {
|
|
printf(" |-> Succeeded \n");
|
|
return 1;
|
|
}else
|
|
{
|
|
printf(" |-> Failed \n");
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
void dataLoop()
|
|
{
|
|
ec_send_processdata();
|
|
wkc = ec_receive_processdata(EC_TIMEOUTRET);
|
|
if(wkc >= expectedWKC)
|
|
{
|
|
|
|
}
|
|
osal_usleep(5000);
|
|
}
|
|
|
|
int main(int argc, char *argv[]) {
|
|
if (argc > 1) {
|
|
// Init Network adapter
|
|
if(!initNetworkAdapterSingle(argv[1])){
|
|
printf("Unable to Initiate Network Adapter -> Quitting");
|
|
ec_close();
|
|
return 0;
|
|
}
|
|
|
|
if(!enumerateSlaves()){
|
|
printf("Unable to Enumerate Slaves -> Quitting");
|
|
ec_close();
|
|
return 0;
|
|
}
|
|
|
|
if(!mapSlavesGrouped()){
|
|
printf("Unable to Map Slaves -> Quitting");
|
|
ec_close();
|
|
return 0;
|
|
}
|
|
|
|
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
|
|
printf("Calculated workcounter %d\n", expectedWKC);
|
|
|
|
if(!initDisributedClocks()){
|
|
printf("Unable to Initialise Distributed Clocks -> Quitting");
|
|
ec_close();
|
|
return 0;
|
|
}
|
|
|
|
for (uint8_t i = 1; i <= ec_slavecount; i++) {
|
|
printf("-----------------------------------------------------------------------\n");
|
|
print_device_info();
|
|
printf("-----------------------------------------------------------------------\n");
|
|
if(!setSlaveState(1,EC_STATE_INIT)){
|
|
printf("Unable chnage Slave mode -> Quitting");
|
|
ec_close();
|
|
return 0;
|
|
}
|
|
if(!setSlaveState(i,EC_STATE_PRE_OP)){
|
|
printf("Unable chnage Slave mode -> Quitting");
|
|
ec_close();
|
|
return 0;
|
|
}
|
|
if(!setSlaveState(i,EC_STATE_SAFE_OP)){
|
|
printf("Unable chnage Slave mode -> Quitting");
|
|
ec_close();
|
|
return 0;
|
|
}
|
|
if(!setSlaveState(i,EC_STATE_OPERATIONAL)){
|
|
printf("Unable chnage Slave mode -> Quitting");
|
|
ec_close();
|
|
}
|
|
printf("-----------------------------------------------------------------------\n");
|
|
}
|
|
|
|
setPin1Out();
|
|
|
|
for(uint16_t j = 0; j < 10000; j++)
|
|
{
|
|
ec_send_processdata();
|
|
wkc = ec_receive_processdata(EC_TIMEOUTRET);
|
|
|
|
if (wkc >= expectedWKC) {
|
|
} else {
|
|
printf("Process data exchange failed. WKC: %d, expectedWKC: %d\n", wkc, expectedWKC);
|
|
}
|
|
usleep(100);
|
|
}
|
|
|
|
ec_close();
|
|
} else {
|
|
printf("Usage : sudo %s <network interface name> need to be passed\n", argv[0]);
|
|
printf("Example : sudo %s eth0 \n", argv[0]);
|
|
}
|
|
return 0;
|
|
}
|