Need help with gyro turn

Technical Programming
1

I have gyro onto the robot. As soon as i get the angle from the cam. I want my gyro to turn exactly at that angle. so for e.g. my pan angle is 34 degrees than i want my robot to turn 34 degree. However, my Gyro just can’t do that. Can someone help me how can i code it so it turns the angle i want it to turn? this is what i have.

/VARIABLE/
int unit_speed = 50;
/********************/

angle = ((((int)PAN_SERVO - 124) * 65)/124); /* +/- 65 degree [0 to 148] */
error_angle = (angle - (int)temp_gyro_angle); /Check the angle difference between camera and Gyro/
printf("Error_angle=%d
", error_angle); /Print Error Angle/

	if (error_angle < -5)								/*If Error angle is less than 5 degrees*/
	{
		speed = ((error_angle * unit_speed)/100);		/*PID Forumula to control the speed as we move close to target*/
		printf("Target is to the left

"); /Prints if target is to left/

		while(speed > 5)
		{
			pwm13 = pwm14= 127;								/*Turning Left and the turn is controlled, see below comment*/
			pwm15 = pwm16 = 127 + speed;					/*The motor movements are controlled by the PID Speed Calculated*/
		
			printf("PWM 13=%d	pwm14=%d	pwm14=%d	pwm15=%d

", (int)pwm13, (int) pwm14, (int) pwm15, (int) pwm15); /Printing motor values for Debugging/
printf("Speed=%d
", speed); /Tells us what is the value of speed according to PID Calculation/
}
}

	if (error_angle > 5)								/*If Error angle is greater than 5 degrees*/
	{
		speed = ((error_angle * unit_speed)/100);		/*PID Formula to control the speed as we move close to the target*/
		printf("Target is to the right

"); /Prints that the target is to right/

		while(speed > 5)
		{
			pwm13 = pwm14 = 127 + speed;					/*Turning Right and the turn is controlled, see below comment*/
			pwm15 = pwm16 = 127;							/*The motor movements are controlled by the PID Speed Calculated*/

			printf("PWM 13=%d	pwm14=%d	pwm14=%d	pwm15=%d

", (int)pwm13, (int) pwm14, (int) pwm15, (int) pwm15); /Printing motor values for Debugging/
printf("Speed=%d
", speed); /Tells us what is the value of speed according to PID Calculation/
}
}

	if (error_angle > -5 && error_angle < 5)			/*If the error angle is between -5 degree and + 5 degree*/
	{
		printf("Target is straight on");				/*Prints that we are right on target*/
		pwm13 = pwm14= pwm15 = pwm16 = 127;				/*Stops our motor for a while*/
		printf("PWM 13=%d	pwm14=%d	pwm15=%d	pwm16=%d

", (int)pwm13, (int) pwm14, (int) pwm15, (int) pwm16); /Printing motor values for Debugging/
printf("Speed=%d
", speed); /Tells us what is the value of speed according to PID Calculation/

2

int unit_speed = 50;

3

…hmmmm
Why don’t you, first, try something a bit easier. Let your value of angle be the error term for your P controller. As long as your camera holds its tracking, you should see your robot turn to align to the camera and the value of angle will drive to 0. 0 error, 0 angle :cool: Then keep an eye on the value you are getting from your gyro calculations (I am assuming that by gyro, you mean yaw rate sensor, and you are integrating yaw rate to calculate an angle) There could be error creeping into your gyro calculations that is causing problems with your controller.