[bear24rw]

Here is the core function that Team 11 used to drive to the rack, we were very successful towards the end of the season with this function

void approach_rack(void)
{
	static unsigned char approach_state = PIVOT_ROBOT;		// First state of approach rack
	static int temp_pan_servo = 127;
	static int temp_tilt_servo = 127;
	static int servo_pan_error = 0;
	static int servo_tilt_error = 0;
	static int last_servo_tilt = 0;
	static int encoder_error = 0;
	static int combined_encoder_count = 0;
	static int last_right_encoder_count = 0;
	static int break_friction = 0;

	if(Get_Tracking_State() == TARGET_IN_VIEW || Get_Tracking_State() == CAMERA_ON_TARGET) // CHECK IF THE CAMERA SEES THE TARGET, SHOULD BE FASTER
	{																						//	THAN JUST CHECKING TO SEE IF IT'S ON IT,
		temp_pan_servo = PAN_SERVO;															//		WITH JUST A SMALL SACRIFICE IN ACCURACY
		temp_tilt_servo = TILT_SERVO;
		servo_pan_error = (temp_pan_servo - SERVO_PAN_TARGET);
		servo_tilt_error = (temp_tilt_servo - SERVO_TILT_TARGET);
		service_brakes(0);

		switch (approach_state)
		{
			case PIVOT_ROBOT:		//ROBOT PIVOTING

				if(servo_pan_error >= SERVO_PAN_DEADZONE)
				{
					approach_rack_x =  RIGHT_TURN_SPEED + break_friction; //RIGHT TURN SPEED
					//printf("PIVOTING RIGHT... \r\n");
				}
				else if(servo_pan_error <= -1*SERVO_PAN_DEADZONE)
				{
					approach_rack_x =  LEFT_TURN_SPEED - break_friction;	//LEFT TURN SPEED
					//printf("PIVOTING LEFT... \r\n");
				}
				else
				{
					approach_rack_x = 127;
				//	printf("PIVOTING DONE... \r\n");
					approach_state = APPROACH_AND_SWERVE;
				}

	//		printf("friction: %d\r\n",break_friction);

				if (Get_Encoder_2_Count() >= last_right_encoder_count - 2 && Get_Encoder_2_Count() <= last_right_encoder_count + 2 )
				{
					break_friction += 2;
				}
				else
				{
					break_friction -= 3;
				}
				if (break_friction < 0)
				{
					break_friction = 0;
				}

			break;

			case APPROACH_AND_SWERVE:	//APPROACH AND SWERVE

				if(servo_pan_error >= PAN_SWERVE_DEADZONE || servo_pan_error <= -1*PAN_SWERVE_DEADZONE) // CHECK TO SEE IF WE ARE TOO FAR OFF TO SWERVE
				{
					approach_rack_x = 127;
					approach_rack_y = 127;
					approach_state = PIVOT_ROBOT;
				}
				else
				{

					if(servo_tilt_error >= SERVO_TILT_DEADZONE || servo_tilt_error <= -1*SERVO_TILT_DEADZONE)
					{
						approach_rack_y = 127 - (1.75 * servo_tilt_error);	//multiplier was 1

						if(approach_rack_y >= FORWARD_SPEED)
						{
							approach_rack_y = FORWARD_SPEED;
						//	printf("FORWARD FAST\r\n");
						}
						else if(approach_rack_y <= MIN_FORWARD_SPEED && approach_rack_y > 127)
						{
							approach_rack_y = MIN_FORWARD_SPEED; //MINIMUM FORWARD SPEED, DON'T WANT TO GO TO SLOW
						//	printf("FORWARD SLOW\r\n");
						}
						else if(approach_rack_y <= BACKWARD_SPEED) 	//IF WE OVERSHOT ON THE Y
						{
							approach_rack_y = BACKWARD_SPEED;
						//	printf("BACKWARDS FAST\r\n");
						}
						else if(approach_rack_y >= MIN_BACKWARD_SPEED && approach_rack_y < 127)
						{
							approach_rack_y = MIN_BACKWARD_SPEED; //MINIMUM FORWARD SPEED, DON'T WANT TO GO TO SLOW
						//	printf("BACKWARDS SLOW\r\n");
						}


					}
					else
					{
						approach_rack_x = 127;
						approach_rack_y = 127;
						service_brakes(1);
						approach_state = AT_TARGET;
					}

					if(servo_pan_error >= SERVO_PAN_DEADZONE || servo_pan_error <= -1*SERVO_PAN_DEADZONE)
					{
						approach_rack_x = (servo_pan_error + 127);
					}
					else
					{
						approach_rack_x = 127;
					}

					if(approach_rack_x <= LEFT_SWERVE_SPEED)
					{
						approach_rack_x = LEFT_SWERVE_SPEED;
						printf("SWERVING LEFT\r\n");
					}
					else if(approach_rack_x >= RIGHT_SWERVE_SPEED)
					{
						approach_rack_x = RIGHT_SWERVE_SPEED;
						printf("SWERVING RIGHT\r\n");
					}
				}


			break;

			case AT_TARGET:

				service_brakes(1);
				approach_rack_x = 127;
				approach_rack_y = 127;
				at_target = 1;
				printf("AT TARGET...\r\n");

			break;

			default:

			break;
		}
	}
	else
	{
		approach_rack_y = 127;
		approach_rack_x = 127;
	}

	last_right_encoder_count = Get_Encoder_2_Count();
}