示例#1
0
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];
	std::ofstream mfs(mfilename);

	///// 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;

//==========================  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;
			imuLoop();

			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;
			}


			int stickRx=joy_axis[2];
			int stickRy=joy_axis[3];
			int stickLx=joy_axis[0];
			int stickLy=joy_axis[1];

			if(stickRx> 23170)stickRx= 23170;
			if(stickRx<-23170)stickRx=-23170;	

			if(stickRy> 23170)stickRy= 23170;
			if(stickRy<-23170)stickRy=-23170;	

			if(stickLx> 23170)stickLx= 23170;
			if(stickLx<-23170)stickLx=-23170;	

			if(stickLy> 23170)stickLy= 23170;
			if(stickLy<-23170)stickLy=-23170;	

			float uthrotle=(-(float)stickRy/23170.0)*1.0+1.0;
			float rpitch  =( (float)stickLy/23170.0)*10.0;
			float rroll   =( (float)stickLx/23170.0)*10.0;
			float ryaw    =( (float)stickRx/23170.0)*10.0;

			/*
			   Itolab Drone Configuration

			        F
			        |
			        |
			   L----+----R
			        |
			        |
			        B
			 */

			//------------- Stabilize Control -----------------

			rollerr = rroll  - roll;
			pitcherr= rpitch - pitch;
			yawerr  = ryaw   + gz;

			rsum = rsum + rollerr;
			psum = psum + pitcherr;
			ysum = 0.0;

			if (uthrotle<1.05){
				rsum = 0;
				psum = 0;
			}

			if(rsum> 20000.0)rsum = 20000.0;
			if(rsum<-20000.0)rsum =-20000.0;
			if(psum> 20000.0)psum = 20000.0;
			if(psum<-20000.0)psum =-20000.0;
			//if(ysum>1.0)ysum=1.0;
			//if(ysum<0.0)ysum=0.0;

			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[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;
					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[3]==1){
				break;
			}
			if (joy_button[0]==1){
				breakflag=1;
				break;
			}
			else breakflag = 0;
		}
		if (breakflag == 1)break;
		else breakflag = 0;

		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;
			}
		}
	}
	fs.close();
	//afs.close();
	//gfs.close();
	//mfs.close();
	led.setColor(Colors::Green);
	return 0;
}
示例#2
0
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 );

				}

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

				if (joy_button[4]==1){

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

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

					M = M - 0.001;
					printf ( "PWM down : %f\n" ,M );

				}

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

				if (joy_button[4]==1){

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

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

					M = M - 0.0001;
					printf ( "PWM down : %f\n" ,M );

				}

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

				if (joy_button[4]==1){

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

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

					M = M - 0.0001;
					printf ( "PWM down : %f\n" ,M );

				}

			}

			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;
					break;
				case JS_EVENT_BUTTON:
					joy_button[(int)js.number] = js.value;
					//printf("%5d\n %5d\n",(int)js.number,js.value);
					break;
			}

			M = 1.0;

			if (joy_button[4]==1){
				m = 2;
				printf("set motor 'FRONT'\n");
			}
			if (joy_button[5]==1){
				m = 0;
				printf("set motor 'RIGHT'\n");
			}
			if (joy_button[6]==1){
				m = 3;
				printf("set motor 'REAR'\n");
			}
			if (joy_button[7]==1){
				m = 1;
				printf("set motor 'LEFT'\n");
			}
			if (joy_button[3]==1){
				break;
			}
			if (joy_button[0]==1){
				breakflag=1;
				break;
			}
			else breakflag = 0;
		}
		if (breakflag == 1)break;
		else breakflag = 0;

		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;
			}
		}
	}

	led.setColor(Colors::Green);

	return 0;

}
int main(int argc, char **argv)
{
	ROS_INFO("Start");
	int saturation = 2000;
	int freq = 100;
	Kp = 0.6;
	Ki = 2;
	Kd = 0.01;
	servo_trim = 1500;

	ROS_INFO("number of argc %d", argc);

	if(argc == 1)
	{
		//case with default params
	}
	else if(argc == 2)
	{
		//case with frequency
		if(atoi(argv[1]) > 0 )
			freq = atoi(argv[1]);
		else
		{
			ROS_INFO("Frequency must be more than 0");
			return 0;
		}
	}
	else if(argc == 3)
	{
		//case with frequency and saturation
		if(atoi(argv[1]) > 0 )
			freq = atoi(argv[1]);
		else
		{
			ROS_INFO("Frequency must be more than 0");
			return 0;
		}
	
		if(atoi(argv[2]) > 2000) saturation = 2000;
		else saturation = atoi(argv[2]);
	}
	else if(argc == 6)
	{
		//case with frequency and saturation
		if(atoi(argv[1]) > 0 )
			freq = atoi(argv[1]);
		else
		{
			ROS_INFO("Frequency must be more than 0");
			return 0;
		}
	
		if(atoi(argv[2]) > 2000) saturation = 2000;
		else saturation = atoi(argv[2]);

		Kp = atof(argv[3]);
		Ki = atof(argv[4]);
		Kd = atof(argv[5]);
		
	}
	else if(argc == 7)
	{
		//case with frequency and saturation
		if(atoi(argv[1]) > 0 )
			freq = atoi(argv[1]);
		else
		{
			ROS_INFO("Frequency must be more than 0");
			return 0;
		}
	
		if(atoi(argv[2]) > 2000) saturation = 2000;
		else saturation = atoi(argv[2]);

		Kp = atof(argv[3]);
		Ki = atof(argv[4]);
		Kd = atof(argv[5]);

		servo_trim = atoi(argv[6]);
		
	}
	else
	{
		ROS_INFO("not enough arguments ! Specify prbs value and throttle saturation.");
		return 0;
	}

	ROS_INFO("frequency %d, and saturation  : %d, Trim", freq, saturation, servo_trim);


 	/***********************/
	/* Initialize The Node */
	/***********************/
	ros::init(argc, argv, "remote_reading_handler");
	ros::NodeHandle n;
	ros::Publisher remote_pub = n.advertise<sensor_msgs::Temperature>("remote_readings", 1000);
	ros::Publisher control_pub = n.advertise<sensor_msgs::Temperature>("control_readings", 1000);
	
	//subscribe to imu topic
	ros::Subscriber imu_sub = n.subscribe("imu_readings", 1000, read_Imu);

	//running rate = freq Hz
	ros::Rate loop_rate(freq);

	/****************************/
	/* 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);
		
		//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;
}