void check_domain1_state() {
ec_domain_state_t ds;
ecrt_domain_state(gkDomain1, &ds);
#if 0
if (ds.working_counter != gkDomain1State.working_counter)
printf("Domain1: WC %u.\n", ds.working_counter);
if (ds.wc_state != gkDomain1State.wc_state)
printf("Domain1: State %u.\n", ds.wc_state);
#endif
gkDomain1State = ds;
}
void check_domain_w_state(void)
{
ec_domain_state_t ds2;
ecrt_domain_state(domain_w, &ds2);
if (ds2.working_counter != domain_w_state.working_counter)
printf("domain_w: WC %u.\n", ds2.working_counter);
if (ds2.wc_state != domain_w_state.wc_state)
printf("domain_w: State %u.\n", ds2.wc_state);
domain_w_state = ds2;
}
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_domain_r_state(void)
{
ec_domain_state_t ds;
ecrt_domain_state(domain_r, &ds);
//struct timespec time_wc1,time_wc2;
if (ds.working_counter != domain_r_state.working_counter)
printf("domain_r: WC %u.\n", ds.working_counter);
if (ds.wc_state != domain_r_state.wc_state)
printf("domain_r: State %u.\n", ds.wc_state);
domain_r_state = ds;
}
void check_domain1_state(master_setup_variables_t *master_setup)
{
ec_domain_state_t ds;
ecrt_domain_state(master_setup->domain, &ds);
if (ds.working_counter != master_setup->domain_state.working_counter)
logmsg(1, "Domain1: WC %u.\n", ds.working_counter);
if (ds.wc_state != master_setup->domain_state.wc_state)
logmsg(1, "Domain1: State %u.\n", ds.wc_state);
master_setup->domain_state = ds;
}
static inline void check_domain1_state(void)
{
ec_domain_state_t ds;
ecrt_domain_state(domain, &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);
memcpy(&domain1_state, &ds, sizeof(ds));
}
void sdo_handle_ecat(master_setup_variables_t *master_setup,
ctrlproto_slv_handle *slv_handles,
int update_sequence, int slave_number)
{
int slv;
if(sig_alarms == user_alarms) pause();
while (sig_alarms != user_alarms)
{
/* sync the dc clock of the slaves */
// ecrt_master_sync_slave_clocks(master);
// receive process data
ecrt_master_receive(master_setup->master);
ecrt_domain_process(master_setup->domain);
//for (slv = 0; slv < total_no_of_slaves; ++slv)
{
slv_handles[slave_number].motor_config_param = \
sdo_motor_config_update(slv_handles[slave_number].motor_config_param, \
slv_handles[slave_number].__request, update_sequence);
}
// send process data
ecrt_domain_queue(master_setup->domain);
ecrt_master_send(master_setup->master);
//Check for master und domain state
ecrt_master_state(master_setup->master, &master_setup->master_state);
ecrt_domain_state(master_setup->domain, &master_setup->domain_state);
if (master_setup->domain_state.wc_state == EC_WC_COMPLETE && !master_setup->op_flag)
{
//printf("System up!\n");
master_setup->op_flag = 1;
}
else
{
if(master_setup->domain_state.wc_state != EC_WC_COMPLETE && master_setup->op_flag)
{
//printf("System down!\n");
master_setup->op_flag = 0;
}
}
user_alarms++;
}
}
void check_domain1_state(void)
{
ec_domain_state_t ds;
down(&master_sem);
ecrt_domain_state(domain1, &ds);
up(&master_sem);
if (ds.working_counter != domain1_state.working_counter)
printk(KERN_INFO PFX "Domain1: WC %u.\n", ds.working_counter);
if (ds.wc_state != domain1_state.wc_state)
printk(KERN_INFO PFX "Domain1: State %u.\n", ds.wc_state);
domain1_state = ds;
}
void pdo_handle_ecat(master_setup_variables_t *master_setup,
ctrlproto_slv_handle *slv_handles,
unsigned int total_no_of_slaves)
{
int slv;
if(sig_alarms == user_alarms) pause();
while (sig_alarms != user_alarms)
{
/* sync the dc clock of the slaves */
// ecrt_master_sync_slave_clocks(master);
// receive process data
ecrt_master_receive(master_setup->master);
ecrt_domain_process(master_setup->domain);
// check process data state (optional)
//check_domain1_state(master_setup);
// check for master state (optional)
//check_master_state(master_setup);
// check for islave configuration state(s) (optional)
// check_slave_config_states();
for(slv=0;slv<total_no_of_slaves;++slv)
{
/* Read process data */
slv_handles[slv].motorctrl_status_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[0]);
slv_handles[slv].operation_mode_disp = EC_READ_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[1]);
slv_handles[slv].position_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[2]);
slv_handles[slv].speed_in = EC_READ_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[3]);
slv_handles[slv].torque_in = EC_READ_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_in[4]);
}
/* printf("\n%x", slv_handles[slv].motorctrl_status_in);
printf("\n%x", slv_handles[slv].operation_mode_disp);
printf("\n%x", slv_handles[slv].position_in);
printf("\n%x", slv_handles[slv].speed_in);
printf("\n%x", slv_handles[slv].torque_in);
*/
for(slv=0;slv<total_no_of_slaves;++slv)
{
EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[0], (slv_handles[slv].motorctrl_out)&0xffff);
EC_WRITE_U8(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[1], (slv_handles[slv].operation_mode)&0xff);
EC_WRITE_U16(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[2], (slv_handles[slv].torque_setpoint)&0xffff);
EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[3], slv_handles[slv].position_setpoint);
EC_WRITE_U32(master_setup->domain_pd + slv_handles[slv].__ecat_slave_out[4], slv_handles[slv].speed_setpoint);
}
// send process data
ecrt_domain_queue(master_setup->domain);
ecrt_master_send(master_setup->master);
//Check for master und domain state
ecrt_master_state(master_setup->master, &master_setup->master_state);
ecrt_domain_state(master_setup->domain, &master_setup->domain_state);
if (master_setup->domain_state.wc_state == EC_WC_COMPLETE && !master_setup->op_flag)
{
//printf("System up!\n");
master_setup->op_flag = 1;
}
else
{
if(master_setup->domain_state.wc_state != EC_WC_COMPLETE && master_setup->op_flag)
{
//printf("System down!\n");
master_setup->op_flag = 0;
}
}
user_alarms++;
}
}
