C++ PWM::set_period方法代码示例

int main()
{
	char sensor_name[]="mpu";
	float rollerr,pitcherr,yawerr;
	float prollerr[5],ppitcherr[5],pyawerr[5];
	int pcounter=0;	
	int breakflag=0;
	float rsum=0.0;
	float psum=0.0;
	float ysum=0.0;	

	//-------- IMU setting -------------------
	imu = create_inertial_sensor(sensor_name);

	if (!imu) {
		printf("Wrong sensor name. Select: mpu or lsm\n");
		return EXIT_FAILURE;
	}

	if (!imu->probe()) {
		printf("Sensor not enable\n");
		return EXIT_FAILURE;
	}

	imuSetup();


	PWM pwm;
	RGBled led;
	js_event js;

	if(!led.initialize()) return EXIT_FAILURE;


	//-------- PS3 Controller setting --------
	int joy_fd(-1), num_of_axis(0), num_of_buttons(0);
	char name_of_joystick[80];
	vector<char> joy_button;
	vector<int> joy_axis;

	if((joy_fd = open(JOY_DEV, O_RDONLY)) < 0) {
		printf("Failed to open %s", JOY_DEV);
		cerr << "Failed to open " << JOY_DEV << endl;
		return -1;
	}

	ioctl(joy_fd, JSIOCGAXES, &num_of_axis);
	ioctl(joy_fd, JSIOCGBUTTONS, &num_of_buttons);
	ioctl(joy_fd, JSIOCGNAME(80), &name_of_joystick);

	joy_button.resize(num_of_buttons, 0);
	joy_axis.resize(num_of_axis, 0);

	printf("Joystick: %s axis: %d buttons: %d\n", name_of_joystick, num_of_axis, num_of_buttons);

	fcntl(joy_fd, F_SETFL, O_NONBLOCK); // using non-blocking mode
	


	if (check_apm()) {
		return 1;
	}

	
	for (int i=0; i<10000; i++){
		imuLoop();
		usleep(1000);
		if(i%1000==0){
			printf("#");
			fflush(stdout);
				
		}
	}
	
	
	for (int i=0;i<4;i++){
		if (!pwm.init(i)) {
			fprintf(stderr, "Output Enable not set. Are you root?\n");
			return 0;
		}
		pwm.enable(i);
		pwm.set_period(i, 500);
	}
	printf("\nReady to Fly !\n");

	////// Log system setting
	char filename[] = "flightlog.txt";
	char outstr[255];
	std::ofstream fs(filename);
	
	char afilename[] = "atest.txt";
	char aoutstr[255];
	std::ofstream afs(afilename);

	char gfilename[] = "gtest.txt";
	char goutstr[255];
	std::ofstream gfs(gfilename);

	char mfilename[] = "mtest.txt";
	char moutstr[255];
//.........这里部分代码省略.........

int main()
{

	int breakflag=0;
	float M = 1.0;
	short m = 0;
	short cnt = 0;

	PWM pwm;
	RGBled led;
	js_event js;

	if(!led.initialize()) return EXIT_FAILURE;


	//-------- PS3 Controller setting --------
	int joy_fd(-1), num_of_axis(0), num_of_buttons(0);
	char name_of_joystick[80];
	vector<char> joy_button;
	vector<int> joy_axis;

	if((joy_fd = open(JOY_DEV, O_RDONLY)) < 0) {
		printf("Failed to open %s", JOY_DEV);
		cerr << "Failed to open " << JOY_DEV << endl;
		return -1;
	}

	ioctl(joy_fd, JSIOCGAXES, &num_of_axis);
	ioctl(joy_fd, JSIOCGBUTTONS, &num_of_buttons);
	ioctl(joy_fd, JSIOCGNAME(80), &name_of_joystick);

	joy_button.resize(num_of_buttons, 0);
	joy_axis.resize(num_of_axis, 0);

	printf("Joystick: %s axis: %d buttons: %d\n", name_of_joystick, num_of_axis, num_of_buttons);

	fcntl(joy_fd, F_SETFL, O_NONBLOCK); // using non-blocking mode

	for ( int i = 0 ; i < 4 ; i++ ) {

		if ( !pwm.init(i) ) {
			fprintf(stderr, "Output Enable not set. Are you root?\n");
			return 0;
		}
		pwm.enable(i);
		pwm.set_period(i, 500);

	}

	printf("\nStart thrust test !\n");

	///// Clock setting
	struct timeval tval;
	unsigned long now_time,past_time,interval;

	gettimeofday(&tval,NULL);
	now_time=1000000 * tval.tv_sec + tval.tv_usec;
	past_time = now_time;
	interval = now_time - past_time;

	printf("set motor 'RIGHT'\n");

//==========================  Main Loop ==============================
	while(true) {

		led.setColor(Colors::Red);

		while(true) {

			gettimeofday(&tval,NULL);
			past_time = now_time;
			now_time=1000000 * tval.tv_sec + tval.tv_usec;
			interval = now_time - past_time;

			//js_event js;

			read(joy_fd, &js, sizeof(js_event));

			switch(js.type & ~JS_EVENT_INIT) {
				case JS_EVENT_AXIS:
					joy_axis[(int)js.number] = js.value;
					break;
				case JS_EVENT_BUTTON:
					joy_button[(int)js.number] = js.value;
					//printf("%5d\n %5d\n",(int)js.number,js.value);
					break;
			}

			if (joy_button[12]==1){

				if (joy_button[4]==1){

					M = M + 0.1;
					printf ( "PWM UP : %f\n" ,M );

				}
				if (joy_button[6]==1){

					M = M - 0.1;
					printf ( "PWM down : %f\n" ,M );
//.........这里部分代码省略.........

//.........这里部分代码省略.........
	/* Initialize the PID Stuff */
	/****************************/

	currentRoll = 0;
	currentTime = ros::Time::now();
	previousTime = ros::Time::now();
	Kierr = 0;
	err = 0;
	derr = 0;
	derr_filt = 0;
	
	/*******************************************/
	/* Initialize the RC input, and PWM output */
	/*******************************************/

	RCInput rcin;
	rcin.init();
	PWM servo;
	PWM motor;

	if (!motor.init(MOTOR_PWM_OUT)) {
		fprintf(stderr, "Motor Output Enable not set. Are you root?\n");
		return 0;
    	}

	if (!servo.init(SERVO_PWM_OUT)) {
		fprintf(stderr, "Servo Output Enable not set. Are you root?\n");
		return 0;
    	}

	motor.enable(MOTOR_PWM_OUT);
	servo.enable(SERVO_PWM_OUT);

	motor.set_period(MOTOR_PWM_OUT, 50); //frequency 50Hz
	servo.set_period(SERVO_PWM_OUT, 50); 

	int motor_input = 0;
	int servo_input = 0;

	sensor_msgs::Temperature rem_msg;
	sensor_msgs::Temperature ctrl_msg;

	float desired_roll = 0;

	//speed in m/s
	float speed = 0;
	float speed_filt = 0;
	int dtf = 0;// dtf read from arduino. dtf = dt*4 in msec
	float R = 0.0625f; //Rear Wheel Radius

	while (ros::ok())
	{

		//Throttle saturation
		if(rcin.read(3) >= saturation)
			motor_input = saturation;
		else
			motor_input = rcin.read(3);

		//read desired roll angle with remote ( 1250 to 1750 ) to range of -30 to 30 deg
		desired_roll = -((float)rcin.read(2)-1500.0f)*MAX_ROLL_ANGLE/250.0f;
		ROS_INFO("rcin usec = %d    ---   desired roll = %f", rcin.read(2), desired_roll);

		//calculate output to servo from pid controller
		servo_input = pid_Servo_Output(desired_roll);