Autonomous Not Working

[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?

[Lawlhwut]

Addition:

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

[PatrickS]

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

[jwakeman]

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.

[mandrews281]

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().