本篇文章僅做學習SOEM研究參考使用，不能做為商業。目前網上還沒有windows版本的CSP模式控制。這裡給出實際能運作的代碼，以供有興趣的同學參考。
#include <stdlib.h>
#include <sched.h>
#include <stdio.h>
#include <string.h>
#include <time.h>
#include <pthread.h>
#include <math.h>
#include "osal.h"
#include "ethercattype.h"
#include "nicdrv.h"
#include "ethercatbase.h"
#include "ethercatmain.h"
#include "ethercatdc.h"
#include "ethercatcoe.h"
#include "ethercatfoe.h"
#include "ethercatconfig.h"
#include "ethercatprint.h"
#define EC_TIMEOUTMON 500
#define NSEC_PER_SEC 1000000000
#define CYCLETIME 4000000U
#define SHIFTTIME 20000U
#define CYCLE2TIME 8000000U
#define POSITIONSTEP 1000
#ifndef EC_STATE_NONE
#define EC_STATE_NONE 0
#endif
#define SWAP_WORD(l) (LO_WORD(l) << 16 | HI_WORD(l) & 0xffff)
struct sched_param schedp;
char IOmap[4096];
pthread_t thread1, thread2;
struct timeval tv, t1, t2;
int dorun = 0;
int deltat, tmax = 0;
int64 toff, gl_delta;
int DCdiff;
int os;
uint8 ob;
uint16 ob2;
uint8 *digout = 0;
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;
int ec507_pos = 1;
typedef struct PACKED
{
uint16_t Controlword; //control word
int16_t TargetPosLow; //target pos int32
int16_t TargetPosHigh;
} Drive_Outputs;
typedef struct PACKED
{
uint16_t ErrorCode;
uint16_t Statusword;
int8_t ModeOpDisp;
int32_t ActualPos;
int16_t TouchProbStatus;
int32_t TouchProb1PPosition;
int32_t DigitalInput;
} Drive_Inputs;
static int drive_write8(uint16 slave, uint16 index, uint8 subindex, uint8 value)
{
int wkc;
wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
return wkc;
}
static int drive_write16(uint16 slave, uint16 index, uint8 subindex, uint16 value)
{
int wkc;
wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
return wkc;
}
static int drive_write32(uint16 slave, uint16 index, uint8 subindex, int32 value)
{
int wkc;
wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
return wkc;
}
int EC507_setup(uint16 slave)
{
int wkc = 0;
printf("LeadShine EC507 Drive Setup\n");
printf("set 1c12 and 1c13\n");
wkc += drive_write8(slave, 0x1C12, 0, 0);
wkc += drive_write8(slave, 0x1C13, 0, 0);
printf("Setup TxPDO 0x1A00\n");
wkc += drive_write8(slave, 0x1A00, 0, 0);
wkc += drive_write32(slave, 0x1A00, 1, 0x603F0010); // Error Code
wkc += drive_write32(slave, 0x1A00, 2, 0x60410010); // Statusword
wkc += drive_write32(slave, 0x1A00, 3, 0x60610008); // Modes of operation display
wkc += drive_write32(slave, 0x1A00, 4, 0x60640020); // Position actual value
wkc += drive_write32(slave, 0x1A00, 5, 0x60B90010); // Touch Probe status
wkc += drive_write32(slave, 0x1A00, 6, 0x60BA0020); // Touch Probe1 Positive Position
wkc += drive_write32(slave, 0x1A00, 7, 0x60FD0020); // Digital Input
wkc += drive_write8(slave, 0x1A00, 0, 7);
printf("Setup RxPDO 0x1600\n");
wkc += drive_write8(slave, 0x1600, 0, 0);
wkc += drive_write32(slave, 0x1600, 1, 0x60400010); // Controlword
wkc += drive_write32(slave, 0x1600, 2, 0x607A0020); // Target position,
//wkc += drive_write32(slave, 0x1600, 3, 0x60600008); // Operation Mode
wkc += drive_write8(slave, 0x1600, 0, 2);
wkc += drive_write16(slave, 0x1C12, 1, 0x1600);
wkc += drive_write8(slave, 0x1C12, 0, 1);
wkc += drive_write16(slave, 0x1C13, 1, 0x1A00);
wkc += drive_write8(slave, 0x1C13, 0, 1);
strncpy(ec_slave[slave].name, "Drive", EC_MAXNAME);
if (wkc != 19)
{
printf("Drive %d setup failed\nwkc: %d\n", slave, wkc);
return -1;
}
else
printf("Drive %d setup succeed.\n", slave);
return 0;
}
// CanOPEN input/output rw helper functions
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;
/* 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;
}
int16 get_input_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].inputs;
/* Move pointer to correct module index */
data_ptr += module_index;
/* Read value byte by byte since all targets can't handle misaligned
* addresses
*/
return_value = *data_ptr++;
return_value += (*data_ptr++ << 8);
return return_value;
}
int8 get_input_int8(uint16 slave_no, uint8 module_index) {
int8 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;
/* Read value byte by byte since all targets can't handle misaligned
* addresses
*/
return_value = *data_ptr++;
return return_value;
}
//only for debug
#if 1
int32 get_output_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].outputs;
/* Move pointer to correct module index */
data_ptr += module_index;
/* 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;
}
#endif
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;
/* Read value byte by byte since all targets can handle misaligned
* addresses
*/
*data_ptr++ = (value >> 0) & 0xFF;
*data_ptr++ = (value >> 8) & 0xFF;
}
void slavetop(int i)
{
ec_slave[i].state = EC_STATE_OPERATIONAL;
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
ec_writestate(0);
}
void simpletest(char *ifname) {
int cnt, i, j, oloop, iloop;
printf("Starting Simple test\n");
/* initialise SOEM, bind socket to ifname */
if (ec_init(ifname)) {
printf("ec_init on %s succeeded.\n", ifname);
/* find and auto-config slaves */
if (ec_config_init(FALSE) > 0) // == ec_config_init + ec_config_map
{
printf("%d slaves found and configured.\n", ec_slavecount);
// PO2SOconfig is for registering a hook function that will be called when the slave does the transition
// between Pre-OP and Safe-OP.
if ((ec_slavecount > 1)) {
for (cnt = 1; cnt <= ec_slavecount; cnt++) {
if (cnt == 1) { // eep_id == ProductCode on ESI
ec507_pos = cnt;
printf("Found %s at position %d\n", ec_slave[cnt].name, ec507_pos);
ec_slave[cnt].PO2SOconfig = &EC507_setup;
}
}
}
/* configure DC options for every DC capable slave found in the list */
// Special configuration parameters for EL7201 contoller (copied from TwinCat configuration)
// This step should be done before config_map
//ec_dcsync0(ec507_pos, TRUE, 2000000U, 10000U);
//ec_dcsync01(ec507_pos, TRUE, 2000000U, 4000000U, 10000U);
//ec_dcsync0(ec507_pos, TRUE, 1000000U, 5000U);
//ec_dcsync01(ec507_pos, TRUE, 1000000U, 2000000U, 5000U);
//ec_dcsync0(ec507_pos, TRUE, 1000000U, 10000U);
//ec_dcsync01(ec507_pos, TRUE, 1000000U, 2000000U, 10000U);
ec_dcsync0(ec507_pos, TRUE, CYCLETIME, SHIFTTIME);
ec_dcsync01(ec507_pos, TRUE, CYCLETIME, CYCLE2TIME, SHIFTTIME);
ec_configdc();
ec_config_map(&IOmap);
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
/* read indevidual slave state and store in ec_slave[] */
ec_readstate();
for (cnt = 1; cnt <= ec_slavecount; cnt++) {
printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
ec_slave[cnt].state, (int) ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
printf(" Out:%li,%4d In:%li,%4d\n",
(intptr_t) ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (intptr_t) ec_slave[cnt].inputs,
ec_slave[cnt].Ibytes);
}
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
printf("Calculated workcounter %d\n", expectedWKC);
printf("Request operational state for all slaves\n");
ec_slave[0].state = EC_STATE_OPERATIONAL;
/* request OP state for all slaves */
ec_writestate(0);
//slavetop(1);
/* activate cyclic process data */
dorun = 1;
/* wait for all slaves to reach OP state */
ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
oloop = ec_slave[0].Obytes;
if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1;
if (oloop > 8) oloop = 8;
iloop = ec_slave[0].Ibytes;
if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1;
if (iloop > 8) iloop = 8;
if (ec_slave[0].state == EC_STATE_OPERATIONAL) {
printf("Operational state reached for all slaves.\n");
inOP = TRUE;
/* acyclic loop 5000 x 20ms = 10s */
for (i = 1; i <= 1000; i++) {
printf("Processdata cycle %5d , Wck %3d, DCtime %lli, dt %li, O:",
dorun, wkc, ec_DCtime, gl_delta);
for (j = 0; j < oloop; j++) {
printf(" %2.2x", *(ec_slave[0].outputs + j));
}
printf(" I:");
for (j = 0; j < iloop; j++) {
printf(" %2.2x", *(ec_slave[0].inputs + j));
}
printf("\r");
fflush(stdout);
osal_usleep(20000);
}
dorun = 0;
inOP = FALSE;
} else {
printf("Not all slaves reached operational state.\n");
ec_readstate();
for (i = 1; i <= ec_slavecount; i++) {
if (ec_slave[i].state != EC_STATE_OPERATIONAL) {
printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
i, ec_slave[i].state, ec_slave[i].ALstatuscode,
ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
}
}
}
printf("Request safe operational state for all slaves\n");
ec_slave[0].state = EC_STATE_SAFE_OP;
/* request SAFE_OP state for all slaves */
ec_writestate(0);
// wait
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
} else {
printf("No slaves found!\n");
}
printf("End simple test, close socket\n");
/* stop SOEM, close socket */
ec_close();
} else {
printf("No socket connection on %s\nExcecute as root\n", ifname);
}
}
/* add ns to timespec */
void add_timespec(struct timespec *ts, int64 addtime) {
int64 sec, nsec;
nsec = addtime % NSEC_PER_SEC;
sec = (addtime - nsec) / NSEC_PER_SEC;
ts->tv_sec += sec;
ts->tv_nsec += nsec;
if (ts->tv_nsec > NSEC_PER_SEC) {
nsec = ts->tv_nsec % NSEC_PER_SEC;
ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
ts->tv_nsec = nsec;
}
}
/* PI calculation to get linux time synced to DC time */
void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime) {
static int64 integral = 0;
int64 delta;
/* set linux sync point 50us later than DC sync, just as example */
delta = (reftime - 50000) % cycletime;
if (delta > (cycletime / 2)) { delta = delta - cycletime; }
if (delta > 0) { integral++; }
if (delta < 0) { integral--; }
*offsettime = -(delta / 100) - (integral / 20);
gl_delta = delta;
}
/* RT EtherCAT thread */
OSAL_THREAD_FUNC_RT ecatthread(void *ptr) {
struct timespec ts;
int ht;
int64 cycletime;
int16 errorcode;
int16 statusword;
int8 modeopdisplay;
int32 position_actual;
int32 position_target;
int16 position_target_low;
int16 position_target_high;
int16 control = 0;
int32 velocity_actual;
int n_steps = 0;
//following line can be replaced by sections to get timespec
//clock_gettime(CLOCK_MONOTONIC, &ts);
int64 sec, nsec;
LARGE_INTEGER num;
long long longtimenum;
long long start, end, freq;
QueryPerformanceFrequency(&num);
freq = num.QuadPart;
QueryPerformanceCounter(&num);
start = num.QuadPart;
longtimenum = longtimenum = (start * 1000 ) / freq; //ns
nsec = longtimenum % NSEC_PER_SEC;
sec = (longtimenum - nsec) / NSEC_PER_SEC;
ts.tv_nsec = nsec;
ts.tv_sec = sec;
printf("sec=%lld, nsec=%lld\n", sec,nsec);
ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */
ts.tv_nsec = ht * 1000000;
//cycletime = *(int *) ptr * 1000; /* cycletime in ns */
cycletime = CYCLETIME;
toff = 0;
dorun = 0;
ec_send_processdata();
int ntick=0;
while (1) {
/* calculate next cycle start */
add_timespec(&ts, cycletime + toff);
/* wait to cycle start */
//clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft);
osal_usleep((cycletime + toff) / 1000);
if (dorun > 0) {
wkc = ec_receive_processdata(EC_TIMEOUTRET);
dorun++;
if (ec_slave[0].hasdc) {
printf("master have configured DC\n");
/* calulate toff to get linux time and DC synced */
ec_sync(ec_DCtime, cycletime, &toff);
} else {
printf("master have not configured DC\n");
}
if (ec_slave[0].state == EC_STATE_OPERATIONAL) {
// TODO: read as struct
int32 nReg607a = get_output_int32(ec507_pos,2);
printf("last target position: %8.8x\t", nReg607a);
errorcode = get_input_int16(ec507_pos, 0);
printf("err: %8.8x\t", errorcode);
statusword = get_input_int16(ec507_pos, 2);
printf("sts: %8.8x\t", statusword);
modeopdisplay = get_input_int8(ec507_pos, 4);
printf("mod: %8.8x\t", modeopdisplay);
position_actual = get_input_int32(ec507_pos, 5);
printf("act pos: %8.8x\t", position_actual);
position_target = position_actual + POSITIONSTEP;
//position_target = 5 * ntick++;
printf("tar pos: %8.8x\n", position_target);
position_target_low = LO_WORD(position_target);
position_target_high = HI_WORD(position_target);
printf("tar pos low: %8.8x\n", position_target_low);
printf("tar pos high: %8.8x\n", position_target_high);
if ((statusword & 0x004f) == 0x0040) { // status: init
control = 0x0006;
printf("Status: init\n");
position_target = position_actual;
position_target_low = LO_WORD(position_target);
position_target_high = HI_WORD(position_target);
set_output_int16(ec507_pos, 2, position_target_low);
set_output_int16(ec507_pos, 4, position_target_high);
} else if ((statusword & 0x006f) == 0x0021) { // status: to enable
control = 0x0007;
printf("Status: to enable\n");
} else if ((statusword & 0x006f) == 0x0023) { // status: enable
control = 0x001f;
printf("Status: enable\n");
} else if ((statusword & 0x006f) == 0x0027) { // status: to send command
printf("Status: send target position\n");
control = 0x001f;
set_output_int16(ec507_pos, 2, position_target_low);
set_output_int16(ec507_pos, 4, position_target_high);
}
set_output_int16(ec507_pos, 0, control);
}
ec_send_processdata();
}
}
}
OSAL_THREAD_FUNC ecatcheck() // void *ptr )
{
int slave;
while (1) {
if (inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate)) {
if (needlf) {
needlf = FALSE;
printf("\n");
}
/* one ore more slaves are not responding */
ec_group[currentgroup].docheckstate = FALSE;
ec_readstate();
for (slave = 1; slave <= ec_slavecount; slave++) {
if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) {
ec_group[currentgroup].docheckstate = TRUE;
if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) {
printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
ec_writestate(slave);
} else if (ec_slave[slave].state == EC_STATE_SAFE_OP) {
printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
ec_slave[slave].state = EC_STATE_OPERATIONAL;
ec_writestate(slave);
} else if (ec_slave[slave].state > EC_STATE_NONE) {
if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) {
ec_slave[slave].islost = FALSE;
printf("MESSAGE : slave %d reconfigured\n", slave);
}
} else if (!ec_slave[slave].islost) {
/* re-check state */
ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
if (ec_slave[slave].state == EC_STATE_NONE) {
ec_slave[slave].islost = TRUE;
printf("ERROR : slave %d lost\n", slave);
}
}
}
if (ec_slave[slave].islost) {
if (ec_slave[slave].state == EC_STATE_NONE) {
if (ec_recover_slave(slave, EC_TIMEOUTMON)) {
ec_slave[slave].islost = FALSE;
printf("MESSAGE : slave %d recovered\n", slave);
}
} else {
ec_slave[slave].islost = FALSE;
printf("MESSAGE : slave %d found\n", slave);
}
}
}
if (!ec_group[currentgroup].docheckstate)
printf("OK : all slaves resumed OPERATIONAL.\n");
}
osal_usleep(10000);
}
}
#define stack64k (64 * 1024)
int main(int argc, char *argv[]) {
int ctime;
printf("SOEM (Simple Open EtherCAT Master)\nSimple Step Motor for EC507 test\n");
if (argc > 2) {
dorun = 0;
ctime = atoi(argv[2]);
/* create RT thread */
osal_thread_create_rt(&thread1, stack64k * 4, &ecatthread, (void *) &ctime);
/* create thread to handle slave error handling in OP */
osal_thread_create(&thread2, stack64k * 4, &ecatcheck, NULL);
/* start acyclic part */
simpletest(argv[1]);
} else {
printf("Usage: simple_test ifname1 cycletime\nifname = eth0 for example\ncycletime in us\n");
}
printf("End program\n");
return (0);
}
![HTTP 錯誤 403.9 - 禁止通路：連接配接的使用者過多 XP IIS伺服器連接配接數的修改[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)