[Lawlhwut]

#include "WPILib.h"

class RobotDemo : public SimpleRobot
{
	// Declare Motors
	RobotDrive myRobot;
	Victor LeftDrive;
	Jaguar RightDrive;
	Jaguar Lift;
	Jaguar Claw;

	// Declare Joysticks
	Joystick LeftStick;
	Joystick RightStick;

	// Declare Sensors
	Encoder LeftEncoder, RightEncoder;
	Gyro gyro1;
	static const float Kp = 0.03;

public:
	
	
	
	
	
	RobotDemo(void):

		// Set Channels
		myRobot(1,2),
		LeftDrive(1),
		RightDrive(2),
		Lift(5),
		Claw(4),
		LeftStick(1),
		RightStick(2),
		LeftEncoder(3,4,true),
		RightEncoder(5, 6, true),
		gyro1(7)
		
		{
			// Set Motor Expiration
			LeftDrive.SetExpiration(0.005);
			RightDrive.SetExpiration(0.005);
			Lift.SetExpiration(0.005);
			Claw.SetExpiration(0.005);
			
			// Set Encoder Displacement Values
			LeftEncoder.SetDistancePerPulse(0.26333);
			RightEncoder.SetDistancePerPulse(0.26333);
		}
		
	void Autonomous(void)
	{
		//Delcare variables
		int AutoStep = 1;
		
		// Start sensors
		LeftEncoder.Start();
		RightEncoder.Start();
		gyro1.Reset();

		// Run while in autonomous mode
        while(IsAutonomous())
		{							
        	// Return sensor values for tracking		
        	float leftdistance = LeftEncoder.GetDistance();  
        	float rightdistance = RightEncoder.GetDistance();
        	float angle = gyro1.GetAngle();	
        	
        	// Update LCD with new data
        	DriverStationLCD *screen = DriverStationLCD::GetInstance();	
        	screen->PrintfLine(DriverStationLCD::kUser_Line1,"Left Motor",LeftDrive.Get());
        	screen->PrintfLine(DriverStationLCD::kUser_Line2,"Right Motor",RightDrive.Get());
        	screen->PrintfLine(DriverStationLCD::kUser_Line3,"Direction",gyro1.GetAngle());
        	screen->UpdateLCD();
        	
			switch(AutoStep)
			{
				case 1:
				{
					// Move forward to set position
					myRobot.Drive(1.0, -angle / Kp);	
					Wait(0.005);
	
					// Stop if distance is reached
					if(leftdistance >= 4953 || rightdistance >= 4953)
					{
						LeftDrive.Set(0);
						RightDrive.Set(0);
						AutoStep = 2;
					}
					break;
				}
				
				case 2:
				{
					// Drop tube
					Claw.Set(1.0);
					Wait(1.0);
					Claw.Set(0);

					AutoStep = 3;
					break;
				}
				
				case 3:
				{
					// Adjust for gameplay
					LeftDrive.Set(-0.5);
					RightDrive.Set(-0.5);
					Wait(1.0);
					while(gyro1.GetAngle() > -180 || gyro1.GetAngle() < 180)
					{
						LeftDrive.Set(-0.25);
						RightDrive.Set(0.25);
						Lift.Set(-1.0);
					}
					while(IsAutonomous())
					{
						Wait(0.1);
					}
					break;
				}
			}
		}
	}

	void OperatorControl(void)
	{
		// Reset motors for teleoperator
		LeftDrive.Set(0);	
		RightDrive.Set(0);
		Lift.Set(0);
		Claw.Set(0);
		gyro1.Reset();
		
		// Run while in operator control mode
		while(IsOperatorControl())
		{
			// Get Drive
			float lefty = (LeftStick.GetY());
			float righty = (RightStick.GetY());
			
			// Desensitize joysticks
			lefty = ((lefty/128)*(lefty/128));
			righty = ((righty/128)*(righty/128));
			
			// Set Drive
			LeftDrive.Set(lefty);
			RightDrive.Set(righty);
			
			// Claw Control
			if(RightStick.GetRawButton(2))
			{
					Claw.Set(1.0);
			}
			else
			{
				if(RightStick.GetRawButton(3))
				{
					Claw.Set(-1.0);
				}
				else
					Claw.Set(0.0);
			}
			
			// Lift Control
			if(LeftStick.GetRawButton(2))
			{
				Lift.Set(1.0);
			}
			else
			{
				if(LeftStick.GetRawButton(3))
				{
					Lift.Set(-1.0);
				}
				else
					Lift.Set(0.0);
			}
			
			// Update LCD with new data
			DriverStationLCD *screen = DriverStationLCD::GetInstance();	
			screen->PrintfLine(DriverStationLCD::kUser_Line1,"Left Motor:",LeftDrive.Get());
			screen->PrintfLine(DriverStationLCD::kUser_Line2,"Right Motor:",RightDrive.Get());
			screen->UpdateLCD();
		}
	}
};

START_ROBOT_CLASS(RobotDemo);

Builds flawlessly, but when we try to run it on our prototype robot, the communication is lost and the robot acts funky.

Solutions?