View Single Post
  #1   Spotlight this post!  
Unread 11-03-2007, 15:26
magical hands magical hands is offline
Jigar Patel
AKA: Jigar Patel
FRC #2185 (Etobicoke CI)
Team Role: Mentor
 
Join Date: Dec 2003
Rookie Year: 2004
Location: TORONTO
Posts: 93
magical hands is on a distinguished road
Need help with gyro turn

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\r\n", 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\r\n"); /*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\r\n", (int)pwm13, (int) pwm14, (int) pwm15, (int) pwm15); /*Printing motor values for Debugging*/
printf("Speed=%d\r\n", 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\n"); /*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\r\n", (int)pwm13, (int) pwm14, (int) pwm15, (int) pwm15); /*Printing motor values for Debugging*/
printf("Speed=%d\r\n", 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 \r\n", (int)pwm13, (int) pwm14, (int) pwm15, (int) pwm16); /*Printing motor values for Debugging*/
printf("Speed=%d \r\n", speed); /*Tells us what is the value of speed according to PID Calculation*/

Last edited by magical hands : 11-03-2007 at 15:34.