示例#1
0
int main()
{
	int slave_number = 0;

	/* Initialize EtherCAT Master */
	init_master(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	printf("starting Master application\n");
	while(1)
	{
		/* Update the process data (EtherCAT packets) sent/received from the node */
		pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

		if(master_setup.op_flag) /*Check if the master is active*/
		{
			/* Write Process data */
			slv_handles[slave_number].motorctrl_out = 12;
			slv_handles[slave_number].torque_setpoint = 200;
			slv_handles[slave_number].speed_setpoint = 4000;
			slv_handles[slave_number].position_setpoint = 10000;
			slv_handles[slave_number].operation_mode = 125;

			/* Read Process data */
			printf("Status: %d\n", slv_handles[slave_number].motorctrl_status_in);
			printf("Position: %d \n", slv_handles[slave_number].position_in);
			printf("Speed: %d\n", slv_handles[slave_number].speed_in);
			printf("Torque: %d\n", slv_handles[slave_number].torque_in);
			printf("Operation Mode disp: %d\n", slv_handles[slave_number].operation_mode_disp);
		}
	}

	return 0;
}
int main()
{
	float target_torque = -25.0; 	// mNm
	float actual_torque = 0;		// mNm
	float tolerance = 0.76; 		// mNm
	int actual_position = 0;		// ticks
	int actual_velocity = 0;		// rpm
	int ack = 0;
	int sdo_update = 1;             // 1- yes / 0 - no
	int slave_number = 0;

	/* Initialize Ethercat Master */
	init_master(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	/* Initialize all connected nodes with Mandatory Motor Configurations (specified under config/motor/)*/
	init_nodes(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	/* Initialize torque parameters */
	initialize_torque(slave_number, slv_handles);

	/* Initialize the node specified with slave_number with Profile Torque(TQ) configurations (specified under config/motor/)*/
	set_operation_mode(TQ, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	/* Enable operation of node in TQ mode */
	enable_operation(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);


	while(1)
	{
		/* Update the process data (EtherCat packets) sent/received from the node */
		pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

		if(master_setup.op_flag)		/*Check if the master is active*/
		{
			/* Send target torque for the node specified by slave_number */
			set_torque_mNm(target_torque, slave_number, slv_handles);

			/* Check if target torque is reached with specified tolerance */
			ack = target_torque_reached(slave_number, target_torque, tolerance, slv_handles);

			/* Read actual node sensor values */
			actual_torque= get_torque_actual_mNm(slave_number, slv_handles);
			actual_position = get_position_actual_ticks(slave_number, slv_handles);
			actual_velocity = get_velocity_actual_rpm(slave_number, slv_handles);
			printf("target_torque %f \n",target_torque);
			printf("actual_torque %f position %d velocity %d ack %d\n", actual_torque, actual_position, actual_velocity, ack);
		}

		if(ack == 1)
		{
			break;
		}
	}

	printf("reached \n");


	ack = 0;
	while(!ack)
	{
		pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

		if(master_setup.op_flag)	/*Check if the master is active*/
		{
			actual_torque =  get_torque_actual_mNm(slave_number, slv_handles);
			actual_position = get_position_actual_ticks(slave_number, slv_handles);
			actual_velocity = get_velocity_actual_rpm(slave_number, slv_handles);
			if(actual_torque > tolerance || actual_torque < -tolerance)
			{
				/* Quick stop Profile Torque mode (for emergency) */
				quick_stop_torque(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
				ack = 1;
			}
			printf("actual_torque %f position %d velocity %d ack %d\n", actual_torque, actual_position, actual_velocity, ack);
		}
	}
	printf("reached \n");

	/* Regain control of node to continue after quick stop */
	renable_ctrl_quick_stop(TQ, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	set_operation_mode(TQ, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	enable_operation(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	/*target_torque = 15.0; // mNm
	while(1)
	{
		pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

		if(master_setup.op_flag)	// Check if the master is active
		{

			set_torque_mNm(target_torque, slave_number, slv_handles);
			ack = target_torque_reached(slave_number, target_torque, tolerance, slv_handles);
			actual_torque = get_torque_actual_mNm(slave_number, slv_handles);
			printf("target_torque %f \n",target_torque);
			printf("actual_torque %f ack %d\n", actual_torque, ack);
		}

		if(ack == 1)
		{
			break;
		}
	}
*/
	/*printf("reached \n");

	while(1)
	{
		pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

		if(master_setup.op_flag)//Check if we are up
		{

			actual_torque = get_torque_actual_mNm(slave_number, slv_handles);
			printf("actual_torque %f \n",actual_torque);
		}
	}*/


	/* Shutdown node operations */
	shutdown_operation(TQ, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	return 0;
}
int main() {

    int target_velocity = 2000; //rpm
    int acceleration = 1000; //rpm/s
    int deceleration = 1000; //rpm/s

    int actual_velocity = 0; // rpm
    int actual_position; // ticks
    float actual_torque; // mNm
    int steps = 0;
    int velocity_ramp = 0; // rpm

    /* Initialize Ethercat Master */
    init_master(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

    /* Initialize all connected nodes with Mandatory Motor Configurations (specified in config/motor/)*/
    init_nodes(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

    /* Initialize the node specified with ECAT_SLAVE_0 with CSV configurations (specified in config/motor/)*/
    set_operation_mode(CSV, ECAT_SLAVE_0, &master_setup, slv_handles,
            TOTAL_NUM_OF_SLAVES);

    /* Enable operation of node in CSV mode */
    enable_operation(ECAT_SLAVE_0, &master_setup, slv_handles,
            TOTAL_NUM_OF_SLAVES);

    /* Initialize velocity profile parameters */
    steps = init_velocity_profile_params(target_velocity,
            actual_velocity, acceleration, deceleration, ECAT_SLAVE_0,
            slv_handles);

    /* catch interrupt signal */
    signal(SIGINT, INThandler);

    /* Just for better printing result */
    printf("\n");
    system("setterm -cursor off");

    while(1)
    {
        if (master_setup.op_flag && actual_velocity == 0) /*Check if the master is active and we haven't started moving yet*/
        {
            for (int step = 1; step < steps + 1; step++) {
                /* Update the process data (EtherCat packets) sent/received from the node */
                pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

                /* Generate target velocity steps */
                velocity_ramp = generate_profile_velocity(step, ECAT_SLAVE_0,
                        slv_handles);

                /* Send target velocity for the node specified by ECAT_SLAVE_0 */
                set_velocity_rpm(velocity_ramp, ECAT_SLAVE_0, slv_handles);

                /* Read actual node sensor values */
                actual_velocity
                        = get_velocity_actual_rpm(ECAT_SLAVE_0, slv_handles);
                actual_position = get_position_actual_ticks(ECAT_SLAVE_0,
                        slv_handles);
                actual_torque = get_torque_actual_mNm(ECAT_SLAVE_0, slv_handles);

                printf("\r    Velocity: %d    Position: %d    Torque: %f        ",
                        actual_velocity, actual_position, actual_torque);
            }
        }
        else if (break_loop){
            break;
        }
        else {
            /* Update the process data (EtherCat packets) sent/received from the node */
            pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);
            /* Read actual node sensor values */
            actual_velocity
                    = get_velocity_actual_rpm(ECAT_SLAVE_0, slv_handles);
            actual_position = get_position_actual_ticks(ECAT_SLAVE_0,
                    slv_handles);
            actual_torque = get_torque_actual_mNm(ECAT_SLAVE_0, slv_handles);

            printf("\r    Velocity: %d    Position: %d    Torque: %f        ",
                    actual_velocity, actual_position, actual_torque);
        }
    }
    printf("\n");

    /* Quick stop velocity mode (for emergency) */
    quick_stop_velocity(ECAT_SLAVE_0, &master_setup, slv_handles,
            TOTAL_NUM_OF_SLAVES);

    /* Regain control of node to continue after quick stop */
    renable_ctrl_quick_stop(CSV, ECAT_SLAVE_0, &master_setup, slv_handles,
            TOTAL_NUM_OF_SLAVES);

    set_operation_mode(CSV, ECAT_SLAVE_0, &master_setup, slv_handles,
            TOTAL_NUM_OF_SLAVES);

    enable_operation(ECAT_SLAVE_0, &master_setup, slv_handles,
            TOTAL_NUM_OF_SLAVES);

    /* Shutdown node operations */
    shutdown_operation(CSV, ECAT_SLAVE_0, &master_setup, slv_handles,
            TOTAL_NUM_OF_SLAVES);

    /* Just for better printing result */
    system("setterm -cursor on");

    return 0;
}
int main()
{
	int flag = 0;

	int acceleration = 350;				// rpm/s
	int deceleration = 350;   			// rpm/s
	int velocity = 350;					// rpm
	int actual_position = 0;			// ticks
	int target_position = 0;			// ticks
	int actual_velocity;				// rpm
	float actual_torque;				// mNm
	int steps = 0;
	int i = 1;
	int position_ramp = 0;
	int ack;
	int home_velocity = 250;      		// rpm
	int home_acceleration = 250;		// rpm/s
	int sdo_update = 1;                 // 1- yes / 0 - no
	int slave_number = 0;

	/* Initialize Ethercat Master */
	init_master(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	/* Initialize torque parameters */
	initialize_torque(slave_number, slv_handles);

	/* Initialize all connected nodes with Mandatory Motor Configurations (specified under config/motor/)*/
	init_nodes(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	/* Initialize the node specified with slave_number with Homing configurations (specified under config/motor/)*/
	set_operation_mode(HM, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	/* Enable operation of node in Homing mode */
	enable_operation(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	start_homing(&master_setup, slv_handles, home_velocity, home_acceleration, slave_number, TOTAL_NUM_OF_SLAVES);

	/* Shutdown Homing Operation */
	shutdown_operation(HM, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	/*
	 * Acquire actual position from the node a few times after homing and
	 * set it as target position. (wait for controller to settle)
	 */
	i = 0;
	int difference = 1500;
	int previous=0;
	while(1)
	{

		pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

		if(master_setup.op_flag)	//Check if the master is active
		{
			actual_position = get_position_actual_ticks(slave_number, slv_handles);
			target_position = actual_position;
			set_profile_position_ticks(target_position, slave_number, slv_handles);
			i = i+1;

			difference = actual_position - previous;
			if(difference <1 && difference >-1)
			{
				//printf(" difference %d act %d\n",difference, actual_position);
				break;
			}

			previous = actual_position;

			//printf(" difference %d act %d\n",difference, actual_position);
		}

	}


	/* Now initialize the node specified with slave_number with CSP configurations (specified under config/motor/)*/
	set_operation_mode(CSP, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	/* Enable operation of node in CSP mode */
	enable_operation(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	/* Initialize position profile parameters */
	initialize_position_profile_limits(slave_number, slv_handles);//*/



	/* Now setting a new target position after homing */
	target_position = get_position_actual_ticks(slave_number, slv_handles) + 5000;
	if(target_position > 35000)
		target_position = 35000;
	printf(" target_position %d\n", target_position);

	/*calculate the no. of steps for the profile*/
	steps = init_position_profile_params(target_position, actual_position, velocity, acceleration, \
			deceleration, slave_number, slv_handles);

	/* Execute the position profile steps in a loop */
	i = 1;
	while(1)
	{
		/* Update the process data (EtherCat packets) sent/received from the node */
		pdo_handle_ecat(&master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

		if(master_setup.op_flag)	/*Check if the master is active*/
		{
			if(i<steps)
			{
				/* Generate target position steps */
				position_ramp = generate_profile_position(i, slave_number, slv_handles);

				/* Send target position for the node specified by slave_number */
				set_position_ticks(position_ramp, slave_number, slv_handles);
				i = i+1;
			}
			if(i >= steps)
			{
				printf("ack received");
				break;
			}
			//printf("actual position %d \n", get_position_actual_ticks(slave_number, slv_handles));

			/* Read actual node sensor values */
			actual_position = get_position_actual_ticks(slave_number, slv_handles);
			actual_velocity = get_velocity_actual_rpm(slave_number, slv_handles);
			actual_torque = get_torque_actual_mNm(slave_number, slv_handles);
			printf("actual position %d actual velocity %d actual_torque %f\n", actual_position, actual_velocity, actual_torque);
		}
	}

	/* Quick stop position mode (for emergency) */
	quick_stop_position(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	/* Regain control of node to continue after quick stop */
	renable_ctrl_quick_stop(CSP, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);

	set_operation_mode(CSP, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);			// set operation mode to CSP

	enable_operation(slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);				// enable operation

	/* Shutdown node operations */
	shutdown_operation(CSP, slave_number, &master_setup, slv_handles, TOTAL_NUM_OF_SLAVES);			// stop the node operation. or can continue with new position profile

	return 0;
}