[etherlab-users] SDO is not writting/device is not changing its state
Richard Hacker
ha at igh.de
Thu Dec 5 08:27:25 CET 2013
The ecrt_sdo_config_*() family work _before_ you go into cyclic
operational state.
If you want to change an sdo value during operational state, you need:
* ecrt_slave_config_create_sdo_request() in pre-cyclic state to get a
request handler
* in operational state you use ecrt_sdo_request_*() family to send data
to or receive data from slaves. Note that these operations are not
blocking and could take any number of EtherCAT cycles, therefore you
have an ecrt_sdo_request_state() operation to test the handler's
transactional state.
Note that the states I am talking about are the EtherCAT Slave<->Master
states, not the state of the drive itself!
- Richard
On 12/05/2013 06:06 AM, Muhammad Nabeel wrote:
>
> Dear All,
>
> 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,
>
> 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.
>
>
>
> switch(state_of_the_drive)
> {
> case S_SWITCH_ON_DISABLED:
> printf("S_SWITCH_ON_DISABLED\n");
> //EC_WRITE_U8(ecrt_sdo_request_data(sdo_cnt), 0x06);
> //EC_WRITE_U16(domain0_output +
> off_epos3_cntlwd,device_control_commands[0]);
> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0006);
> // this command is the right command to send controlword data to change
> the device state?
> // EC_WRITE_U16(ecrt_sdo_request_data(sdo_cnt), 0x0006);
>
> //ecrt_master_sdo_download_complete(master,0,0x6040,&a,8,0x00000000);
> return;
>
> case S_READY_TO_SWITCH_ON:
> printf("S_READY_TO_SWITCH_ON\n");
> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0007);
> return;
>
> case S_SWITCHED_ON:
> printf("S_SWITCHED_ON\n");
> ecrt_slave_config_sdo16(sc_epos3, 0x6040,
> 0x00,device_control_commands[6]);
> return;
>
> case S_FAULT:
> printf("S_FAULT 1 send: %x\n",device_control_commands[7]);
> //EC_WRITE_U16(ecrt_sdo_request_data(sdo_cnt),
> device_control_commands[7]);
> ecrt_slave_config_sdo8(sc_epos3, 0x6040,
> 0x00,device_control_commands[7]);
> fault_flag = 1;
> return;
> case S_OPERATION_ENABLE:
> printf("S_OPERATION_ENABLE\n");
> flag_operation = 1;
> return;
> }
>
> Above code is the small part of actual code used for setting the drive
> in operational state. Below is the complete source code.
>
> #include <errno.h>
> #include <signal.h>
> #include <stdio.h>
> #include <string.h>
> #include <sys/resource.h>
> #include <sys/time.h>
> #include <sys/types.h>
> #include <unistd.h>
>
> /****************************************************************************/
>
> #include "ecrt.h"
>
> /****************************************************************************/
>
> // Application parameters
> #define FREQUENCY 100
> #define PRIORITY 1
>
> // Optional features
> #define CONFIGURE_PDOS 1
> #define SDO_ACCESS 1
>
> /****************************************************************************/
>
> // EtherCAT
> static ec_master_t *master = NULL;
> static ec_master_state_t master_state = {};
>
> static ec_domain_t *domain0 = NULL;
> static ec_domain_state_t domain_state = {};
>
> static ec_slave_config_t *sc_epos3 = NULL;
> static ec_slave_config_state_t sc_epos3_state = {};
>
> // Timer
> static unsigned int sig_alarms = 0;
> static unsigned int user_alarms = 0;
>
> /****************************************************************************/
>
> // process data
> static uint8_t *domain0_output = NULL;
>
> #define SLAVE_DRIVE_0 0,0
> #define MAXON_EPOS3 0x000000fb,0x64400000
>
> //
>
> // offsets for PDO entries
>
> static unsigned int off_epos3_cntlwd;
> static unsigned int off_epos3_tpos;
> static unsigned int off_epos3_off_pos;
> static unsigned int off_epos3_off_vel;
> static unsigned int off_epos3_off_toq;
> static unsigned int off_epos3_moo;
> static unsigned int off_epos3_dof;
> static unsigned int off_epos3_tpf;
>
> static unsigned int off_epos3_status;
> static unsigned int off_epos3_pos_val;
> static unsigned int off_epos3_vel_val;
> static unsigned int off_epos3_toq_val;
> static unsigned int off_epos3_mood;
> static unsigned int off_epos3_dif;
> static unsigned int off_epos3_tps;
> static unsigned int off_epos3_tpp1pv;
> static unsigned int off_epos3_tpp1nv;
>
>
> const static ec_pdo_entry_reg_t domain0_regs[] = {
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6040, 0, &off_epos3_cntlwd},
> // U16
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x607a, 0,
> &off_epos3_tpos}, // S32
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60b0, 0,
> &off_epos3_off_pos}, // S32
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60b1, 0,
> &off_epos3_off_vel}, // S32
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60b2, 0,
> &off_epos3_off_toq}, // S16
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6060, 0, &off_epos3_moo},
> // S8
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x2078, 1, &off_epos3_dof},
> // U16
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60b8, 0, &off_epos3_tpf},
> // U16
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6041, 0,
> &off_epos3_status}, //
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6064, 0,
> &off_epos3_pos_val}, //
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x606c, 0,
> &off_epos3_vel_val}, //
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6077, 0,
> &off_epos3_toq_val}, //
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x6061, 0,
> &off_epos3_mood}, //
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x2071, 1,
> &off_epos3_dif}, //
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60b9, 0, &off_epos3_tps}, //
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60ba, 0, &off_epos3_tpp1pv}, //
> {SLAVE_DRIVE_0, MAXON_EPOS3, 0x60bb, 0, &off_epos3_tpp1nv}, //
> {}
> };
>
>
> static unsigned int counter = 0;
> static unsigned int fault_flag = 0;
> static unsigned int state_of_the_drive = 0;
> static unsigned int flag_operation = 0;
>
> const static unsigned int state_table[12] = {
> 0x0000, // Start
> 0x0100, // Not Ready to Switch On
> 0x0140, // Switch On Disabled
> 0x0121, // Ready to Switch On
> 0x0123, // Switched On
> 0x4123, // Refresh
> 0x4133, // Measure Init
> 0x0137, // Operation Enable
> 0x0117, // Quickstop Active
> 0x010f, // Fault Reaction Active (disabled)
> 0x011f, // Fault Reaction Active (enabled)
> 0x0108 // Fault
> };
>
> #define S_START 0x0000
> #define S_NOT_READY_TO_SWITCH_ON 0x0100
> #define S_SWITCH_ON_DISABLED 0x0140
> #define S_READY_TO_SWITCH_ON 0x0121
> #define S_SWITCHED_ON 0x0123
> #define S_REFRESH 0x4123
> #define S_MEASURE_INIT 0x4133
> #define S_OPERATION_ENABLE 0x0137
> #define S_QUICKSTOP_ACTIVE 0x0117
> #define S_FAULT_REACTION_ACTIVE_D 0x010f
> #define S_FAULT_REACTION_ACTIVE_E 0x011f
> #define S_FAULT 0x0108
>
> const static unsigned int device_control_commands[9] ={
> 0x06, // Shutdown 0xxx x110 2, 6, 8
> 0x07, // Switch On 0xxx x111 3
> 0x0f, // Switch On & Enable Operation 0xxx 1111 3, 4
> 0x00, // Disable Voltage 0xxx xx0x 7, 9, 10, 12
> 0x02, // Quickstop 0xxx x01x 7, 10, 11
> 0x07, // Disable Operation 0xxx 0111 5
> 0x0f, // Enable Operation 0xxx 1111 4, 16
> 0x00, // Fault Reset 0xxx xxxx -> 1xxx
> xxxx 15
> 0x80 // Fault Reset 0xxx xxxx -> 1xxx
> xxxx 15
> };
> const uint8_t a=0x06;
> /*****************************************************************************/
>
> #if CONFIGURE_PDOS
>
> /* Master 0, Slave 0, "EPOS3"
> * Vendor ID: 0x000000fb
> * Product code: 0x64400000
> * Revision number: 0x22100000
> */
>
> ec_pdo_entry_info_t slave_0_pdo_entries[] = {
> {0x6040, 0x00, 16}, /* 0x6040:00 */
> {0x607a, 0x00, 32}, /* 0x607A:00 */
> {0x60b0, 0x00, 32}, /* 0x60B0:00 */
> {0x60b1, 0x00, 32}, /* 0x60B1:00 */
> {0x60b2, 0x00, 16}, /* 0x60B2:00 */
> {0x6060, 0x00, 8}, /* 0x6060:00 */
> {0x2078, 0x01, 16}, /* 0x2078:01 */
> {0x60b8, 0x00, 16}, /* 0x60B8:00 */
> {0x6041, 0x00, 16}, /* 0x6041:00 */
> {0x6064, 0x00, 32}, /* 0x6064:00 */
> {0x606c, 0x00, 32}, /* 0x606C:00 */
> {0x6077, 0x00, 16}, /* 0x6077:00 */
> {0x6061, 0x00, 8}, /* 0x6061:00 */
> {0x2071, 0x01, 16}, /* 0x2071:01 */
> {0x60b9, 0x00, 16}, /* 0x60B9:00 */
> {0x60ba, 0x00, 32}, /* 0x60BA:00 */
> {0x60bb, 0x00, 32}, /* 0x60BB:00 */
> };
>
> ec_pdo_info_t slave_0_pdos[] = {
> {0x1600, 8, slave_0_pdo_entries + 0}, /* 1st receive PDO Mapping */
> {0x1a00, 9, slave_0_pdo_entries + 8}, /* 1st transmit PDO Mapping */
> };
>
> ec_sync_info_t epos3_syncs[] = {
> {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
> {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
> {2, EC_DIR_OUTPUT, 1, slave_0_pdos + 0, EC_WD_ENABLE},
> {3, EC_DIR_INPUT, 1, slave_0_pdos + 1, EC_WD_DISABLE},
> {0xff}
> };
> #endif
>
> /*****************************************************************************/
>
> #if SDO_ACCESS
> static ec_sdo_request_t *sdo;
> static ec_sdo_request_t *sdo_cnt;
> #endif
>
> /*****************************************************************************/
>
> void check_domain_state(ec_domain_t *domain)
> {
> ec_domain_state_t ds;
>
> ecrt_domain_state(domain, &ds);
>
> if (ds.working_counter != domain_state.working_counter)
> printf("Domain: WC %u.\n", ds.working_counter);
> if (ds.wc_state != domain_state.wc_state)
> printf("Domain: State %u.\n", ds.wc_state);
>
> domain_state = ds;
> }
>
> /*****************************************************************************/
>
> void check_master_state(void)
> {
> ec_master_state_t ms;
>
> ecrt_master_state(master, &ms);
>
> if (ms.slaves_responding != master_state.slaves_responding)
> printf("%u slave(s).\n", ms.slaves_responding);
> if (ms.al_states != master_state.al_states)
> printf("AL states: 0x%02X.\n", ms.al_states);
> if (ms.link_up != master_state.link_up)
> printf("Link is %s.\n", ms.link_up ? "up" : "down");
>
> master_state = ms;
> }
>
> /*****************************************************************************/
>
> void check_slave_config_states(void)
> {
> ec_slave_config_state_t s;
>
> ecrt_slave_config_state(sc_epos3, &s);
>
> if (s.al_state != sc_epos3_state.al_state)
> printf("EPOS3 slave 0 State 0x%02X.\n", s.al_state);
> if (s.online != sc_epos3_state.online)
> printf("EPOS3 slave 0: %s.\n", s.online ? "online" : "offline");
> if (s.operational != sc_epos3_state.operational)
> printf("EPOS3 slave 0: %soperational.\n",
> s.operational ? "" : "Not ");
> sc_epos3_state = s;
> }
>
> /*****************************************************************************/
>
> #if SDO_ACCESS
> void read_sdo(void)
> {
> switch (ecrt_sdo_request_state(sdo)) {
> case EC_REQUEST_UNUSED: // request was not used yet
> ecrt_sdo_request_read(sdo); // trigger first read
> break;
> case EC_REQUEST_BUSY:
> //fprintf(stderr, "Still busy...\n");
> break;
> case EC_REQUEST_SUCCESS:
> state_of_the_drive = EC_READ_U16(ecrt_sdo_request_data(sdo));
> fprintf(stderr, "SDO value: 0x%04X\n",state_of_the_drive);
> ecrt_sdo_request_read(sdo); // trigger next read
> break;
> case EC_REQUEST_ERROR:
> fprintf(stderr, "Failed to read SDO!\n");
> ecrt_sdo_request_read(sdo); // retry reading
> break;
> }
> }
>
> #endif
>
> #if 1
> void move_state_machine(void)
> {
> int i=0;
> unsigned int abc = 0;
> if (fault_flag == 1)
> {
> printf("S_FAULT 2 send: %x\n",device_control_commands[8]);
> //EC_WRITE_U8(ecrt_sdo_request_data(sdo_cnt),
> device_control_commands[8]);
> ecrt_slave_config_sdo8(sc_epos3, 0x6040,
> 0x00,device_control_commands[7]);
> ecrt_slave_config_sdo8(sc_epos3, 0x6040,
> 0x00,device_control_commands[8]);
> fault_flag = 0;
> return;
> }
>
> if (state_of_the_drive & 0x8000)
> state_of_the_drive = state_of_the_drive ^ 0x8000;
> if (state_of_the_drive & 0x0200)
> state_of_the_drive = state_of_the_drive ^ 0x0200;
> if (state_of_the_drive & 0x1000)
> state_of_the_drive = state_of_the_drive ^ 0x1000;
> if (state_of_the_drive & 0x0800)
> state_of_the_drive = state_of_the_drive ^ 0x0800;
> if (state_of_the_drive & 0x0400)
> state_of_the_drive = state_of_the_drive ^ 0x0400;
> if (state_of_the_drive & 0x0200)
> state_of_the_drive = state_of_the_drive ^ 0x0200;
> if (state_of_the_drive & 0x0080)
> state_of_the_drive = state_of_the_drive ^ 0x0080;
>
> //abc = state_of_the_drive & state_table[i];
> printf("state_of_the_drive :%x\n",state_of_the_drive);
> switch(state_of_the_drive)
> {
> case S_SWITCH_ON_DISABLED:
> printf("S_SWITCH_ON_DISABLED\n");
> //EC_WRITE_U8(ecrt_sdo_request_data(sdo_cnt), 0x06);
> //EC_WRITE_U16(domain0_output +
> off_epos3_cntlwd,device_control_commands[0]);
> //
> usleep(100);
> //printf("%d",ecrt_slave_config_sdo16(sc_epos3, 0x6040,
> 0x00,0x0006));
> //EC_WRITE_U1d6(ecrt_sdo_request_data(sdo_cnt), 0x0006);
>
> //ecrt_master_sdo_download_complete(master,0,0x6040,&a,8,0x00000000);
> ecrt_slave_config_sdo(sc_epos3, 0x6040, 0x00, &a,
> sizeof(a));
>
>
> return;
>
> case S_READY_TO_SWITCH_ON:
> printf("S_READY_TO_SWITCH_ON\n");
> // ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0007);
> printf("%d",ecrt_slave_config_sdo(sc_epos3, 0x6040,
> 0x00,0x0007));
> return;
>
> case S_SWITCHED_ON:
> printf("S_SWITCHED_ON\n");
> ecrt_slave_config_sdo16(sc_epos3, 0x6040,
> 0x00,device_control_commands[6]);
> return;
>
> case S_FAULT:
> printf("S_FAULT 1 send: %x\n",device_control_commands[7]);
> //EC_WRITE_U16(ecrt_sdo_request_data(sdo_cnt),
> device_control_commands[7]);
> ecrt_slave_config_sdo8(sc_epos3, 0x6040,
> 0x00,device_control_commands[7]);
> fault_flag = 1;
> return;
> case S_OPERATION_ENABLE:
> printf("S_OPERATION_ENABLE\n");
> flag_operation = 1;
> return;
> }
>
> }
> #endif
> /****************************************************************************/
>
> void cyclic_task(){
> int i;
> unsigned int data_input=0;
>
> // receive process data
> ecrt_master_receive(master);
> ecrt_domain_process(domain0);
>
> // check process data state (optional)
> //check_domain_state(domain0);
>
> if (counter) {
> counter--;
> }else { // do this at 1 Hz
> counter = FREQUENCY;
>
> // check for master state (optional)
> //check_master_state();
>
> // check for islave configuration state(s) (optional)
> //check_slave_config_states();
>
> #if SDO_ACCESS
> // read process data SDO
> /*
> read_sdo();
> //printf("read_sdo\n");
> printf("receive :%x ",state_of_the_drive);
>
> // read process data
> if (flag_operation == 1)
> {
> flag_operation = 0;
> printf("e_Operation\n");
>
> ecrt_slave_config_sdo16(sc_epos3, 0x607a, 0x00,0x00ff);
>
> }else
> {
> move_state_machine();
> }
> */
>
> #endif
> }
> ecrt_slave_config_sdo16(sc_epos3, 0x607a, 0x00,0x0000);
> // send process data
> ecrt_domain_queue(domain0);
> ecrt_master_send(master);
> }
>
> /****************************************************************************/
>
> void signal_handler(int signum) {
> switch (signum) {
> case SIGALRM:
> sig_alarms++;
> break;
> }
> }
>
> /****************************************************************************/
>
> int main(int argc, char **argv)
> {
> ec_slave_config_t *sc;
> struct sigaction sa;
> struct itimerval tv;
>
> master = ecrt_request_master(0);
> printf("ecrt_request_master is called \n");
> if (!master)
> return -1;
>
> domain0 = ecrt_master_create_domain(master);
> if(!domain0)
> return -1;
>
> if(!(sc_epos3 = ecrt_master_slave_config(
> master, SLAVE_DRIVE_0, MAXON_EPOS3))){
> fprintf(stderr, "Failed to get slave configuration. \n");
> return -1;
> }
> /*
> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0006);
> usleep(5000);
> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0007);
> usleep(5000);
> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x000f);
> usleep(5000);
> */
> //ecrt_slave_config_sdo16(sc_epos3, 0x6060, 0x00,0x0008);
> //added by kbkbc
> //if (ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,9 ) == 0)
> //printf("change SDO : 0x6040 \n");
>
>
> #if SDO_ACCESS
> fprintf(stderr, "Creating SDO requests...\n");
> if (!(sdo = ecrt_slave_config_create_sdo_request(sc_epos3, 0x6041,
> 0x00, 16))) {
> fprintf(stderr, "Failed to create SDO request.\n");
> return -1;
> }
> if (!(sdo_cnt = ecrt_slave_config_create_sdo_request(sc_epos3, 0x6040,
> 0x00, 8))) {
> fprintf(stderr, "Failed to create SDO request.\n");
> return -1;
> }
>
>
> ecrt_slave_config_sdo16(sc_epos3, 0x6060, 0x00,0x0008);
> //usleep(5000);
> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0006);
> //usleep(5000);
> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x0007);
> //usleep(5000);
> ecrt_slave_config_sdo16(sc_epos3, 0x6040, 0x00,0x000f);
>
>
>
> while(1)
> {
> read_sdo();
>
> //printf("read_sdo\n");
> printf("receive :%x ",state_of_the_drive);
>
> // read process data
> if (state_of_the_drive == state_table[8])
> {
> printf("e_Operation\n");
> while(1);
> }else
> {
> move_state_machine();
> }
> usleep(100000);
> }
>
> //ecrt_sdo_request_timeout(sdo, 10); // ms
> #endif
>
> #if CONFIGURE_PDOS
> printf("Configuring PDOs...\n");
> if (ecrt_slave_config_pdos(sc_epos3, EC_END, epos3_syncs)) {
> fprintf(stderr, "Failed to configure PDOs.\n");
> return -1;
> }
> printf("configureing PDO is completed!\n");
> #endif
> if( ecrt_domain_reg_pdo_entry_list(domain0, domain0_regs)){
> fprintf(stderr, "PDO entty registration filed! \n");
> return -1;
> }
>
> printf("Activating master...\n");
> if (ecrt_master_activate(master))
> return -1;
>
> if( !(domain0_output = ecrt_domain_data(domain0))) {
> return -1;
> }
>
> #if PRIORITY
> pid_t pid = getpid();
> if (setpriority(PRIO_PROCESS, pid, -19))
> fprintf(stderr, "Warning: Failed to set priority: %s\n",
> strerror(errno));
> #endif
>
> sa.sa_handler = signal_handler;
> sigemptyset(&sa.sa_mask);
> sa.sa_flags = 0;
> if (sigaction(SIGALRM, &sa, 0)) {
> fprintf(stderr, "Failed to install signal handler!\n");
> return -1;
> }
>
> printf("Starting timer...\n");
> tv.it_interval.tv_sec = 0;
> tv.it_interval.tv_usec = 1000000 / FREQUENCY;
> tv.it_value.tv_sec = 0;
> tv.it_value.tv_usec = 1000;
> if (setitimer(ITIMER_REAL, &tv, NULL)) {
> fprintf(stderr, "Failed to start timer: %s\n", strerror(errno));
> return 1;
> }
>
> printf("Started.\n");
>
> while (1) {
> //pause();
>
> #if 0
> struct timeval t;
> gettimeofday(&t, NULL);
> printf("%u.%06u\n", t.tv_sec, t.tv_usec);
> #endif
> while (sig_alarms != user_alarms) {
>
> cyclic_task();
> user_alarms++;
> }
> }
>
> return 0;
> }
>
>
>
> _______________________________________________
> etherlab-users mailing list
> etherlab-users at etherlab.org
> http://lists.etherlab.org/mailman/listinfo/etherlab-users
>
Mit freundlichem Gruß
Richard Hacker
--
------------------------------------------------------------------------
Richard Hacker M.Sc.
richard.hacker at igh-essen.com
Tel.: +49 201 / 36014-16
Ingenieurgemeinschaft IgH
Gesellschaft für Ingenieurleistungen mbH
Heinz-Bäcker-Str. 34
D-45356 Essen
Amtsgericht Essen HRB 11500
USt-Id.-Nr.: DE 174 626 722
Geschäftsführung:
- Dr.-Ing. T. Finke,
- Dr.-Ing. W. Hagemeister
Tel.: +49 201 / 360-14-0
http://www.igh-essen.com
------------------------------------------------------------------------
More information about the Etherlab-users
mailing list