<html>
<head>
<style><!--
.hmmessage P
{
margin:0px;
padding:0px
}
body.hmmessage
{
font-size: 12pt;
font-family:Calibri
}
--></style></head>
<body class='hmmessage'><div dir='ltr'><br><div>
<style><!--
.ExternalClass .ecxhmmessage P {
padding:0px;
}
.ExternalClass body.ecxhmmessage {
font-size:12pt;
font-family:Calibri;
}
--></style><div dir="ltr">Dear All,<br><br>Kindly help me, I have trying a lot but not finding my answer, I am writing SDO but it is not working, below is the code,<br><br>In this code with the help of state of the drive I am willing to set my drive in operational state using state machine theory.<br><br><br> <br> switch(state_of_the_drive)<br> {<br> case S_SWITCH_ON_DISABLED:<br> printf("S_SWITCH_ON_DISABLED\n");<br> //EC_WRITE_U8(ecrt_sdo_request_data(sdo_cnt), 0x06);<br> //EC_WRITE_U16(domain0_output + off_epos3_cntlwd,device_control_commands[0]);<br> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0006); <font style="" color="#AC193D">// this command is the right command to send controlword data to change the device state?</font><br> // EC_WRITE_U16(ecrt_sdo_request_data(sdo_cnt), 0x0006);<br> //ecrt_master_sdo_download_complete(master,0,0x6040,&a,8,0x00000000);<br> return;<br><br> case S_READY_TO_SWITCH_ON:<br> printf("S_READY_TO_SWITCH_ON\n");<br> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0007); <br> return;<br><br> case S_SWITCHED_ON:<br> printf("S_SWITCHED_ON\n");<br> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,device_control_commands[6]); <br> return;<br><br> case S_FAULT:<br> printf("S_FAULT 1 send: %x\n",device_control_commands[7]);<br> //EC_WRITE_U16(ecrt_sdo_request_data(sdo_cnt), device_control_commands[7]);<br> ecrt_slave_config_sdo8(sc_epos3, 0x6040, 0x00,device_control_commands[7]); <br> fault_flag = 1;<br> return;<br> case S_OPERATION_ENABLE:<br> printf("S_OPERATION_ENABLE\n");<br> flag_operation = 1;<br> return;<br> }<br><br>Above code is the small part of actual code used for setting the drive in operational state. Below is the complete source code.<br><br>#include <errno.h><br>#include <signal.h><br>#include <stdio.h><br>#include <string.h><br>#include <sys/resource.h><br>#include <sys/time.h><br>#include <sys/types.h><br>#include <unistd.h><br><br>/****************************************************************************/<br><br>#include "ecrt.h"<br><br>/****************************************************************************/<br><br>// Application parameters<br>#define FREQUENCY 100<br>#define PRIORITY 1<br><br>// Optional features<br>#define CONFIGURE_PDOS 1<br>#define SDO_ACCESS 1<br><br>/****************************************************************************/<br><br>// EtherCAT<br>static ec_master_t *master = NULL;<br>static ec_master_state_t master_state = {};<br><br>static ec_domain_t *domain0 = NULL;<br>static ec_domain_state_t domain_state = {};<br><br>static ec_slave_config_t *sc_epos3 = NULL;<br>static ec_slave_config_state_t sc_epos3_state = {};<br><br>// Timer<br>static unsigned int sig_alarms = 0;<br>static unsigned int user_alarms = 0;<br><br>/****************************************************************************/<br><br>// process data<br>static uint8_t *domain0_output = NULL;<br><br>#define SLAVE_DRIVE_0 0,0<br>#define MAXON_EPOS3 0x000000fb,0x64400000<br><br>// <br><br>// offsets for PDO entries<br><br>static unsigned int off_epos3_cntlwd;<br>static unsigned int off_epos3_tpos;<br>static unsigned int off_epos3_off_pos;<br>static unsigned int off_epos3_off_vel;<br>static unsigned int off_epos3_off_toq;<br>static unsigned int off_epos3_moo;<br>static unsigned int off_epos3_dof;<br>static unsigned int off_epos3_tpf;<br><br>static unsigned int off_epos3_status;<br>static unsigned int off_epos3_pos_val;<br>static unsigned int off_epos3_vel_val;<br>static unsigned int off_epos3_toq_val;<br>static unsigned int off_epos3_mood;<br>static unsigned int off_epos3_dif;<br>static unsigned int off_epos3_tps;<br>static unsigned int off_epos3_tpp1pv;<br>static unsigned int off_epos3_tpp1nv;<br><br><br>const static ec_pdo_entry_reg_t domain0_regs[] = {<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6040, 0, &off_epos3_cntlwd}, // U16<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x607a, 0, &off_epos3_tpos}, // S32<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60b0, 0, &off_epos3_off_pos}, // S32<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60b1, 0, &off_epos3_off_vel}, // S32<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60b2, 0, &off_epos3_off_toq}, // S16<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6060, 0, &off_epos3_moo}, // S8<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x2078, 1, &off_epos3_dof}, // U16<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60b8, 0, &off_epos3_tpf}, // U16<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6041, 0, &off_epos3_status}, //<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6064, 0, &off_epos3_pos_val}, //<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x606c, 0, &off_epos3_vel_val}, //<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6077, 0, &off_epos3_toq_val}, //<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6061, 0, &off_epos3_mood}, //<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x2071, 1, &off_epos3_dif}, //<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60b9, 0, &off_epos3_tps}, //<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60ba, 0, &off_epos3_tpp1pv}, //<br> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60bb, 0, &off_epos3_tpp1nv}, //<br> {}<br>};<br><br><br>static unsigned int counter = 0;<br>static unsigned int fault_flag = 0;<br>static unsigned int state_of_the_drive = 0;<br>static unsigned int flag_operation = 0;<br><br>const static unsigned int state_table[12] = {<br> 0x0000, // Start<br> 0x0100, // Not Ready to Switch On<br> 0x0140, // Switch On Disabled<br> 0x0121, // Ready to Switch On<br> 0x0123, // Switched On<br> 0x4123, // Refresh<br> 0x4133, // Measure Init<br> 0x0137, // Operation Enable<br> 0x0117, // Quickstop Active<br> 0x010f, // Fault Reaction Active (disabled)<br> 0x011f, // Fault Reaction Active (enabled)<br> 0x0108 // Fault<br>};<br><br>#define S_START 0x0000<br>#define S_NOT_READY_TO_SWITCH_ON 0x0100<br>#define S_SWITCH_ON_DISABLED 0x0140<br>#define S_READY_TO_SWITCH_ON 0x0121<br>#define S_SWITCHED_ON 0x0123<br>#define S_REFRESH 0x4123<br>#define S_MEASURE_INIT 0x4133<br>#define S_OPERATION_ENABLE 0x0137<br>#define S_QUICKSTOP_ACTIVE 0x0117<br>#define S_FAULT_REACTION_ACTIVE_D 0x010f<br>#define S_FAULT_REACTION_ACTIVE_E 0x011f<br>#define S_FAULT 0x0108<br><br>const static unsigned int device_control_commands[9] ={<br> 0x06, // Shutdown 0xxx x110 2, 6, 8<br> 0x07, // Switch On 0xxx x111 3<br> 0x0f, // Switch On & Enable Operation 0xxx 1111 3, 4<br> 0x00, // Disable Voltage 0xxx xx0x 7, 9, 10, 12<br> 0x02, // Quickstop 0xxx x01x 7, 10, 11<br> 0x07, // Disable Operation 0xxx 0111 5<br> 0x0f, // Enable Operation 0xxx 1111 4, 16<br> 0x00, // Fault Reset 0xxx xxxx -> 1xxx xxxx 15<br> 0x80 // Fault Reset 0xxx xxxx -> 1xxx xxxx 15<br>};<br>const uint8_t a=0x06;<br>/*****************************************************************************/<br><br>#if CONFIGURE_PDOS<br><br>/* Master 0, Slave 0, "EPOS3" <br> * Vendor ID: 0x000000fb <br> * Product code: 0x64400000 <br> * Revision number: 0x22100000 <br> */ <br><br>ec_pdo_entry_info_t slave_0_pdo_entries[] = { <br> {0x6040, 0x00, 16}, /* 0x6040:00 */ <br> {0x607a, 0x00, 32}, /* 0x607A:00 */ <br> {0x60b0, 0x00, 32}, /* 0x60B0:00 */ <br> {0x60b1, 0x00, 32}, /* 0x60B1:00 */ <br> {0x60b2, 0x00, 16}, /* 0x60B2:00 */ <br> {0x6060, 0x00, 8}, /* 0x6060:00 */ <br> {0x2078, 0x01, 16}, /* 0x2078:01 */ <br> {0x60b8, 0x00, 16}, /* 0x60B8:00 */ <br> {0x6041, 0x00, 16}, /* 0x6041:00 */ <br> {0x6064, 0x00, 32}, /* 0x6064:00 */ <br> {0x606c, 0x00, 32}, /* 0x606C:00 */ <br> {0x6077, 0x00, 16}, /* 0x6077:00 */ <br> {0x6061, 0x00, 8}, /* 0x6061:00 */ <br> {0x2071, 0x01, 16}, /* 0x2071:01 */ <br> {0x60b9, 0x00, 16}, /* 0x60B9:00 */ <br> {0x60ba, 0x00, 32}, /* 0x60BA:00 */ <br> {0x60bb, 0x00, 32}, /* 0x60BB:00 */ <br>}; <br><br>ec_pdo_info_t slave_0_pdos[] = { <br> {0x1600, 8, slave_0_pdo_entries + 0}, /* 1st receive PDO Mapping */ <br> {0x1a00, 9, slave_0_pdo_entries + 8}, /* 1st transmit PDO Mapping */ <br>}; <br><br>ec_sync_info_t epos3_syncs[] = { <br> {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, <br> {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, <br> {2, EC_DIR_OUTPUT, 1, slave_0_pdos + 0, EC_WD_ENABLE}, <br> {3, EC_DIR_INPUT, 1, slave_0_pdos + 1, EC_WD_DISABLE}, <br> {0xff} <br>};<br>#endif<br><br>/*****************************************************************************/<br><br>#if SDO_ACCESS<br>static ec_sdo_request_t *sdo;<br>static ec_sdo_request_t *sdo_cnt;<br>#endif<br><br>/*****************************************************************************/<br><br>void check_domain_state(ec_domain_t *domain)<br>{<br> ec_domain_state_t ds;<br><br> ecrt_domain_state(domain, &ds);<br><br> if (ds.working_counter != domain_state.working_counter)<br> printf("Domain: WC %u.\n", ds.working_counter);<br> if (ds.wc_state != domain_state.wc_state)<br> printf("Domain: State %u.\n", ds.wc_state);<br><br> domain_state = ds;<br>}<br><br>/*****************************************************************************/<br><br>void check_master_state(void)<br>{<br> ec_master_state_t ms;<br><br> ecrt_master_state(master, &ms);<br><br> if (ms.slaves_responding != master_state.slaves_responding)<br> printf("%u slave(s).\n", ms.slaves_responding);<br> if (ms.al_states != master_state.al_states)<br> printf("AL states: 0x%02X.\n", ms.al_states);<br> if (ms.link_up != master_state.link_up)<br> printf("Link is %s.\n", ms.link_up ? "up" : "down");<br><br> master_state = ms;<br>}<br><br>/*****************************************************************************/<br><br>void check_slave_config_states(void)<br>{<br> ec_slave_config_state_t s;<br><br> ecrt_slave_config_state(sc_epos3, &s);<br><br> if (s.al_state != sc_epos3_state.al_state)<br> printf("EPOS3 slave 0 State 0x%02X.\n", s.al_state);<br> if (s.online != sc_epos3_state.online)<br> printf("EPOS3 slave 0: %s.\n", s.online ? "online" : "offline");<br> if (s.operational != sc_epos3_state.operational)<br> printf("EPOS3 slave 0: %soperational.\n",<br> s.operational ? "" : "Not ");<br> sc_epos3_state = s;<br>}<br><br>/*****************************************************************************/<br><br>#if SDO_ACCESS<br>void read_sdo(void)<br>{<br> switch (ecrt_sdo_request_state(sdo)) {<br> case EC_REQUEST_UNUSED: // request was not used yet<br> ecrt_sdo_request_read(sdo); // trigger first read<br> break;<br> case EC_REQUEST_BUSY:<br> //fprintf(stderr, "Still busy...\n");<br> break;<br> case EC_REQUEST_SUCCESS:<br> state_of_the_drive = EC_READ_U16(ecrt_sdo_request_data(sdo));<br> fprintf(stderr, "SDO value: 0x%04X\n",state_of_the_drive);<br> ecrt_sdo_request_read(sdo); // trigger next read<br> break;<br> case EC_REQUEST_ERROR:<br> fprintf(stderr, "Failed to read SDO!\n");<br> ecrt_sdo_request_read(sdo); // retry reading<br> break;<br> }<br>}<br><br>#endif<br><br>#if 1<br>void move_state_machine(void)<br>{<br> int i=0;<br> unsigned int abc = 0;<br> if (fault_flag == 1)<br> {<br> printf("S_FAULT 2 send: %x\n",device_control_commands[8]);<br> //EC_WRITE_U8(ecrt_sdo_request_data(sdo_cnt), device_control_commands[8]);<br> ecrt_slave_config_sdo8(sc_epos3, 0x6040, 0x00,device_control_commands[7]); <br> ecrt_slave_config_sdo8(sc_epos3, 0x6040, 0x00,device_control_commands[8]); <br> fault_flag = 0;<br> return;<br> }<br><br> if (state_of_the_drive & 0x8000)<br> state_of_the_drive = state_of_the_drive ^ 0x8000;<br> if (state_of_the_drive & 0x0200)<br> state_of_the_drive = state_of_the_drive ^ 0x0200;<br> if (state_of_the_drive & 0x1000)<br> state_of_the_drive = state_of_the_drive ^ 0x1000;<br> if (state_of_the_drive & 0x0800)<br> state_of_the_drive = state_of_the_drive ^ 0x0800;<br> if (state_of_the_drive & 0x0400)<br> state_of_the_drive = state_of_the_drive ^ 0x0400;<br> if (state_of_the_drive & 0x0200)<br> state_of_the_drive = state_of_the_drive ^ 0x0200;<br> if (state_of_the_drive & 0x0080)<br> state_of_the_drive = state_of_the_drive ^ 0x0080;<br><br> //abc = state_of_the_drive & state_table[i];<br> printf("state_of_the_drive :%x\n",state_of_the_drive);<br> switch(state_of_the_drive)<br> {<br> case S_SWITCH_ON_DISABLED:<br> printf("S_SWITCH_ON_DISABLED\n");<br> //EC_WRITE_U8(ecrt_sdo_request_data(sdo_cnt), 0x06);<br> //EC_WRITE_U16(domain0_output + off_epos3_cntlwd,device_control_commands[0]);<br> // <br> usleep(100);<br> //printf("%d",ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0006));<br> //EC_WRITE_U1d6(ecrt_sdo_request_data(sdo_cnt), 0x0006);<br> //ecrt_master_sdo_download_complete(master,0,0x6040,&a,8,0x00000000);<br> ecrt_slave_config_sdo(sc_epos3, 0x6040, 0x00, &a, sizeof(a));<br><br><br> return;<br><br> case S_READY_TO_SWITCH_ON:<br> printf("S_READY_TO_SWITCH_ON\n");<br> // ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0007); <br> printf("%d",ecrt_slave_config_sdo(sc_epos3, 0x6040, 0x00,0x0007)); <br> return;<br><br> case S_SWITCHED_ON:<br> printf("S_SWITCHED_ON\n");<br> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,device_control_commands[6]); <br> return;<br><br> case S_FAULT:<br> printf("S_FAULT 1 send: %x\n",device_control_commands[7]);<br> //EC_WRITE_U16(ecrt_sdo_request_data(sdo_cnt), device_control_commands[7]);<br> ecrt_slave_config_sdo8(sc_epos3, 0x6040, 0x00,device_control_commands[7]); <br> fault_flag = 1;<br> return;<br> case S_OPERATION_ENABLE:<br> printf("S_OPERATION_ENABLE\n");<br> flag_operation = 1;<br> return;<br> }<br> <br>}<br>#endif<br>/****************************************************************************/<br><br>void cyclic_task(){<br> int i;<br> unsigned int data_input=0;<br><br> // receive process data<br> ecrt_master_receive(master);<br> ecrt_domain_process(domain0);<br><br> // check process data state (optional)<br> //check_domain_state(domain0);<br><br> if (counter) {<br> counter--;<br> }else { // do this at 1 Hz<br> counter = FREQUENCY;<br><br> // check for master state (optional)<br> //check_master_state();<br><br> // check for islave configuration state(s) (optional)<br> //check_slave_config_states();<br><br>#if SDO_ACCESS<br> // read process data SDO<br> /*<br> read_sdo();<br> //printf("read_sdo\n");<br> printf("receive :%x ",state_of_the_drive);<br><br> // read process data<br> if (flag_operation == 1)<br> {<br> flag_operation = 0;<br> printf("e_Operation\n");<br><br> ecrt_slave_config_sdo16(sc_epos3, 0x607a, 0x00,0x00ff);<br><br> }else<br> {<br> move_state_machine();<br> }<br> */<br> <br>#endif<br> }<br> ecrt_slave_config_sdo16(sc_epos3, 0x607a, 0x00,0x0000);<br> // send process data<br> ecrt_domain_queue(domain0);<br> ecrt_master_send(master);<br>}<br><br>/****************************************************************************/<br><br>void signal_handler(int signum) {<br> switch (signum) {<br> case SIGALRM:<br> sig_alarms++;<br> break;<br> }<br>}<br><br>/****************************************************************************/<br><br>int main(int argc, char **argv)<br>{<br> ec_slave_config_t *sc;<br> struct sigaction sa;<br> struct itimerval tv;<br><br> master = ecrt_request_master(0);<br> printf("ecrt_request_master is called \n");<br> if (!master)<br> return -1;<br><br> domain0 = ecrt_master_create_domain(master);<br> if(!domain0)<br> return -1;<br><br> if(!(sc_epos3 = ecrt_master_slave_config(<br> master, SLAVE_DRIVE_0, MAXON_EPOS3))){<br> fprintf(stderr, "Failed to get slave configuration. \n");<br> return -1;<br> }<br>/*<br> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0006);<br> usleep(5000);<br> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0007);<br> usleep(5000);<br> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x000f);<br> usleep(5000);<br> */<br> //ecrt_slave_config_sdo16(sc_epos3, 0x6060, 0x00,0x0008);<br> //added by kbkbc<br> //if (ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,9 ) == 0)<br> //printf("change SDO : 0x6040 \n");<br><br><br>#if SDO_ACCESS<br> fprintf(stderr, "Creating SDO requests...\n");<br> if (!(sdo = ecrt_slave_config_create_sdo_request(sc_epos3, 0x6041,<br>0x00, 16))) {<br> fprintf(stderr, "Failed to create SDO request.\n");<br> return -1;<br> }<br> if (!(sdo_cnt = ecrt_slave_config_create_sdo_request(sc_epos3, 0x6040,<br>0x00, 8))) {<br> fprintf(stderr, "Failed to create SDO request.\n");<br> return -1;<br> }<br><br> <br> ecrt_slave_config_sdo16(sc_epos3, 0x6060, 0x00,0x0008);<br> //usleep(5000);<br> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0006);<br> //usleep(5000);<br> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0007);<br> //usleep(5000);<br> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x000f);<br> <br><br><br> while(1)<br> {<br> read_sdo();<br> <br> //printf("read_sdo\n");<br> printf("receive :%x ",state_of_the_drive);<br><br> // read process data<br> if (state_of_the_drive == state_table[8])<br> {<br> printf("e_Operation\n");<br> while(1);<br> }else<br> {<br> move_state_machine();<br> }<br> usleep(100000);<br> }<br> <br> //ecrt_sdo_request_timeout(sdo, 10); // ms<br>#endif<br><br>#if CONFIGURE_PDOS<br> printf("Configuring PDOs...\n");<br> if (ecrt_slave_config_pdos(sc_epos3, EC_END, epos3_syncs)) {<br> fprintf(stderr, "Failed to configure PDOs.\n");<br> return -1;<br> }<br> printf("configureing PDO is completed!\n");<br>#endif<br> if( ecrt_domain_reg_pdo_entry_list(domain0, domain0_regs)){<br> fprintf(stderr, "PDO entty registration filed! \n");<br> return -1;<br> }<br><br> printf("Activating master...\n");<br> if (ecrt_master_activate(master))<br> return -1;<br><br> if( !(domain0_output = ecrt_domain_data(domain0))) {<br> return -1;<br> }<br><br>#if PRIORITY<br> pid_t pid = getpid();<br> if (setpriority(PRIO_PROCESS, pid, -19))<br> fprintf(stderr, "Warning: Failed to set priority: %s\n",<br> strerror(errno));<br>#endif<br><br> sa.sa_handler = signal_handler;<br> sigemptyset(&sa.sa_mask);<br> sa.sa_flags = 0;<br> if (sigaction(SIGALRM, &sa, 0)) {<br> fprintf(stderr, "Failed to install signal handler!\n");<br> return -1;<br> }<br><br> printf("Starting timer...\n");<br> tv.it_interval.tv_sec = 0;<br> tv.it_interval.tv_usec = 1000000 / FREQUENCY;<br> tv.it_value.tv_sec = 0;<br> tv.it_value.tv_usec = 1000;<br> if (setitimer(ITIMER_REAL, &tv, NULL)) {<br> fprintf(stderr, "Failed to start timer: %s\n", strerror(errno));<br> return 1;<br> }<br><br> printf("Started.\n");<br><br> while (1) {<br> //pause();<br><br> #if 0<br> struct timeval t;<br> gettimeofday(&t, NULL);<br> printf("%u.%06u\n", t.tv_sec, t.tv_usec);<br> #endif<br> while (sig_alarms != user_alarms) {<br><br> cyclic_task();<br> user_alarms++;<br> }<br> }<br><br> return 0;<br>}<br><br> </div></div> </div></body>
</html>