int _motor_config_update(ec_sdo_request_t *request, int update, int value, int sequence)
{
int sdo_update_value;
if(update==0)
{
write_sdo(request, value);
sdo_update_value = read_sdo(request);
if(sdo_update_value == value)
{
update = 1;
printf("%d ", sequence);
fflush(stdout);
}
}
return update;
}
void cyclic_task()
{
/* sync the dc clock of the slaves */
ecrt_master_sync_slave_clocks(master);
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain1);
// check process data state (optional)
check_domain1_state();
if (counter) {
counter--;
} else { // do this at 1 Hz
counter = FREQUENCY;
// calculate new process data
blink = !blink;
// 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(sdo);
read_sdo(request[0]);
read_sdo(request[1]);
read_sdo(request[2]);
write_sdo(sdo_download_requests[0], sdoexample); /* SDO download value to the node */
#endif
}
/* Read process data */
unsigned int sn_status = EC_READ_U16(domain1_pd + off_pdo1_in);
unsigned int sn_modes = EC_READ_U8(domain1_pd + off_pdo2_in);
unsigned int sn_position = EC_READ_U32(domain1_pd + off_pdo3_in);
unsigned int sn_velocity = EC_READ_U32(domain1_pd + off_pdo4_in);
unsigned int sn_torque = EC_READ_U16(domain1_pd + off_pdo5_in);
logmsg(2, "[REC] 0x%4x 0x%4x 0x%8x 0x%8x 0x%4x\n",
sn_status, sn_modes,
sn_position, sn_velocity, sn_torque);
#if 0
// read process data
printf("AnaIn: state %u value %u\n",
EC_READ_U8(domain1_pd + off_ana_in_status),
EC_READ_U16(domain1_pd + off_ana_in_value));
#endif
// write process data
//EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
#ifdef CIA402
#define STATUSW1 0x88AA
#define STATUSW2 0xAA88
#define OPMODES1 0xf1
#define OPMODES2 0x1f
#define TORVAL1 0xabab
#define TORVAL2 0xbaba
#define VELVAL1 0x2d2d4d4d
#define VELVAL2 0xd4d4d2d2
#define POSVAL1 0xe4e4e2e2
#define POSVAL2 0x2e2e4e4e
EC_WRITE_U16(domain1_pd + off_pdo1_out, (blink ? STATUSW1 : STATUSW2)&0xffff);
EC_WRITE_U8(domain1_pd + off_pdo2_out, (blink ? OPMODES1 : OPMODES2)&0xff);
EC_WRITE_U16(domain1_pd + off_pdo3_out, (blink ? TORVAL1 : TORVAL2)&0xffff);
EC_WRITE_U32(domain1_pd + off_pdo4_out, blink ? POSVAL1 : POSVAL2);
EC_WRITE_U32(domain1_pd + off_pdo5_out, blink ? VELVAL1 : VELVAL2);
#else
#define TESTWORD1 0xdead
#define TESTWORD2 0xbeef
#define TESTWORD3 0xfefe
#define TESTWORD4 0xa5a5
EC_WRITE_U16(domain1_pd + off_pdo1_out, blink ? TESTWORD1 : TESTWORD2);
EC_WRITE_U16(domain1_pd + off_pdo2_out, blink ? TESTWORD3 : TESTWORD4);
#endif
// send process data
ecrt_domain_queue(domain1);
ecrt_master_send(master);
//printf("Wrote %x to slave\n", blink ? TESTWORD1 : TESTWORD2);
}
