//=================================Call Camera=====================================================
			Tracking_Info_Terminal();
			Camera_Handler();
			Servo_Track();
		
			//Distance_To_Light = Distance_Camera_Func();
			//printf("PWM value is %i/r/n", pwm04);
			//printf("Distance to light = %i\r\n", Distance_To_Light);
	
			//=======================Reset Values - Must be the first value item in the loop ===================
			if ((First_Time_Through_Loop == 1) && (disabled_mode == 0))
			{
				Reset_Encoder_1_Count();
				Reset_Encoder_2_Count();
				First_Time_Through_Loop = 0;
				Reset_Gyro_Angle();	
				relay2_fwd=1; //Extend Camera Piston
				relay2_rev=0; //Stop retracting camera piston
				Arm_Position_Needed=Get_ADC_Result(2);
				Wrist_Position_Desired=Get_ADC_Result(4);
				State_Machine = 0;
			}
			else if (disabled_mode == 1)
				First_Time_Through_Loop = 1;
			
			//===================================Get Encoder Counts===============================
			Encoder_Count_1 = Get_Encoder_1_Count();
			Encoder_Count_2 = Get_Encoder_2_Count();
			
			//printf("Encoder Count 1 %i\r\n", (int)Encoder_Count_1);
			//printf("Encoder Count 2 %i\r\n", (int)Encoder_Count_2);
			//printf("Distance Number %i\r\n", (int)distance_number);

			//=====================================Get Arm Position===============================
			Current_Arm_Position = Get_ADC_Result(2);

			//=====================================Get Wrist Position=============================
			Current_Wrist_Position = Get_ADC_Result(4);
	
			//printf("Auton Mode is %i\r\n", User_Mode_byte);

			//============================Pick Starting Location==================================
	
			if (State_Machine == 0)
			{
				switch(User_Mode_byte)
				{
					case 2:  //Start on Left Side, Score Low
							Distance_Needed=240;  //Was 240
							Ok_To_Drive=1;
							Gyro_Angle_Needed = 45;
							Arm_Position_Needed=Arm_Low_Height_Auton;
							Wrist_Position_Desired=Wrist_Low_Height_Auton;
							State_Machine = 1;
						break;
					case 3: //Start in Middle, Score Low
							Distance_Needed=121;  //Was 121 
							Ok_To_Drive=1;  
							Gyro_Angle_Needed = 0;
							Arm_Position_Needed=Arm_Low_Height_Auton;
							Wrist_Position_Desired=Wrist_Low_Height_Auton;
							State_Machine = 1;
						break;
					case 4: //Start on Right, Score Low
							Distance_Needed=240; //Was 240
							Ok_To_Drive=1;
							Gyro_Angle_Needed = -45;
							Arm_Position_Needed=Arm_Low_Height_Auton;
							Wrist_Position_Desired=Wrist_Low_Height_Auton;
							State_Machine = 1;
						break;

					case 6:  //Start left score middle
							Distance_Needed=280;  //Was 240
							Ok_To_Drive=1;
							Gyro_Angle_Needed = 45;
							Arm_Position_Needed=Current_Arm_Position;
							Wrist_Position_Desired=Wrist_Low_Height_Auton;
							State_Machine = 1;
						break;
					case 7:  //Start middle score middle
							Distance_Needed=121;  //Was 121 
							Ok_To_Drive=1;  
							Gyro_Angle_Needed = 0;
							Arm_Position_Needed=Current_Arm_Position;
							Wrist_Position_Desired=Wrist_Low_Height_Auton;
							State_Machine = 1;
						break;
					case 8:  //Start right score middle
							Distance_Needed=280; //Was 240
							Ok_To_Drive=1;
							Gyro_Angle_Needed = -45;
							Arm_Position_Needed=Current_Arm_Position;
							Wrist_Position_Desired=Wrist_Low_Height_Auton;
							State_Machine = 1;
						break;
				}			
			}
			//===================================State_Machine=============================
			if (State_Machine==1)  //Drive and Allow Initial Turn
			{
				//Drive Till Done
				distance_number = (unsigned long)Distance_Needed * counts_per_turn * 100 / wheel_circum;
				if (Ok_To_Drive == 0)
				{
					Left_Drive = Right_Drive = 127;
					Time_Counter++;
				}
				if (Time_Counter == 16)  //Was 26
				{
					State_Machine = 2;
					Ok_To_Turn = 1;
					Time_Counter = 0;
				}
			}
			else if (State_Machine==2) // Move Arm
			{
					Ok_To_Move_Arm=1;					
					if (Ok_To_Turn ==0)
					{
						Left_Drive = Right_Drive = 127;
						Time_Counter++;
					}
					if (Time_Counter == 16)  //Was 76
					{
						Reset_Encoder_1_Count();
						Reset_Encoder_2_Count();
						Total_Angle = Total_Angle + Get_Gyro_Angle()/10;
						Reset_Gyro_Angle();	
						State_Machine = 3;
						Time_Counter = 0;
						
					}
			}
			else if (State_Machine==3) // Turn to face light then drive forward
			{
				if (Get_Tracking_State() == 2)  //Look at the light to get the Gyro Angle
				{
					
					Gyro_Angle_Needed = ((((int)PAN_SERVO - 124) * 65)/124) + Angle_Bias;
					if (Gyro_Angle_Needed > 0)
					Gyro_Angle_Needed = Gyro_Angle_Needed + Right_Turn_Bias;
					else if (Gyro_Angle_Needed <0)
					Gyro_Angle_Needed = Gyro_Angle_Needed + Left_Turn_Bias;				
					Dist = Distance_Camera_Func();	
					Ok_To_Turn=1;
					State_Machine = 4;	
					//printf("We've locked on\r\n");
					//printf("We've locked on\r\n\r\n\r\n\r\n");
					//printf("Gyro Error %i \r\n", Gyro_Angle_Needed);
					//printf(" Pan Error (Pixels)  = %d\r\n", (int)T_Packet_Data.mx - PAN_TARGET_PIXEL_DEFAULT);
					//printf("Tilt Error (Pixels)  = %d\r\n", (int)T_Packet_Data.my - TILT_TARGET_PIXEL_DEFAULT);
				}	
			}

			else if (State_Machine==4)  //Move Arm Down over spider	
			{	
				if (Ok_To_Turn == 0)
					Time_Counter++;
				if (Time_Counter == 16)  //Was 38
				{	
					Left_Drive = Right_Drive = 127;
					Total_Angle = Total_Angle + Get_Gyro_Angle()/10;
					//printf("Counting prior to the last drive.... \r\n");
				}
				if ((Time_Counter >= 38) && (Get_Tracking_State() == 2)) //Drive Forward based on the light amount Was 100 time counter
				{
					Reset_Encoder_1_Count();
					Reset_Encoder_2_Count();
					Encoder_Count_1=0;
					Encoder_Count_2=0;			
					// Dist = Distance_Camera_Func();  Original location before attempted improvement
					if ((User_Mode_byte == 2) || (User_Mode_byte == 3) || (User_Mode_byte == 4))
					{
						distance_number = (Dist-Low_Offset) * counts_per_turn * 100 / wheel_circum;
						Ok_To_Drive = 1;
						Time_Counter = 0;
						State_Machine = 5;				
						Reset_Gyro_Angle();	
					}
					else if ((User_Mode_byte == 6) || (User_Mode_byte == 7) || (User_Mode_byte == 8))
					{
						distance_number = (Dist-Middle_Offset) * counts_per_turn * 100 / wheel_circum;
						Arm_Position_Needed = Arm_Middle_Height_Auton; //Keep Arm Position down until we get distance
					}
					if (((User_Mode_byte == 6) || (User_Mode_byte == 7) || (User_Mode_byte == 8)) && (Time_Counter >= 60))
					{
						Ok_To_Drive = 1;
						Time_Counter = 0;
						State_Machine = 5;				
						Reset_Gyro_Angle();	
					}
				}

			}
			else if (State_Machine==5)  //Move Arm Down over spider
			{
				//printf("Move the arm down a bit\r\n");
				if (Ok_To_Drive == 0)
				{
					Left_Drive = Right_Drive = 127;
					Time_Counter++;
				}
				if (Time_Counter == 26)
				{
					Arm_Position_Needed = Arm_Position_Needed - 200;
					State_Machine = 6;
					Time_Counter = 0;
				}
			}
			else if (State_Machine==6) //Open Gripper 
			{
				//printf("Open the gripper\r\n");
				Time_Counter++;			
				if (Time_Counter == 38)
				{	
					relay3_fwd = 1;	 //Open the claw
					relay3_rev = 0;	 //Stop closing the claw
					Ok_To_Move_Wrist=1;
					Wrist_Position_Desired = Wrist_Pickup;
				}		
				if (Time_Counter == 76)  //Was 106
				{
					Left_Drive = 90;
					Right_Drive = 90;
					State_Machine = 7;
					Time_Counter = 0;
					Arm_Extended_On;  //Extend Arm for drive back
					Arm_Retracted_Stop;  //Extend Arm for drive back
					
				}
			}

			else if (State_Machine==7)  //Finish backwards stop and start turn back to home
			{
				Time_Counter++;
				if (Time_Counter == 46)  // Tell the robot to stop was 38
				{

					Arm_Position_Needed = Arm_Pickup;
					Left_Drive=127;
					Right_Drive=127;
				}
				if (Time_Counter == 66) // Wait for the robot to stop  was 38
				{
					Total_Angle = Total_Angle + Get_Gyro_Angle()/10;
					Reset_Gyro_Angle();	
					State_Machine = 20;
					Time_Counter = 0;
					Ok_To_Turn = 0;
					//printf("Total angle is =============================================== %i\r\n",Total_Angle);
					//Gyro_Angle_Needed = 180-Total_Angle;

				}

			}

			else if (State_Machine==8)  //Drive back to home zone
			{
				if ((Ok_To_Turn == 0) && (Time_Counter < 26))
				{	

					Left_Drive = Right_Drive = 127;
					Time_Counter++;	
					printf("Counting up the last timer counter....\r\n");				
				}
				if (Time_Counter == 26)
				{
					printf("Got in the last time counter");
					Reset_Gyro_Angle();	
					Reset_Encoder_1_Count();
					Reset_Encoder_2_Count();
					Encoder_Count_1=0;
					Encoder_Count_2=0;
					Ok_To_Drive = 1;
					if ((User_Mode_byte == 2) || (User_Mode_byte == 4))
					{
						distance_number = (unsigned long)180 * counts_per_turn * 100 / wheel_circum;	//Start Driving from left or right position
						printf("Assigned the last distance number %i \r\n", (int)distance_number);
						State_Machine = 9;
					}
					else if (User_Mode_byte == 3)
					{
						distance_number = (unsigned long)100 * counts_per_turn * 100 / wheel_circum;	//Start Driving from middle
						State_Machine = 9;
					}
					
				}
			}
			else if (State_Machine == 9)
			{
				printf("Waiting on final ok to drive = 0 \r\n");
				if (Ok_To_Drive == 0)
				{
					Arm_Position_Needed = Arm_Pickup;

					State_Machine = 10;
				}
			}

			//printf("Gyro Angle is: %i \r\n",(int)(Get_Gyro_Angle()/10));
			//printf("Gyro Angle needed is: %i \r\n", (int)Gyro_Angle_Needed);
		    //printf("Distance Number is: %i \r\n", (int)Distance_Camera_Func());
			//printf("Encoder 1 is: %i \r\n", (int)Encoder_Count_1);
			//printf("Encoder 2 is: %i \r\n", (int)Encoder_Count_2);
			//printf("Ok to Drive: %i \r\n", (int)Ok_To_Drive);
			//printf("Ok to Turn: %i \r\n", (int)Ok_To_Turn);
			//printf("Ok to move arm %i \r\n", (int)Ok_To_Move_Arm);
			//printf("Ok to move wrist %i \r\n", (int)Ok_To_Move_Wrist);
			//printf("State Machine is: %i \r\n", (int)State_Machine);
			//printf("Tracking State is %i \r\n", (int)Get_Tracking_State());

			//=====================Drive Arm===================================
			if (Ok_To_Move_Arm==1)
				Arm_Height= 127 + Arm(Arm_Position_Needed);

			//=======================Drive Wrist===============================
			if (Ok_To_Move_Wrist==1)
			{

				if (Current_Wrist_Position > (Wrist_Position_Desired + 40))
				{
					relay6_fwd=0;
					relay6_rev=1;
				}
				else if (Current_Wrist_Position < Wrist_Position_Desired)
				{
					relay6_fwd=1;
					relay6_rev=0;
				}
				else
				{
					relay6_fwd = relay6_rev = 0;
					Wrist_Position_Desired = Current_Wrist_Position;
					Ok_To_Move_Wrist=0;
				}

			}

			//============================Drive a Distance and Keep an Angle =====================
			if ((distance_number >= ((Encoder_Count_1 + Encoder_Count_2) / 2)) && (Ok_To_Drive == 1) && (Ok_To_Turn == 0))
			{ 
			
					Left_Drive = Distance_Func(distance_number);
					Right_Drive = Distance_Func(distance_number);	
				
				if (((Encoder_Count_1 + Encoder_Count_2)/2) < 200) //Limit startup speed to prevent auto-wheelie
				{
					if (Left_Drive > 160)
						Left_Drive = 160;
					if (Right_Drive > 160)
						Right_Drive = 160;
				}
			}
			else
			{
				Ok_To_Drive = 0;
			}
	
			
			//printf("Gyro Angle Needed %i \r\n\r\n", (int)Gyro_Angle_Needed);
			//printf("Angle of the Camera %i\r\n", (((((int)PAN_SERVO - 124) * 65)/124)-Angle_Bias)); 
			//printf("Tracking state is %i\r\n", Get_Tracking_State());
			

			//====================================Turn an Angle========================================
			if ((Ok_To_Drive == 0) && (Ok_To_Turn == 1) && ((Gyro_Angle_Needed < (Get_Gyro_Angle()/10 - 3)) || (Gyro_Angle_Needed > (Get_Gyro_Angle()/10 + 3))))
			{
				Left_Drive = 127 + Angle_Func(Gyro_Angle_Needed, Ok_To_Drive);
				Right_Drive = 127 - Angle_Func(Gyro_Angle_Needed, Ok_To_Drive);
			}
			else
			{
				Ok_To_Turn = 0;
			}