Question about CMUCAM servo output

[iBheat]

I haven't done any coding since the last FRC season so correct me if I am wrong. This is the way I would do it:

#DEFINE pan_allowance 5 //You have to allow for an allowance or your robot will jitter after the position is acquired
#DEFINE tilt_allowance 5 //Same thing as above
#DEFINE desired_tilt 190 //This has to be greater then 127 (which is the center) because if it was 127 then either you'll have to be really far away or the light has to be at the same height as the robot. I just picked 190 :)

unsigned char tilt;
unsigned char pan;

while (1) //Infinite Loop
{
	CaptureTrackingData(0, 0, 0, 0, 0, 0, 0, 0, &pan, &tilt); //Get the pan and tilt data back from the camera
	if (pan > 127 + pan_allowance) //The camera has to look to the right inorder to look at the center of the box
		Drive(255, 40); //Turn the robot to the right
	elseif (pan < 127 - pan_allowance) //The camera has to look to the left inorder to look at the center of the box
		Drive(40, 255); //Turn the robot to the left
	elseif (til > desired_tilt + tilt_allowance) //The camera has to look way too far up inorder to see the light meaning its too close
		Drive(0, 0); //Move the robot backwards
	elseif (tilt < desired_tilt - tilt_allowance) // The camera has to look below what it should to see the light so its too far away
		Drive(255, 255); //Drive forward
	else //If everything is right
		Drive(127, 127); //Don't drive!
}

Basically what happens is that first it aligns it so that it is parallel to the light and then it either moves forward or backwards to be with the light. I am not sure how you are doing the drive function but I assumed that what you are trying to do is Drive(Left Side, Right Side) and to go forward you would set both sides to a value greater then 127 and to go backwards you would set both sides to a value less then 127 which is what I did. Also you would want an allowance on both the pan and tilt which was discovered through testing that it would be very jittery if you didn't have it and it makes sense because the center of the rectangle might shift a little bit with each frame. Also you don't need a bunch of nested while statements unless you have some other piece of code that resets the values for the motors on the left and right side of the robot. Also I set the speed of the motors when going forward and backwards to full speed this is probably not a good idea since it'll pass it both ways again making it jittery what you want to do is test it and see what speed you can effectively track the green light at!