Chief Delphi - Autonomous Not Working

Code:
#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?