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

//.........这里部分代码省略.........
			float uroll = 0.005*rollerr  +  0.2 *(rollerr - prollerr[pcounter] ) + 0.000001*rsum;
			float upitch= 0.01 *pitcherr +  0.4 *(pitcherr- ppitcherr[pcounter]) + 0.000001*psum;
			float uyaw  = 0.02 *yawerr   +  0.01*(yawerr  - pyawerr[pcounter]  ) ;

			prollerr[pcounter]  = rollerr;
			ppitcherr[pcounter] = pitcherr;
			pyawerr[pcounter]   = yawerr;
			pcounter++;
			if(pcounter>4)pcounter=0;

			if (uthrotle<1.05){
				uroll  = 0;
				upitch = 0;
				uyaw   = 0;
			}



			float R = uthrotle - uroll  + uyaw;
			float L = uthrotle + uroll  + uyaw;
			float F = uthrotle + upitch - uyaw;
			float B = uthrotle - upitch - uyaw;

			//Limitter
			if(R>2.0)R=2.0;
			if(R<1.0)R=1.0;
			if(L>2.0)L=2.0;
			if(L<1.0)L=1.0;
			if(F>2.0)F=2.0;
			if(F<1.0)F=1.0;
			if(B>2.0)B=2.0;
			if(B<1.0)B=1.0;

			pwm.set_duty_cycle(RIGHT_MOTOR, R);
			pwm.set_duty_cycle(LEFT_MOTOR, L);
			pwm.set_duty_cycle(FRONT_MOTOR, F);
			pwm.set_duty_cycle(REAR_MOTOR, B);


			//////Logging

			//gettimeofday(&tval,NULL);
			//past_time = now_time;
			//now_time = 1000000 * tval.tv_sec + tval.tv_usec;
			//interval = now_time - past_time;
			sprintf(outstr,"%lu %lu %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f"
					,now_time,interval,
					roll,pitch,yaw,
					rroll,rpitch,ryaw,
					gx,gy,gz,
					ax,ay,az,
					mx,my,mz,
					R,L,F,B
			       );
			fs << outstr << endl;

			/*
			   sprintf(aoutstr,"%lf %lf %lf",ax,ay,az);
			   afs << aoutstr << std::endl;

			   sprintf(goutstr,"%lf %lf %lf",gx,gy,gz);
			   gfs << goutstr << std::endl;

			   sprintf(moutstr,"%lf %lf %lf",mx,my,mz);
			   mfs << moutstr << std::endl;
			 */

//.........这里部分代码省略.........

			}

			if (joy_button[11]==1){

				cnt++;

				if (cnt>30){

					M = 2.0;
					printf( "Set PWM MAX!\n" );
					cnt = 0;

				}

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

				cnt++;

				if (cnt>30){

					M = 1.0;
					printf( "Set PWM MIN!\n" );
					cnt = 0;

				}

			}

			if ( M > 2.0 ) M = 2.0;
			if ( M < 1.0 ) M = 1.0;

			pwm.set_duty_cycle(m, M);

			if (joy_button[3]==1){
				break;
			}

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

			}while(interval<2000);

		}
		led.setColor(Colors::Blue);
		while (joy_button[3]==1){
			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;
			}
		}
		while (true){
			read(joy_fd, &js, sizeof(js_event));
			
			switch(js.type & ~JS_EVENT_INIT) {
				case JS_EVENT_AXIS:
					joy_axis[(int)js.number] = js.value;

//.........这里部分代码省略.........
	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);
		
		//write readings on pwm output
		motor.set_duty_cycle(MOTOR_PWM_OUT, ((float)motor_input)/1000.0f); 
		servo.set_duty_cycle(SERVO_PWM_OUT, ((float)servo_input)/1000.0f);
		
		//save values into msg container a
		rem_msg.header.stamp = ros::Time::now();
		rem_msg.temperature = motor_input;
		rem_msg.variance = servo_input;


		dtf = rcin.read(4)-1000;
		speed = 4.0f*PI*R*1000.0f/((float)dtf);
		if(speed < 0 || dtf < 40) speed = 0;

		
		// low pass filtering of the speed with tau = 0.1
		float alpha = (1.0f/freq)/((1.0f/freq)+0.1f);
		speed_filt = alpha*speed + (1.0f-alpha)*speed_filt;

		//save values into msg container for the control readings

		ctrl_msg.header.stamp = ros::Time::now();
		ctrl_msg.temperature = speed_filt;//_filt;
		ctrl_msg.variance = desired_roll;//here it's supposed to be the desired roll


		//debug info
		printf("[Thrust:%d] - [Steering:%d] - [dtf:%4d] - [Speed:%2.2f]\n", motor_input, servo_input, dtf, speed_filt);

		//remote_pub.publish(apub);
		remote_pub.publish(rem_msg);
		control_pub.publish(ctrl_msg);

		ros::spinOnce();

		loop_rate.sleep();

	}


  	return 0;
}