Autonomous Not Working

Technical C/C++
1 
#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?

2

Addition:

Nothing is wrong with the robot. We tested other programs and it worked perfectly fine.

3

This sounds more like a hardware problem. Code normally does not affect communication.

4

I notice you are setting the expiration time for your motor controllers to 5ms. Any reason to use this instead of the default time of 100ms? Otherwise just comment portions out till you find out what is killing you.

5

Perhaps I was burned by it too much last year, but isn’t the watchdog active by default? It will not get fed with the calls to Wait().