void cyclic_task(unsigned long data)
{
// receive process data
down(&master_sem);
ecrt_master_receive(master);
ecrt_domain_process(domain1);
up(&master_sem);
// check process data state (optional)
check_domain1_state();
if (counter) {
counter--;
} else { // do this at 1 Hz
counter = FREQUENCY;
// check for master state (optional)
check_master_state();
}
run_serial_devices(domain1_pd);
// send process data
down(&master_sem);
ecrt_domain_queue(domain1);
ecrt_master_send(master);
up(&master_sem);
// restart timer
timer.expires += HZ / FREQUENCY;
add_timer(&timer);
}
void cyclic_task()
{
static unsigned int counter = 10;
static uint8_t outputValue = 0;
static int numAsyncCycles = 0;
uint8_t inputValue = 0;
static uint8_t error = 0;
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain1);
// check process data state (optional)
check_domain1_state();
inputValue = EC_READ_U8(domain1_pd + off_dig_in[1]) & 0x0F;
if(inputValue != outputValue) {
numAsyncCycles++;
} else {
numAsyncCycles = 0;
}
if(numAsyncCycles > 2) {
if(error != 0xff) {
error++;
}
}
if (counter) {
counter--;
} else {
counter = 5; //update delay
// calculate new process data
outputValue = (outputValue + 1) & 0x0F;
// check for master state (optional)
check_master_state();
// check for islave configuration state(s) (optional)
// check_slave_config_states();
}
// write process data
EC_WRITE_U8(domain1_pd + off_dig_out[0], outputValue);
EC_WRITE_U8(domain1_pd + off_dig_out[1], error);
// send process data
ecrt_domain_queue(domain1);
ecrt_master_send(master);
}
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 ECDomain::update() {
ecrt_domain_process(domain_);
for (std::vector<ECDriver::Ptr>::iterator driver = drivers_.begin();
driver != drivers_.end(); ++driver) {
(*driver)->updateInputs();
}
for (std::vector<ECDriver::Ptr>::iterator driver = drivers_.begin();
driver != drivers_.end(); ++driver) {
(*driver)->updateOutputs();
}
ecrt_domain_queue(domain_);
}
void cyclic_task()
{
// 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();
#endif
}
#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
#if 1
// write process data
EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
#endif
// send process data
ecrt_domain_queue(domain1);
ecrt_master_send(master);
}
void uei_ethercat_cleanup(void)
{
ecrt_master_receive(master);
ecrt_domain_process(domain);
EC_WRITE_S16(rx_controller_state.current_val, 0);
EC_WRITE_U16(rx_controller_state.amp_state, ECAT_STATE_DISABLED);
// EC_WRITE_S16(pv_controller_state.current_val, 0);
// EC_WRITE_U16(pv_controller_state.amp_state, ECAT_STATE_DISABLED);
//
// EC_WRITE_S16(el_controller_state.current_val, 0);
// EC_WRITE_U16(el_controller_state.amp_state, ECAT_STATE_DISABLED);
ecrt_domain_queue(domain);
ecrt_master_send(master);
ecrt_release_master(master);
}
void my_cyclic_task()
{
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain1);
// check process data state (optional)
check_domain1_state();
// check for master state (optional)
check_master_state();
// check for islave configuration state(s) (optional)
check_slave_config_states();
// read process data SDO
read_sdo();
// send process data
ecrt_domain_queue(domain1);
ecrt_master_send(master);
}
void run(long data)
{
int i;
struct timeval tv;
unsigned int sync_ref_counter = 0;
count2timeval(nano2count(rt_get_real_time_ns()), &tv);
// while (deactive!=20) {
while (1) {
t_last_cycle = get_cycles();
// receive process data
rt_sem_wait(&master_sem);
ecrt_master_receive(master);
ecrt_domain_process(domain1);
rt_sem_signal(&master_sem);
// check process data state (optional)
//check_domain1_state();
inpu[0]=EC_READ_U16(domain1_pd + status_word);
inpu[1]=EC_READ_U32(domain1_pd + pos_act);
// if(servooff==1){//servo off
// if(stop==1){
// if( ( inpu[0] == 0x1637 ) && ( inpu[2] == 0x1637 ) ){
// EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
// EC_WRITE_U16(domain1_pd+ctrl_word2, 0x0006 );
// }
// else if( ( inpu[0] == 0x0650 ) && ( inpu[2] == 0x0650 ) ){
// printk(KERN_INFO PFX "want to PREOP");
// deactive++;
// }
// }
if( (inpu[0]&0x004f) == 0x0040 ){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
}
else if( (inpu[0]&0x006f) == 0x0021){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 );
}
else if( (inpu[0]&0x006f) == 0x0023){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f);
EC_WRITE_S32(domain1_pd+tar_pos, 0);
EC_WRITE_S32(domain1_pd+max_torq, 0xf00);
}
else if( (inpu[0]&0x006f) == 0x0027){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f);
EC_WRITE_S32(domain1_pd+tar_pos , value ); //for mode 8 no sin
if(value==180000){
speedup=0;
speeddown=1;
//printk(KERN_INFO PFX "top");
value=value-1;
}
else if(speeddown==1 && value!=0){
value=value-1;
//printk(KERN_INFO PFX "slow down");
}
else if(speeddown==1 && value==0){
speedup=0;
speeddown=0;
// stop=1;
//printk(KERN_INFO PFX "stop");
}
else if(!stop){
speedup=1;
speeddown=0;
value=value+1;
//printk(KERN_INFO PFX "fast up ");
}
// change++;
// }
// else
// change = 0;
}
rt_sem_wait(&master_sem);
tv.tv_usec += 1000;
if (tv.tv_usec >= 1000000) {
tv.tv_usec -= 1000000;
tv.tv_sec++;
}
ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; //original = 9
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
ecrt_domain_queue(domain1);
ecrt_master_send(master);
rt_sem_signal(&master_sem);
rt_task_wait_period();
}
}
void cyclic_task()
{
struct timespec wakeupTime, time;
// get current time
clock_gettime(CLOCK_TO_USE, &wakeupTime);
while(1)
{
if(deactive==1)
{
break;
}
wakeupTime = timespec_add(wakeupTime, cycletime);
clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
// writter_receive(master);
ecrt_master_receive(master);
ecrt_domain_process(domain_r);
ecrt_domain_process(domain_w);
temp[0]=EC_READ_U16(domain_w_pd + status_word);
temp[1]=EC_READ_S32(domain_w_pd + mode_display);
if (counter)
{
counter--;
}
else
{ // do this at 1 Hz
counter = FREQUENCY;
check_master_state();
blink = !blink;
}
// write process data
if(servo_flag==1)
{
//servo off
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0006 );
}
else if( (temp[0]&0x004f) == 0x0040 )
{
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0006 );
}
else if( (temp[0]&0x006f) == 0x0021)
{
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x0007 );
}
else if( (temp[0]&0x006f) == 0x0023)
{
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x000f );
EC_WRITE_S32(domain_r_pd+tar_pos,0);
EC_WRITE_S32(domain_r_pd+tar_vel, 0xffff);
EC_WRITE_S32(domain_r_pd+max_torq, 0xf00);
}
//operation enabled
else if( (temp[0]&0x006f) == 0x0027)
{
EC_WRITE_S32(domain_r_pd+tar_pos, (move_value+=2000) );
EC_WRITE_U16(domain_r_pd+ctrl_word, 0x001f);
}
clock_gettime(CLOCK_TO_USE, &time);
ecrt_master_application_time(master, TIMESPEC2NS(time));
if (sync_ref_counter)
{
sync_ref_counter--;
}
else
{
sync_ref_counter = 1; // sync every cycle
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
// send process data
ecrt_domain_queue(domain_r);
ecrt_domain_queue(domain_w);
ecrt_master_send(master);
}
}
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++;
}
}
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);
}
void cyclic_task()
{
// receive process data
ecrt_master_receive(master);
ecrt_domain_process(domain_output);
ecrt_domain_process(domain_input);
// check process data state (optional)
check_domain1_state();
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();
// read process data
// {0x6040, 0x00, 16}, /* Controlword */
// {0x6060, 0x00, 8}, /* Mode_of_Operation */
// {0x6098, 0x00, 8}, /* Homing_Method */
// {0x607a, 0x00, 32}, /* Target_Position */
// {0x60ff, 0x00, 32}, /* Target_Velocity */
// {0x6071, 0x00, 16}, /* Target_Torque */
// {0x6041, 0x00, 16}, /* Statusword */
// {0x6064, 0x00, 32}, /* Position_Actual_Value */
// {0x6061, 0x00, 8}, /* Modes_Of_Operation_Display */
// {0x1001, 0x00, 8}, /* Error_Register */
// {0x606c, 0x00, 32}, /* Velocity_Actual_Value */
// {0x6077, 0x00, 16}, /* Torque_Actual_Value */
// printf("pdo value: %02x offset %u\n",
// EC_READ_U16(domain1_pd + off_0x6040),off_0x6040);
// printf("pdo value: %02x offset %u\n",
// EC_READ_U16(domain1_pd + off_0x1001),off_0x1001);
printf("pdo value: %04x offset %u\n",
EC_READ_U16(domain_input_pd + off_0x6041),off_0x6041);
printf("pdo value 6061asfsadf: %04x offset %u\n",
EC_READ_U8(domain_input_pd + off_0x6061),off_0x6061);
printf("pd: %u \n",*domain_input_pd);
// EC_READ_U8(domain1_pd + off_ana_in_value));
#if SDO_ACCESS
// read process data SDO
read_sdo();
#endif
}
// send process data
ecrt_domain_queue(domain_output);
ecrt_domain_queue(domain_input);
ecrt_master_send(master);
#if 0
// write process data
// EC_WRITE_U8(domain1_pd + off_dig_out, blink ? 0x06 : 0x09);
#endif
}
void run(long data)
{
int i;
struct timeval tv;
unsigned int sync_ref_counter = 0;
count2timeval(nano2count(rt_get_real_time_ns()), &tv);
// while (deactive!=20) {
while (1) {
t_last_cycle = get_cycles();
/*
if ( (inpu[0]==0x0650) &&(stop==1) )
break;
*/
// receive process data
rt_sem_wait(&master_sem);
ecrt_master_receive(master);
ecrt_domain_process(domain1);
ecrt_domain_process(domain2);
rt_sem_signal(&master_sem);
// check process data state (optional)
//check_domain1_state();
inpu[0]=EC_READ_U16(domain2_pd + status_word);
inpu[1]=EC_READ_U32(domain2_pd + actual_pos);
/*
if (counter) {
counter--;
} else {
u32 c;
counter = FREQUENCY;
// check for master state (optional)
check_master_state();
c = EC_READ_U32(domain1_pd + off_counter_in);
if (counter_value != c) {
counter_value = c;
printk(KERN_INFO PFX "counter=%u\n", counter_value);
}
}
*/
/*
if (blink_counter) {
blink_counter--;
} else {
blink_counter = 9;
// calculate new process data
blink = !blink;
}
*/
// write process data
/*
for (i = 0; i < NUM_DIG_OUT; i++) {
EC_WRITE_U8(domain1_pd + off_dig_out[i], blink ? 0x66 : 0x99);
}
EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02);
*/
// if(servooff==1){//servo off
if(stop==1){
if ( inpu[0] == 0x1637 ) {
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
}
else if( inpu[0] == 0x0650 ){
//++deactive ;
//EC_WRITE_U16(domain1_pd+alstat, 0x0002 );
printk(KERN_INFO PFX "want to PREOP");
deactive++;
}
/*
else{
EC_WRITE_U16(domain1_pd+alstat, 0x0002 );
printk(KERN_INFO PFX "want to PREOP");
break;
}
*/
}
else if( (inpu[0]&0x0040) == 0x0040){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
}
else if( (inpu[0]&0x006f) == 0x0021 ){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 );
}
else if( (inpu[0]&0x027f) == 0x0233){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f);
EC_WRITE_S32(domain1_pd+interpolateddata, 0);
//EC_WRITE_S32(domain1_pd+tar_velo, 0xffffff);
EC_WRITE_S32(domain1_pd+max_torq, 0xf00);
EC_WRITE_S32(domain1_pd+modeofoper, 8);
}
else if( (inpu[0]&0x027f) == 0x0237){
//if(change >= 0 && change<2 ){
if( change<1 ){
//start=1;
//if(i==0){
//EC_WRITE_S32(domain1_pd+interpolateddata , 0 );
//EC_WRITE_S32(domain1_pd+interpolateddata2 , 0 );
//EC_WRITE_S32(domain1_pd+target_pos , 0 );
//EC_WRITE_S32(domain1_pd+target_pos2 , 0 );
//EC_WRITE_S32(domain1_pd+tar_velo , 0 );
//EC_WRITE_S32(domain1_pd+tar_velo2 , 0 );
//}
//else {
//EC_WRITE_S32(domain1_pd+interpolateddata , (sin(i)*180000) ); //for mode 7
//EC_WRITE_S32(domain1_pd+interpolateddata2 , (sin(i)*180000) );
//EC_WRITE_S32(domain1_pd+target_pos , (sin(i)*180000) ); //for mode 8 with sin
//EC_WRITE_S32(domain1_pd+target_pos2 , (sin(i)*180000) );
EC_WRITE_S32(domain1_pd+target_pos , inpu[7] ); //for mode 8 no sin
//EC_WRITE_S32(domain1_pd+tar_velo , 500000 ); //for mode 9
//EC_WRITE_S32(domain1_pd+tar_velo2 , 500000 );
//if(1){
if(inpu[7]==1800000){
speedup=0;
speeddown=1;
//printk(KERN_INFO PFX "top");
inpu[7]=inpu[7]-200;
}
else if(speeddown==1 && inpu[7]!=0){
inpu[7]=inpu[7]-200;
//printk(KERN_INFO PFX "slow down");
}
else if(speeddown==1 && inpu[7]==0){
speedup=0;
speeddown=0;
stop=1;
//printk(KERN_INFO PFX "stop");
}
else if(!stop){
speedup=1;
speeddown=0;
inpu[7]=inpu[7]+2000;
//printk(KERN_INFO PFX "fast up ");
}
/*
if(speedup==1)
inpu[7]+=500;
else if(speeddown==1)
inpu[7]-=1000;
else{
inpu[7]=0;
servooff=1;
}
*/
//EC_WRITE_S32(domain1_pd+tar_velo , inpu[7] ); //for mode 9
//EC_WRITE_S32(domain1_pd+tar_velo2 , inpu[7] );
//}
//else{
//EC_WRITE_S32(domain1_pd+tar_velo , inpu[7] ); //for mode 9
//EC_WRITE_S32(domain1_pd+tar_velo2 , inpu[7] );
//}
//}
EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f);
change++;
/*
if(datacount<10001){
data[datacount][0]=(sin(i)*360000);
data[datacount][1]=inpu[1];
data[datacount][2]=inpu[3];
data[datacount][3]=(inpu[1] - inpu[3]);
datacount++;
}
*/
}
else
change = 0;
}
//printk(KERN_INFO PFX "pos1=%d pos2=%d inpu7=%d\n",inpu[1],inpu[3],inpu[7]);
rt_sem_wait(&master_sem);
tv.tv_usec += 1000;
if (tv.tv_usec >= 1000000) {
tv.tv_usec -= 1000000;
tv.tv_sec++;
}
ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; //original = 9
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
ecrt_domain_queue(domain1);
ecrt_domain_queue(domain2);
ecrt_master_send(master);
rt_sem_signal(&master_sem);
rt_task_wait_period();
}
}
void motor_cmd_routine(void *m_arg)
{
int ret;
RT_TIMER_INFO timer_info;
long long task_period;
unsigned long overruns = 0;
int16_t req_current = 0;
int sync_ref_counter=0;
float cos_el;
float sin_el;
float v_req_az;
float V_REQ_AZ = 0;
float P_term_az, error_az;
float p_az = 20.0;
float i_az = 1.0;
static float az_integral = 0.0;
float I_term_az, INTEGRAL_CUTOFF=0.5;
printf("Starting Motor Commanding task\n");
rt_timer_inquire(&timer_info);
if (timer_info.period == TM_ONESHOT)
{
// When using an aperiodic timer, task period is specified in ns
task_period = rt_timer_ns2ticks(1000000000ll / 100);
}
else
{
// When using a periodic timer, task period is specified in number of timer periods
task_period = (1000000000ll / 100) / timer_info.period;
}
ret = rt_task_set_periodic(NULL, TM_NOW, task_period);
if (ret)
{
printf("error while set periodic, code %d\n", ret);
return;
}
// Make sure we are in primary mode before entering the timer loop
rt_task_set_mode(0, T_PRIMARY, NULL);
while (!stop)
{
unsigned long ov;
// Wait for next time period
ret = rt_task_wait_period(&ov);
if (ret && ret != -ETIMEDOUT)
{
printf("error while rt_task_wait_period, code %d (%s)\n", ret,
strerror(-ret));
break;
}
overruns = overruns + ov;
ecrt_master_receive(master);
ecrt_domain_process(domain);
// write application time to master
ecrt_master_application_time(master, rt_timer_tsc2ns(rt_timer_tsc()));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; // sync every cycle
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
/*******************************************************************\
* Card0: Drive the Azimuth Motor (Reaction Wheel) *
\*******************************************************************/
/* Read sin and cos of the inner frame elevation, calculated by mcp */
// cos_el = 1.0; //( COS_EL*0.000030517578125 ) - 1.0;
// sin_el = 0.0; //( SIN_EL*0.000030517578125 ) - 1.0;
//
// v_req_az = 0.0; //(float)(V_REQ_AZ-32768.0)*0.0016276041666666666666666666666667; // = vreq/614.4
//
// //roll, yaw contributions to az both -'ve (?)
// error_az = (gy_ifroll*sin_el + gy_ifyaw*cos_el) + v_req_az;
//
// P_term_az = p_az*error_az;
//
// if( (p_az == 0.0) || (i_az == 0.0) ) {
// az_integral = 0.0;
// } else {
// az_integral = (1.0 - INTEGRAL_CUTOFF)*az_integral + INTEGRAL_CUTOFF*error_az;
// }
//
// I_term_az = az_integral * p_az * i_az;
// if (I_term_az > 100.0) {
// I_term_az = 100.0;
// az_integral = az_integral *0.9;
// }
// if (I_term_az < -100.0) {
// I_term_az = -100.0;
// az_integral = az_integral * 0.9;
// }
// if (P_term_az > 1.0 || P_term_az < -1.0) printf("error_az: %f\tI: %f\tP: %f\n", error_az, I_term_az, P_term_az);
// req_current = 0.5 *(-(P_term_az + I_term_az) ) ;
req_current = 100;
if (req_current > 200)
printf("Error! Requested current is %d\n", req_current);
else {
EC_WRITE_S16(rx_controller_state.current_val, req_current);
}
ecrt_domain_queue(domain);
ecrt_master_send(master);
}
//switch to secondary mode
ret = rt_task_set_mode(T_PRIMARY, 0, NULL);
if (ret)
{
printf("error while rt_task_set_mode, code %d\n", ret);
return;
}
}
int uei_ethercat_initialize (void)
{
uint8_t *data;
ec_pdo_entry_reg_t rw_pdos[] = {
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off, NULL},
{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off, NULL},
{RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATE, state_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATUS, status_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off, NULL},
// {RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off, NULL},
{0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}};
// ec_pdo_entry_reg_t pv_pdos[] = {
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_STATUS, status_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off+1, NULL},
// {PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE, ECAT_DRIVE_STATE, state_off+1, NULL},
// {0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}};
// ec_pdo_entry_reg_t el_pdos[] = {
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CI, current_ci_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_CP, current_cp_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_OFFSET, current_offset_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_CURRENT_LOOP_VAL, current_val_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATUS, status_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_TEMP, drive_temp_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_FAULT, latched_fault_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_LATCHED_DRIVE_STATUS, latched_status_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_POSITION, motor_pos_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_TEMP_VOLTAGE, motor_temp_v_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_MOTOR_ENC_WRAP_POS, motor_enc_wrap_off+2, NULL},
// {EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE, ECAT_DRIVE_STATE, state_off+2, NULL},
// {0, 0, 0x00, 0x00, 0x0, 0x0, NULL, NULL}};
master = ecrt_request_master(0);
if (!master){
printf("Could not request master!\n");
return -1;
}
domain = ecrt_master_create_domain(master);
if (!domain) {
printf("Could not create domain!\n");
return -1;
}
printf("Created Domain\n");
if (!(rx_controller = ecrt_master_slave_config(master,RW_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE))) {
fprintf(stderr, "Failed to get slave configuration for Reaction Wheel controller!\n");
return -1;
}
// if (!(pv_controller = ecrt_master_slave_config(master,PV_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, BEL_090_020_PRODCODE))) {
// fprintf(stderr, "Failed to get slave configuration for Pivot Motor controller!\n");
// return -1;
// }
// if (!(el_controller = ecrt_master_slave_config(master,EL_ETHERCAT_ALIAS, 0, COPLEY_ETHERCAT_VENDOR, AEP_090_036_PRODCODE))) {
// fprintf(stderr, "Failed to get slave configuration for Elevation Motor controller!\n");
// return -1;
// }
if (ecrt_slave_config_pdos(rx_controller, EC_END, copley_pdo_syncs)) {
perror("ecrt_slave_config_pdos() failed for RX controller.");
ecrt_release_master(master);
return 3;
}
// if (ecrt_slave_config_pdos(pv_controller, 1, copley_pdo_syncs)) {
// perror("ecrt_slave_config_pdos() failed for Pivot controller.");
// ecrt_release_master(master);
// return 3;
// }
// if (ecrt_slave_config_pdos(el_controller, 1, copley_pdo_syncs)) {
// perror("ecrt_slave_config_pdos() failed for Elevation controller.");
// ecrt_release_master(master);
// return 3;
// }
/// Register the PDO list and variable mappings
// if (ecrt_domain_reg_pdo_entry_list(domain, rw_pdos)) {
// perror("ecrt_domain_reg_pdo_entry_list() failed for reaction wheel!");
// ecrt_release_master(master);
// return -1;
// }
// if (ecrt_domain_reg_pdo_entry_list(domain, pv_pdos)) {
// perror("ecrt_domain_reg_pdo_entry_list() failed for pivot motor!");
// ecrt_release_master(master);
// return -1;
// }
// if (ecrt_domain_reg_pdo_entry_list(domain, el_pdos)) {
// perror("ecrt_domain_reg_pdo_entry_list() failed for Elevation motor!");
// ecrt_release_master(master);
// return -1;
// }
state_off[0] = ecrt_slave_config_reg_pdo_entry(rx_controller,
ECAT_DRIVE_STATE, domain, NULL);
current_val_off[0] = ecrt_slave_config_reg_pdo_entry(rx_controller,
ECAT_CURRENT_LOOP_VAL, domain, NULL);
// configure SYNC signals for this slave
ecrt_slave_config_dc(rx_controller, 0, 1000000000ll / 100, 4400000, 0, 0);
printf("Set Master/Slave Configuration\n");
if (ecrt_master_activate(master) < 0) {
printf("Could not activate master!\n");
return -1;
}
if (!(data = ecrt_domain_data(domain))) {
perror("ecrt_domain_data() failed!");
ecrt_release_master(master);
return -1;
}
ethercat_set_offsets(&rx_controller_state, data, 0);
// ethercat_set_offsets(&pv_controller_state, data, 1);
// ethercat_set_offsets(&el_controller_state, data, 2);
check_domain1_state();
check_master_state();
printf("Data: %p\t Current: %p\t State: %p\n", data, rx_controller_state.current_val, rx_controller_state.amp_state);
ecrt_master_receive(master);
ecrt_domain_process(domain);
EC_WRITE_S16(data + current_val_off[0], 0);
EC_WRITE_U16(data + state_off[0], ECAT_STATE_DISABLED);
ecrt_domain_queue(domain);
ecrt_master_send(master);
check_domain1_state();
check_master_state();
return 0;
}
void run(long data)
{
int i;
struct timeval tv;
unsigned int sync_ref_counter = 0;
count2timeval(nano2count(rt_get_real_time_ns()), &tv);
while (1) {
t_last_cycle = get_cycles();
// receive process data
rt_sem_wait(&master_sem);
ecrt_master_receive(master);
ecrt_domain_process(domain1);
rt_sem_signal(&master_sem);
// check process data state (optional)
check_domain1_state();
if (counter) {
counter--;
} else {
u32 c;
counter = FREQUENCY;
// check for master state (optional)
check_master_state();
c = EC_READ_U32(domain1_pd + off_counter_in);
if (counter_value != c) {
counter_value = c;
printk(KERN_INFO PFX "counter=%u\n", counter_value);
}
}
if (blink_counter) {
blink_counter--;
} else {
blink_counter = 9;
// calculate new process data
blink = !blink;
}
// write process data
for (i = 0; i < NUM_DIG_OUT; i++) {
EC_WRITE_U8(domain1_pd + off_dig_out[i], blink ? 0x66 : 0x99);
}
EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02);
rt_sem_wait(&master_sem);
tv.tv_usec += 1000;
if (tv.tv_usec >= 1000000) {
tv.tv_usec -= 1000000;
tv.tv_sec++;
}
ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 9;
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
ecrt_domain_queue(domain1);
ecrt_master_send(master);
rt_sem_signal(&master_sem);
rt_task_wait_period();
}
}
int main(int argc, char **argv)
{
// Создаем мастер-объект
gkMaster = ecrt_request_master(0);
if (gkMaster) {
fprintf(stdout, "1. Master created.\n");
} else {
fprintf(stderr, "Unable to get requested master.\n");
return -1;
}
// Создаем объект для обмена PDO в циклическом режиме.
gkDomain1 = ecrt_master_create_domain(gkMaster);
if (gkDomain1) {
fprintf(stdout, "2. Process data domain created.\n");
} else {
fprintf(stderr, "Unable to create process data domain.\n");
return -1;
}
// Создаем объект конфигурации подчиненного.
ec_slave_config_t* sc = ecrt_master_slave_config(gkMaster, 0, gkDriveNum, 0x00007595, 0x00000000);
if (sc) {
fprintf(stdout, "3. Slave configuration object created.\n");
} else {
fprintf(stderr, "Failed to get slave configuration.\n");
return -1;
}
// Конфигурируем PDO подчиненного
// TxPDO
ec_pdo_entry_info_t l7na_tx_channel1[] = {
{0x6041, 0, 16}, // Statusword
{0x6061, 0, 8}, // The Modes of Operation Display
{0x6062, 0, 32}, // The Position Demand Value
{0x6064, 0, 32}, // The Position Actual Value
{0x606B, 0, 32}, // The Velocity Demand Value
{0x6081, 0, 32}, // The Profile Velocity
{0x606C, 0, 32}, // The Actual Velocity Value
{0x607A, 0, 32}, // The Target Position
{0x6077, 0, 16}, // Actual torque value
// {0x200F, 0, 16}, // Position Scale Denominator
};
ec_pdo_info_t l7na_tx_pdos[] = {
{0x1A00, 9, l7na_tx_channel1}
};
// RxPDO
ec_pdo_entry_info_t l7na_rx_channel1[] = {
{0x6040, 0, 16}, // Controlword
{0x6060, 0, 8}, // Modes of Operation
{0x607A, 0, 32}, // The Target Position
{0x606C, 0, 32}, // The Velocity Demand value
{0x6081, 0, 32}, // The Profile Velocity
{0x60FF, 0, 32}, // The Target Velocity (in Profile Velocity (Pv) mode and Cyclic Synchronous Velocity (Csv) modes)
{0x6071, 0, 16}, // The Target Torque
};
ec_pdo_info_t l7na_rx_pdos[] = {
{0x1600, 7, l7na_rx_channel1}
};
// Конфигурация SyncManagers 2 (FMMU0) и 3 (FMMU1)
// { sync_mgr_idx, sync_mgr_direction, pdo_num, pdo_ptr, watch_dog_mode }
// { 0xFF - end marker}
ec_sync_info_t l7na_syncs[] = {
{2, EC_DIR_OUTPUT, 1, l7na_rx_pdos, EC_WD_DISABLE},
{3, EC_DIR_INPUT, 1, l7na_tx_pdos, EC_WD_DISABLE},
{0xFF}
};
if (ecrt_slave_config_pdos(sc, EC_END, l7na_syncs)) {
fprintf(stderr, "Failed to configure slave pdo.\n");
return -1;
}
fprintf(stdout, "4. Configuring slave PDOs and sync managers done.\n");
// Регистируем PDO в домене
if (ecrt_domain_reg_pdo_entry_list(gkDomain1, gkDomain1Regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
return -1;
}
fprintf(stdout, "5. PDO entries registered in domain.\n");
if (ecrt_master_activate(gkMaster)) {
fprintf(stderr,"Master activation failed.\n");
return -1;
}
fprintf(stdout, "6. Master activated.\n");
if (!(gkDomain1PD = ecrt_domain_data(gkDomain1))) {
fprintf(stderr,"Domain data initialization failed.\n");
return -1;
}
fprintf(stdout, "7. Domain data registered.\n");
//goto end;
check_master_state();
check_domain1_state();
int32_t op_flag = 0, ipos = 0;
uint16_t istatus = 0;
//ждать режим OP
for(uint32_t j = 0; ; j++) {
ecrt_master_receive(gkMaster); //RECEIVE A FRAME
ecrt_domain_process(gkDomain1); //DETERMINE THE DATAGRAM STATES
// check_slave_config_states();
if (! op_flag) {
check_domain1_state();
}
if (gkDomain1State.wc_state == EC_WC_COMPLETE && !op_flag) {
printf("Domain is up at %d cycles.\n", j);
op_flag = 1;
}
ipos = EC_READ_U32(gkDomain1PD + gkOffIPos); //READ DATA 0x6064 position
istatus = EC_READ_U16(gkDomain1PD + gkOffIStatus); //READ DATA 0x6041 status
// send process data
ecrt_domain_queue(gkDomain1); //MARK THE DOMAIN DATA AS READY FOR EXCHANGE
ecrt_master_send(gkMaster); //SEND ALL QUEUED DATAGRAMS
usleep(1000); //WAIT 1mS
if (op_flag) {
printf("1-Position: %d Status: 0x%x\n", ipos, istatus);
break;
}
}
fprintf(stdout, "8. Got OP state.\n");
if(argc > 1) {
//перейти в позицию
const int cmdpos = atoi(argv[1]);
printf("cmd pos: %d\n", cmdpos);
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0xF); //0x6040 ControlWord
EC_WRITE_U8(gkDomain1PD + gkOffOMode, 1); // 0x6060 Profile position mode // 3 - for velocity mode, 1- for position mode
EC_WRITE_S32(gkDomain1PD + gkOffPVel, 1000000); // 0x60ff profile velocity // gkOffTVel - for velocity mode
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
//wait
for (uint32_t i = 0; i < 200; ++i) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
}
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
/* comment 2 lines for velocity mode */
EC_WRITE_S32(gkDomain1PD + gkOffOPos, cmdpos);
EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0x11F);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
//wait
for (uint32_t i = 0; i < 200; ++i) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
}
/* ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
EC_WRITE_S32(gkDomain1PD + gkOffOPos, cmdpos);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);*/
//wait
/* for (uint32_t i = 0; i < 1000; ++i) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(1000);
}
*/
timespec tbegin, tend;
::clock_gettime(CLOCK_MONOTONIC, &tbegin);
printf("Time begin: %lds/%ldns\n", tbegin.tv_sec, tbegin.tv_nsec);
const uint32_t kIterationMax = 500000;
uint32_t change_count = 0;
bool target_reached = false;
for (uint32_t j = 0; ; j++) {
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
int32_t ipos_new = EC_READ_S32(gkDomain1PD + gkOffIPos); //READ DATA 0x6064 position
uint16_t istatus_new = EC_READ_U16(gkDomain1PD + gkOffIStatus); //READ DATA 0x6041 status
int32_t imode = EC_READ_S8(gkDomain1PD + gkOffIMode);
int32_t ipvel = EC_READ_S32(gkDomain1PD + gkOffPVel);
int32_t idvel = EC_READ_S32(gkDomain1PD + gkOffDVel);
int32_t iavel = EC_READ_S32(gkDomain1PD + gkOffIVel);
int32_t idpos = EC_READ_S32(gkDomain1PD + gkOffDPos);
int32_t itpos = EC_READ_S32(gkDomain1PD + gkOffOPos);
int32_t icontrol = EC_READ_U16(gkDomain1PD + gkOffOControl);
int16_t iatorq = EC_READ_S16(gkDomain1PD + gkOffITorq);
// int32_t ipdenom = EC_READ_S16(gkDomain1PD + gkOffPDenom);
if (ipos_new != ipos) {
ipos = ipos_new;
change_count++;
printf("Position: %d Status: 0x%x Mode: %d ATorq: %d PVel: %d DVel: %d AVel: %d DPos: %d TPos: %d OControl: 0x%x\n", ipos, istatus, imode, iatorq, ipvel, idvel, iavel, idpos, itpos, icontrol);
}
// position mode
if(! target_reached && ((istatus_new >> 10) & 0x1)) {
::clock_gettime(CLOCK_MONOTONIC, &tend);
printf("Target reached. Pos: %d Status: 0x%x TEnd=%lds/%ldns\n", ipos, istatus, tend.tv_sec, tend.tv_nsec);
target_reached = true;
//break;
}
/* Velocity mode */
if (j == kIterationMax) {
/* clock_gettime(CLOCK_MONOTONIC, &tend);
printf("Iterations=%d, change_count=%d. time_end=%lds/%ldns Stopping...\n", j, change_count, tend.tv_sec, tend.tv_nsec);
EC_WRITE_U16(gkDomain1PD + gkOffOControl, 0x6);
break;
*/
}
ecrt_domain_queue(gkDomain1);
ecrt_master_send(gkMaster);
usleep(100); //WAIT 1mS
}
}
ecrt_master_receive(gkMaster);
ecrt_domain_process(gkDomain1);
printf("...Done. Releasing the master!\n");
// Освобождаем мастер-объект
ecrt_release_master(gkMaster);
return 0;
}
