search

synapticon / Etherlab_EtherCAT_Master

Clone of IgH EtherCAT Master with additional Vectioneer patches.
GNU Lesser General Public License v2.1
81 stars 54 forks source link

why igh master can not controll the servo motor? #16

Closed liuzq71 closed 5 years ago

liuzq71 commented 5 years ago

the code below can not controll the INOVANCE IS620N and panasonic Minas A6B,can someone help me?

/*****

include

include

include

include

include <sys/resource.h>

include <sys/time.h>

include <sys/types.h>

include

include <sys/mman.h>

/****/

include "ecrt.h"

/****/

define SERVO_STAT_SWION_DIS 0x250 /switched on disabled/

define SERVO_STAT_RDY_SWION 0x231 /ready to switch on/

define SERVO_STAT_SWION_ENA 0x233 /switch on enabled/

define SERVO_STAT_OP_ENA 0x237 /operation enable/

define SERVO_STAT_ERROR 0x218 /error/

/Application Parameters/

define TASK_FREQUENCY 100 /Hz/

define TIMOUT_CLEAR_ERROR (1TASK_FREQUENCY) /clearing error timeout*/

define TARGET_VELOCITY 10241024 //8388608 /target velocity*/

define PROFILE_VELOCITY 3 /Operation mode for 0x6060:0/

/*****/

/EtherCAT/ static ec_master_t *master = NULL; static ec_master_state_t master_state = {};

static ec_domain_t *domain1 = NULL; static ec_domain_state_t domain1_state = {};

static ec_slave_config_t *sc_IS620N; static ec_slave_config_state_t sc_IS620N_state = {};

/****/

/Process Data/ static uint8_t *domain1_pd = NULL;

define IS620NSlavePos 0,0 /EtherCAT address on the bus/

define INOVANCE_IS620N 0x00100000, 0x000C0108 /IS620N Vendor ID, product code/

/x86_64: char-1byte,int-4bytes,long-8bytes/ struct IS620N_offset { //RxPDO unsigned int control_word; //0x6040:控制字,subindex:0,bitlen:16 unsigned int target_position;//0x607A:目标位置,subindex:0,bitlen:32 unsigned int touch_probe; //0x60B8:探针,subindex:0,bitlen:16 unsigned int pysical_outputs;//0x60FE:pysical_outputs,subindex:1,bitlen:32 unsigned int target_velocity;//0x60FF:target_velocity,subindex:0,bitlen:32 unsigned int target_torque;//0x6071:int target_torque,subindex:0,bitlen:16 unsigned int modes_operation;//0x6060:Modes of operation,subindex:0,bitlen:8 unsigned int max_profile_velocity;//0x607F:max_profile_velocity,subindex:0,bitlen:32 unsigned int positive_torque_limit_value;//0x60E0:positive_torque_limit_value,subindex:0,bitlen:16 unsigned int negative_torque_limit_value;//0x60E1:negaitive_torque_limit_value,subindex:0,bitlen:16 unsigned int torque_offset;//0x60B2:torque offset,subindex:0,bitlen:16

//TxPDo unsigned int status_word;//0x6041:status_word,subindex:0,bitlen:16 unsigned int position_actual_value;//0x6064:position_actual_value,subindex:0,bitlen:32 unsigned int touch_probe_status;//0x60B9,subindex:0,bitlen:16 unsigned int touch_probe_pos1_pos_value;//0x60BA,subindex:0,bitlen:32 unsigned int touch_probe_pos2_pos_value;//0x60BC ,subindex:0,bitlen:32 unsigned int error_code;//0x603F,subindex:0,bitlen:16 unsigned int digital_inputs;//0x60FD,subindex:0,bitlen:32 unsigned int torque_actual_value;//0x6077,subindex:0,bitlen:16 unsigned int following_error_actual_value;//0x60F4,subindex:0,bitlen:32 unsigned int modes_of_operation_display;//0x6061,subindex:0,bitlen:8 unsigned int velocity_actual_value;//0x606C,subindex:0,bitlen:32 };

static struct IS620N_offset offset;

const static ec_pdo_entry_reg_t domain1_regs[] = { //RxPDO {IS620NSlavePos, INOVANCE_IS620N, 0x6040, 0, &offset.control_word}, {IS620NSlavePos, INOVANCE_IS620N, 0x607A, 0, &offset.target_position}, {IS620NSlavePos, INOVANCE_IS620N, 0x60FF, 0, &offset.target_velocity}, {IS620NSlavePos, INOVANCE_IS620N, 0x6071, 0, &offset.target_torque}, {IS620NSlavePos, INOVANCE_IS620N, 0x6060, 0, &offset.modes_operation}, {IS620NSlavePos, INOVANCE_IS620N, 0x60B8, 0, &offset.touch_probe}, {IS620NSlavePos, INOVANCE_IS620N, 0x607F, 0, &offset.max_profile_velocity}, //TxPDO {IS620NSlavePos, INOVANCE_IS620N, 0x603F, 0, &offset.error_code}, {IS620NSlavePos, INOVANCE_IS620N, 0x6041, 0, &offset.status_word}, {IS620NSlavePos, INOVANCE_IS620N, 0x6064, 0, &offset.position_actual_value}, {IS620NSlavePos, INOVANCE_IS620N, 0x6077, 0, &offset.torque_actual_value}, {IS620NSlavePos, INOVANCE_IS620N, 0x6061, 0, &offset.modes_of_operation_display}, {IS620NSlavePos, INOVANCE_IS620N, 0x60B9, 0, &offset.touch_probe_status}, {IS620NSlavePos, INOVANCE_IS620N, 0x60BA, 0, &offset.touch_probe_pos1_pos_value}, {IS620NSlavePos, INOVANCE_IS620N, 0x60BC, 0, &offset.touch_probe_pos2_pos_value}, {IS620NSlavePos, INOVANCE_IS620N, 0x60FD, 0, &offset.digital_inputs}, {} };

static unsigned int counter = 0; //static int state = -500;

/重新映射PDO才需要定义下面这3个数组,如果使用servo的固定映射则不需要/ /Config PDOs/ static ec_pdo_entry_info_t IS620N_pdo_entries[] = { //RxPdo 0x1702 {0x6040, 0x00, 16}, {0x607A, 0x00, 32}, {0x60FF, 0x00, 32}, {0x6071, 0x00, 16}, {0x6060, 0x00, 8}, {0x60B8, 0x00, 16}, {0x607F, 0x00, 32}, //TxPdo 0x1B02 {0x603F, 0x00, 16}, {0x6041, 0x00, 16}, {0x6064, 0x00, 32}, {0x6077, 0x00, 16}, {0x6061, 0x00, 8}, {0x60B9, 0x00, 16}, {0x60BA, 0x00, 32}, {0x60BC, 0x00, 32}, {0x60FD, 0x00, 32}, };

static ec_pdo_info_t IS620N_pdos[] = { //RxPdo //{0x1600, 3, IS620N_pdo_entries + 0 }, {0x1600, 7, IS620N_pdo_entries + 0 }, //TxPdo //{0x1A00, 8, IS620N_pdo_entries + 3 } {0x1A00, 9, IS620N_pdo_entries + 7 } };

static ec_sync_info_t IS620N_syncs[] = { { 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE }, { 1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE }, { 2, EC_DIR_OUTPUT, 1, IS620N_pdos + 0, EC_WD_DISABLE }, { 3, EC_DIR_INPUT, 1, IS620N_pdos + 1, EC_WD_DISABLE }, { 0xFF} };

/*****/

void check_domain1_state(void) { ec_domain_state_t ds; ecrt_domain_state(domain1, &ds); if (ds.working_counter != domain1_state.working_counter) printf("Domain1: WC %u.\n", ds.working_counter);

if (ds.wc_state != domain1_state.wc_state)
   printf("Domain1: State %u.\n", ds.wc_state);

domain1_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_IS620N, &s); if (s.al_state != sc_IS620N_state.al_state) printf("IS620N: State 0x%02X.\n", s.al_state);

if (s.online != sc_IS620N_state.online)
   printf("IS620N: %s.\n", s.online ? "online" : "offline");

if (s.operational != sc_IS620N_state.operational)
   printf("IS620N: %soperational.\n", s.operational ? "" : "Not ");

sc_IS620N_state = s;

}

/***/

void cyclic_task() { static unsigned int timeout_error = 0; static uint16_t status_mask=0x004F; static int32_t target_velocity = TARGET_VELOCITY;

uint16_t    status;
int8_t      opmode;
int32_t     current_velocity;
//int32_t     command_value;

/*Receive process data*/
ecrt_master_receive(master);
ecrt_domain_process(domain1);
/*Check process data state(optional)*/
check_domain1_state();

counter = TASK_FREQUENCY;
//Check for master state
check_master_state();
//Check for slave configuration state(s)
check_slave_config_states();
/*Read inputs*/

status = EC_READ_U16(domain1_pd + offset.status_word);
opmode = EC_READ_U8(domain1_pd + offset.modes_of_operation_display);
current_velocity = EC_READ_S32(domain1_pd + offset.velocity_actual_value);

/*if((status & 0x4F) == 0x00) {
            printf("status:%d\n",status);//statusWord = SANYO_NOT_READY_TO_SWITCH_ON;
}
else if((status & 0x4F) == 0x08) {
            printf("status:%d\n",status);//statusWord =  SANYO_FAULT;
}
else if((status & 0x4F) == 0x40) {
            printf("status:%d\n",status);//statusWord =  SANYO_SWITCH_ON_DISABLED;
}
else if((status & 0x6F) == 0x27) {
            printf("status:%d\n",status);//statusWord =  SANYO_OPERATION_ENABLED;
}
else if((status & 0x6F) == 0x23) {
            printf("status:%d\n",status);//statusWord =  SANYO_SWITCH_ON_ENABLED;
}
else if((status & 0x6F) == 0x21) {
            printf("status:%d\n",status);//statusWord =  SANYO_READY_TO_SWITCH_ON;
}
else if((status & 0x6F) == 0x07) {
            printf("status:%d\n",status);//statusWord =  SANYO_QUICK_STOP_ACTIVE;
}
else if((status & 0x4F) == 0x0F) {
            printf("status:%d\n",status);//statusWord =  SANYO_FAULT_REACTION_ACTIVE;
}
else {
            printf("i status:%d\n",status);//return 0xFFFF;
}*/

/*EC_WRITE_U16(domain1_pd + offset.control_word, 0x0006);
EC_WRITE_S8(domain1_pd + offset.modes_operation, PROFILE_VELOCITY);
EC_WRITE_U16(domain1_pd + offset.control_word, 0x0007);//接通主回路电
EC_WRITE_U16(domain1_pd + offset.control_word, 0x000f);//伺服运转
EC_WRITE_S32(domain1_pd + offset.target_velocity, target_velocity);
EC_WRITE_U16(domain1_pd + offset.control_word, 0x001f);//伺服运行*/

//DS402 CANOpen over EtherCAT status machine
if((status & status_mask)==0x0040) //status must equal to SERVO_STAT_SWION_DIS
{
    EC_WRITE_U16(domain1_pd + offset.control_word, 0x0006 );
    EC_WRITE_S8(domain1_pd + offset.modes_operation, PROFILE_VELOCITY);
    status_mask=0x6f;
    printf("status:0x%x\n",status);
}
else if((status & status_mask) == 0x0021) //status must equal to SERVO_STAT_RDY_SWION
{
    EC_WRITE_U16(domain1_pd + offset.control_word, 0x0007 );
    status_mask=0x6f;
    printf("status:0x%x\n",status);
}
else if((status & status_mask) == 0x0023) //status must equal to SERVO_STAT_SWION_ENA
{
    EC_WRITE_U16(domain1_pd + offset.control_word, 0x000f );
    EC_WRITE_S32(domain1_pd + offset.target_velocity, target_velocity);
    status_mask=0x6f;
    printf("status:0x%x\n",status);
}
//operation enabled
else if((status & status_mask) ==0x0027) //status must equal to SERVO_STAT_OP_ENA
{
    EC_WRITE_U16(domain1_pd + offset.control_word, 0x001f);
    printf("status:0x%x\n",status);
}

/*Send process data*/
ecrt_domain_queue(domain1);
ecrt_master_send(master);

}

/****/

int main(int argc, char **argv) { master = ecrt_request_master(0); if (!master) exit(EXIT_FAILURE);

domain1 = ecrt_master_create_domain(master);
if (!domain1)
   exit(EXIT_FAILURE);

if (!(sc_IS620N = ecrt_master_slave_config(master, IS620NSlavePos, INOVANCE_IS620N)))
{
    fprintf(stderr, "Failed to get slave configuration for IS620N!\n");
    exit(EXIT_FAILURE);
}

/*设置0x1702为RPDO*/
/*if(ecrt_slave_config_sdo8(sc_IS620N,0x1c12,0,0)) printf("failed to configure slave sdo 0x1c12-00:0x00!\n");
if(ecrt_slave_config_sdo16(sc_IS620N,0x1c12,1,0x1702)) printf("failed to configure slave sdo 0x1c12-01:0x1702!\n");
if(ecrt_slave_config_sdo8(sc_IS620N,0x1c12,0,0x01)) printf("failed to configure slave sdo 0x1c12-00:0x01!\n");*/

/*设置0x1B02为TPDO*/
/*if(ecrt_slave_config_sdo8(sc_IS620N,0x1c13,0,0)) printf("failed to configure slave sdo 0x1c13-00:0x00!\n");
if(ecrt_slave_config_sdo16(sc_IS620N,0x1c13,1,0x1B02)) printf("failed to configure slave sdo 0x1c13-01:0x1B02!\n");
if(ecrt_slave_config_sdo8(sc_IS620N,0x1c13,0,0x01)) printf("failed to configure slave sdo 0x1c13-00:0x01!\n");*/

// printf("alias=%d,vid=%d,watchdog_divider=%d",sc_IS620N->alias,sc_IS620N->position,sc_IS620N->watchdog_divider);

printf("Configuring PDOs...\n");
/*重新映射PDO才需要运行ecrt_slave_config_pdos(),如果使用servo的固定映射则不需要*/
if (ecrt_slave_config_pdos(sc_IS620N, EC_END, IS620N_syncs)) 
{
    fprintf(stderr, "Failed to configure IS620N PDOs!\n");
    exit(EXIT_FAILURE);
}

if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) 
{
    fprintf(stderr, "PDO entry registration failed!\n");
    exit(EXIT_FAILURE);
}

printf("Activating master...\n");
if (ecrt_master_activate(master))
    exit(EXIT_FAILURE);
else
   printf("Master activated\n");

if (!(domain1_pd = ecrt_domain_data(domain1)))
    exit(EXIT_FAILURE);

printf("It's working now...\n");

/*if(ecrt_slave_config_sdo8(sc_IS620N,0x6060,0,8)) 
    printf("fail to configure slave sdo velocity mode!\n");*/

/*EC_WRITE_U16(domain1_pd + offset.control_word, 0x0006);
EC_WRITE_S8(domain1_pd + offset.modes_operation, PROFILE_VELOCITY);
EC_WRITE_U16(domain1_pd + offset.control_word, 0x0007);//接通主回路电
EC_WRITE_U16(domain1_pd + offset.control_word, 0x000f);//伺服运转
EC_WRITE_S32(domain1_pd + offset.target_velocity, TARGET_VELOCITY);
EC_WRITE_U16(domain1_pd + offset.control_word, 0x001f);//伺服运行*/

/if(ecrt_slave_config_sdo32(sc_IS620N,0x60FF,0,TARGET_VELOCITY)) printf("fail to configure slave sdo 0x60FF!\n"); if(ecrt_slave_config_sdo16(sc_IS620N,0x6040,0,0x6)) printf("fail to configure slave sdo 0x6040!\n"); if(ecrt_slave_config_sdo16(sc_IS620N,0x6040,0,0x7)) printf("fail to configure slave sdo 0x6040!\n"); if(ecrt_slave_config_sdo16(sc_IS620N,0x6040,0,0xf)) printf("fail to configure slave sdo 0x6040!\n");/

while (1) 
{
    usleep(100000/TASK_FREQUENCY);
    cyclic_task();
}
ecrt_master_deactivate(master);
return EXIT_SUCCESS;

}

fjes commented 5 years ago

For these kind of questions please use the Etherlab mailing list http://lists.etherlab.org/mailman/listinfo/etherlab-users there are many people who can help you with your problem.

bg, Frank