Esempio n. 1
0
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);
}
Esempio n. 2
0
File: main.c Progetto: Beckhoff/CCAT
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);
}
Esempio n. 3
0
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++;
	}
}
Esempio n. 4
0
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_);
}
Esempio n. 5
0
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);
}
Esempio n. 6
0
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);
}
Esempio n. 7
0
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();

	



    }
}
Esempio n. 9
0
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);

		

	}
}
Esempio n. 10
0
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++;
	}
}
Esempio n. 11
0
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);
}
Esempio n. 12
0
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();

	



    }
}
Esempio n. 14
0
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;
    }

}
Esempio n. 15
0
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;
}
Esempio n. 16
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();
    }
}
Esempio n. 17
0
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;
}