libs
/
SOEM
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
SOEM/test/rtk/main.c

519 lines
15 KiB
C
Raw Blame History

/** \file
* \brief Example code for Simple Open EtherCAT master
*
* This is a minimal example running rt-kernel TTOS. Check
* tutorial files in documentations for som steps to get your
* system running.
* (c)Andreas Karlsson 2012
*/
#include <kern.h>
#include "ethercat.h"
#include "string.h"
#include <oshw.h>
#include <config.h>
#include <defBF537.h>
#define pPORTFIO_SET ((vuint16_t *)PORTFIO_SET)
#define pPORTFIO_CLEAR ((vuint16_t *)PORTFIO_CLEAR)
#define pPORTFIO_DIR ((vuint16_t *)PORTFIO_DIR)
#define EK1100_1 1
#define EL4001_1 2
#define EL3061_1 3
#define EL1008_1 4
#define EL1008_2 5
#define EL2622_1 6
#define EL2622_2 7
#define EL2622_3 8
#define EL2622_4 9
#define NUMBER_OF_SLAVES 9
typedef struct
{
uint8 in1;
uint8 in2;
uint8 in3;
uint8 in4;
uint8 in5;
uint8 in6;
uint8 in7;
uint8 in8;
} in_EL1008_t;
typedef struct
{
uint8 out1;
uint8 out2;
} out_EL2622_t;
typedef struct
{
int16 out1;
} out_EL4001_t;
typedef struct
{
int32 in1;
} in_EL3061_t;
out_EL4001_t slave_EL4001_1;
in_EL3061_t slave_EL3061_1;
in_EL1008_t slave_EL1008_1;
in_EL1008_t slave_EL1008_2;
out_EL2622_t slave_EL2622_1;
out_EL2622_t slave_EL2622_2;
out_EL2622_t slave_EL2622_3;
uint32 network_configuration(void)
{
/* Do we got expected number of slaves from config */
if (ec_slavecount < NUMBER_OF_SLAVES)
return 0;
/* Verify slave by slave that it is correct*/
if (strcmp(ec_slave[EK1100_1].name,"EK1100"))
return 0;
else if (strcmp(ec_slave[EL4001_1].name,"EL4001"))
return 0;
else if (strcmp(ec_slave[EL3061_1].name,"EL3061"))
return 0;
else if (strcmp(ec_slave[EL1008_1].name,"EL1008"))
return 0;
else if (strcmp(ec_slave[EL1008_2].name,"EL1008"))
return 0;
else if (strcmp(ec_slave[EL2622_1].name,"EL2622"))
return 0;
else if (strcmp(ec_slave[EL2622_2].name,"EL2622"))
return 0;
else if (strcmp(ec_slave[EL2622_3].name,"EL2622"))
return 0;
else if (strcmp(ec_slave[EL2622_4].name,"EL2622"))
return 0;
return 1;
}
int32 get_input_int32(uint16 slave_no,uint8 module_index)
{
int32 return_value;
uint8 *data_ptr;
/* Get the IO map pointer from the ec_slave struct */
data_ptr = ec_slave[slave_no].inputs;
/* Move pointer to correct module index */
data_ptr += module_index * 4;
/* Read value byte by byte since all targets can't handle misaligned
* addresses
*/
return_value = *data_ptr++;
return_value += (*data_ptr++ << 8);
return_value += (*data_ptr++ << 16);
return_value += (*data_ptr++ << 24);
return return_value;
}
void set_input_int32 (uint16 slave_no, uint8 module_index, int32 value)
{
uint8 *data_ptr;
/* Get the IO map pointer from the ec_slave struct */
data_ptr = ec_slave[slave_no].inputs;
/* Move pointer to correct module index */
data_ptr += module_index * 4;
/* Read value byte by byte since all targets can handle misaligned
* addresses
*/
*data_ptr++ = (value >> 0) & 0xFF;
*data_ptr++ = (value >> 8) & 0xFF;
*data_ptr++ = (value >> 16) & 0xFF;
*data_ptr++ = (value >> 24) & 0xFF;
}
uint8 get_input_bit (uint16 slave_no,uint8 module_index)
{
/* Get the the startbit position in slaves IO byte */
uint8 startbit = ec_slave[slave_no].Istartbit;
/* Mask bit and return boolean 0 or 1 */
if (*ec_slave[slave_no].inputs & BIT (module_index - 1 + startbit))
return 1;
else
return 0;
}
int16 get_output_int16(uint16 slave_no,uint8 module_index)
{
int16 return_value;
uint8 *data_ptr;
/* Get the IO map pointer from the ec_slave struct */
data_ptr = ec_slave[slave_no].outputs;
/* Move pointer to correct module index */
data_ptr += module_index * 2;
/* Read value byte by byte since all targets can handle misaligned
* addresses
*/
return_value = *data_ptr++;
return_value += (*data_ptr++ << 8);
return return_value;
}
void set_output_int16 (uint16 slave_no, uint8 module_index, int16 value)
{
uint8 *data_ptr;
/* Get the IO map pointer from the ec_slave struct */
data_ptr = ec_slave[slave_no].outputs;
/* Move pointer to correct module index */
data_ptr += module_index * 2;
/* Read value byte by byte since all targets can handle misaligned
* addresses
*/
*data_ptr++ = (value >> 0) & 0xFF;
*data_ptr++ = (value >> 8) & 0xFF;
}
uint8 get_output_bit (uint16 slave_no,uint8 module_index)
{
/* Get the the startbit position in slaves IO byte */
uint8 startbit = ec_slave[slave_no].Ostartbit;
/* Mask bit and return boolean 0 or 1 */
if (*ec_slave[slave_no].outputs & BIT (module_index - 1 + startbit))
return 1;
else
return 0;
}
void set_output_bit (uint16 slave_no, uint8 module_index, uint8 value)
{
/* Get the the startbit position in slaves IO byte */
uint8 startbit = ec_slave[slave_no].Ostartbit;
/* Set or Clear bit */
if (value == 0)
*ec_slave[slave_no].outputs &= ~(1 << (module_index - 1 + startbit));
else
*ec_slave[slave_no].outputs |= (1 << (module_index - 1 + startbit));
}
extern tt_sched_t * tt_sched[];
char IOmap[128];
int dorun = 0;
uint8_t load1s, load5s, load10s;
uint32_t error_counter = 0;
void tt_error (uint32_t task_ix);
void tt_error (uint32_t task_ix)
{
error_counter++;
}
static void my_cyclic_callback (void * arg)
{
while (1)
{
stats_get_load (&load1s, &load5s, &load10s);
rprintp ("Processor load was %d:%d:%d (1s:5s:10s) with TT errors: %d\n",
load1s, load5s, load10s,error_counter);
task_delay(20000);
}
}
void read_io (void * arg)
{
/* Function connected to cyclic TTOS task
* The function is executed cyclic according
* sceduel specified in schedule.tt
*/
*pPORTFIO_SET = BIT (6);
/* Send and receive processdata
* Note that you need som synchronization
* case you modifey IO in local application
*/
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
*pPORTFIO_CLEAR = BIT (6);
}
void simpletest(void *arg)
{
char *ifname = arg;
int cnt, i, j;
*pPORTFIO_DIR |= BIT (6);
rprintp("Starting simple test\n");
/* initialise SOEM */
if (ec_init(ifname))
{
rprintp("ec_init succeeded.\n");
/* find and auto-config slaves */
if ( ec_config_init(FALSE) > 0 )
{
rprintp("%d slaves found and configured.\n",ec_slavecount);
/* Check network setup */
if (network_configuration())
{
/* Run IO mapping */
ec_config_map(&IOmap);
rprintp("Slaves mapped, state to SAFE_OP.\n");
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
/* Print som information on the mapped network */
for( cnt = 1 ; cnt <= ec_slavecount ; cnt++)
{
rprintp("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %d[ns]\n Has DC: %d\n",
cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
rprintp(" Configured address: %x\n", ec_slave[cnt].configadr);
rprintp(" Outputs address: %x\n", ec_slave[cnt].outputs);
rprintp(" Inputs address: %x\n", ec_slave[cnt].inputs);
for(j = 0 ; j < ec_slave[cnt].FMMUunused ; j++)
{
rprintp(" FMMU%1d Ls:%x Ll:%4d Lsb:%d Leb:%d Ps:%x Psb:%d Ty:%x Act:%x\n", j,
(int)ec_slave[cnt].FMMU[j].LogStart, ec_slave[cnt].FMMU[j].LogLength, ec_slave[cnt].FMMU[j].LogStartbit,
ec_slave[cnt].FMMU[j].LogEndbit, ec_slave[cnt].FMMU[j].PhysStart, ec_slave[cnt].FMMU[j].PhysStartBit,
ec_slave[cnt].FMMU[j].FMMUtype, ec_slave[cnt].FMMU[j].FMMUactive);
}
rprintp(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n",
ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU1func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func);
}
rprintp("Request operational state for all slaves\n");
ec_slave[0].state = EC_STATE_OPERATIONAL;
/* send one valid process data to make outputs in slaves happy*/
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
/* request OP state for all slaves */
ec_writestate(0);
/* wait for all slaves to reach OP state */
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
if (ec_slave[0].state == EC_STATE_OPERATIONAL )
{
rprintp("Operational state reached for all slaves.\n");
}
else
{
rprintp("Not all slaves reached operational state.\n");
ec_readstate();
for(i = 1; i<=ec_slavecount ; i++)
{
if(ec_slave[i].state != EC_STATE_OPERATIONAL)
{
rprintp("Slave %d State=0x%04x StatusCode=0x%04x\n",
i, ec_slave[i].state, ec_slave[i].ALstatuscode);
}
}
}
/* Simple blinking lamps BOX demo */
uint8 digout = 0;
slave_EL4001_1.out1 = (int16)0x3FFF;
set_output_int16(EL4001_1,0,slave_EL4001_1.out1);
task_spawn ("t_StatsPrint", my_cyclic_callback, 20, 1024, (void *)NULL);
tt_start_wait (tt_sched[0]);
while(1)
{
dorun = 0;
slave_EL1008_1.in1 = get_input_bit(EL1008_1,1); // Start button
slave_EL1008_1.in2 = get_input_bit(EL1008_1,2); // Turnkey RIGHT
slave_EL1008_1.in3 = get_input_bit(EL1008_1,3); // Turnkey LEFT
/* (Turnkey MIDDLE + Start button) OR Turnkey RIGHT OR Turnkey LEFT
Turnkey LEFT: Light positions bottom to top. Loop, slow operation.
Turnkey MIDDLE: Press start button to light positions bottom to top. No loop, fast operation.
Turnkey RIGHT: Light positions bottom to top. Loop, fast operation.
*/
if (slave_EL1008_1.in1 || slave_EL1008_1.in2 || slave_EL1008_1.in3)
{
digout = 0;
/* *ec_slave[6].outputs = digout; */
/* set_output_bit(slave_name #,index as 1 output on module , value */
set_output_bit(EL2622_1,1,(digout & BIT (0))); /* Start button */
set_output_bit(EL2622_1,2,(digout & BIT (1))); /* Turnkey RIGHT */
set_output_bit(EL2622_2,1,(digout & BIT (2))); /* Turnkey LEFT */
set_output_bit(EL2622_2,2,(digout & BIT (3)));
set_output_bit(EL2622_3,1,(digout & BIT (4)));
set_output_bit(EL2622_3,2,(digout & BIT (5)));
while(dorun < 95)
{
dorun++;
if (slave_EL1008_1.in3)
task_delay(tick_from_ms(20));
else
task_delay(tick_from_ms(5));
digout = (uint8) (digout | BIT((dorun / 16) & 0xFF));
set_output_bit(EL2622_1,1,(digout & BIT (0))); /* LED1 */
set_output_bit(EL2622_1,2,(digout & BIT (1))); /* LED2 */
set_output_bit(EL2622_2,1,(digout & BIT (2))); /* LED3 */
set_output_bit(EL2622_2,2,(digout & BIT (3))); /* LED4 */
set_output_bit(EL2622_3,1,(digout & BIT (4))); /* LED5 */
set_output_bit(EL2622_3,2,(digout & BIT (5))); /* LED6 */
slave_EL1008_1.in1 = get_input_bit(EL1008_1,2); /* Turnkey RIGHT */
slave_EL1008_1.in2 = get_input_bit(EL1008_1,3); /* Turnkey LEFT */
slave_EL3061_1.in1 = get_input_int32(EL3061_1,0); /* Read AI */
}
}
task_delay(tick_from_ms(2));
}
}
else
{
rprintp("Mismatch of network units!\n");
}
}
else
{
rprintp("No slaves found!\n");
}
rprintp("End simple test, close socket\n");
/* stop SOEM, close socket */
ec_close();
}
else
{
rprintp("ec_init failed");
}
rprintp("End program\n");
}
static void test_osal_timer_timeout_us (const uint32 timeout_us)
{
osal_timert timer;
ASSERT (timeout_us > 4000);
osal_timer_start (&timer, timeout_us);
ASSERT (osal_timer_is_expired (&timer) == false);
osal_usleep (timeout_us - 2000);
ASSERT (osal_timer_is_expired (&timer) == false);
osal_usleep (4000);
ASSERT (osal_timer_is_expired (&timer));
}
static void test_osal_timer (void)
{
test_osal_timer_timeout_us (10*1000); /* 10ms */
test_osal_timer_timeout_us (100*1000); /* 100ms */
test_osal_timer_timeout_us (1000*1000); /* 1s */
test_osal_timer_timeout_us (2000*1000); /* 2s */
}
#define USECS_PER_SEC 1000000
#define USECS_PER_TICK (USECS_PER_SEC / CFG_TICKS_PER_SECOND)
#ifndef ABS
#define ABS(x) ((x) < 0 ? -(x) : (x))
#endif
static int32 time_difference_us (const ec_timet stop, const ec_timet start)
{
int32 difference_us;
ASSERT (stop.sec >= start.sec);
if (stop.sec == start.sec)
{
ASSERT (stop.usec >= start.usec);
}
difference_us = (stop.sec - start.sec) * USECS_PER_SEC;
difference_us += ((int32)stop.usec - (int32)start.usec);
ASSERT (difference_us >= 0);
return difference_us;
}
/**
* Test osal_current_time() by using it for measuring how long an osal_usleep()
* takes, in specified number of microseconds.
*/
static void test_osal_current_time_for_delay_us (const int32 sleep_time_us)
{
ec_timet start;
ec_timet stop;
int32 measurement_us;
int32 deviation_us;
const int32 usleep_accuracy_us = USECS_PER_TICK;
boolean is_deviation_within_tolerance;
start = osal_current_time ();
osal_usleep (sleep_time_us);
stop = osal_current_time ();
measurement_us = time_difference_us (stop, start);
deviation_us = ABS (measurement_us - sleep_time_us);
is_deviation_within_tolerance = deviation_us <= usleep_accuracy_us;
ASSERT (is_deviation_within_tolerance);
}
static void test_osal_current_time (void)
{
test_osal_current_time_for_delay_us (0);
test_osal_current_time_for_delay_us (1);
test_osal_current_time_for_delay_us (500);
test_osal_current_time_for_delay_us (999);
test_osal_current_time_for_delay_us (USECS_PER_TICK);
test_osal_current_time_for_delay_us (USECS_PER_TICK-1);
test_osal_current_time_for_delay_us (USECS_PER_TICK+1);
test_osal_current_time_for_delay_us (2 * 1000 * 1000); /* 2s */
}
#include <lwip/inet.h>
static void test_oshw_htons (void)
{
uint16 network;
uint16 host;
host = 0x1234;
network = oshw_htons (host);
ASSERT (network == htons (host));
}
static void test_oshw_ntohs (void)
{
uint16 host;
uint16 network;
network = 0x1234;
host = oshw_ntohs (network);
ASSERT (host == ntohs (network));
}
int main (void)
{
test_oshw_htons ();
test_oshw_ntohs ();
test_osal_timer ();
test_osal_current_time ();
rprintp("SOEM (Simple Open EtherCAT Master)\nSimple test\n");
task_spawn ("simpletest", simpletest, 9, 8192, NULL);
return (0);
}
Reference in New Issue View Git Blame Copy Permalink