[Thxe]

Ok, here is the code. Sorry this took so long. It is based off of the Defaut Robot. The watchdog says it is not fed about 1:45 to 2:00 minutes into teleop mode.

DefaultRobot.cpp

#include <iostream.h>
#include "math.h"

//#include "Vision/AxisCamera2010.h"
#include "WPILib.h"
#include "Mecanum.cpp"

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */
class DefaultRobot : public SimpleRobot
{			// robot drive system
	Joystick *rightStick;			// joystick 1 (arcade stick or right tank stick)
	Joystick *leftStick;			// joystick 2 (tank left stick)
	DriverStation *ds; 				// driver station object
	Mecanum *mecanum;				// Mecanum drive system

                            

public:
	/**
	 * 
	 * 
	 * Constructor for this robot subclass.
	 * Create an instance of a RobotDrive with left and right motors plugged into PWM
	 * ports 1 and 2 on the first digital module.
	 */
	DefaultRobot(void)
	{
		ds = DriverStation::GetInstance();	// create robot drive base
		rightStick = new Joystick(1);			// create the joysticks
		leftStick = new Joystick(2);
		mecanum = new Mecanum(4, 1, 2, 3, 4);
		//Update the motors at least every 100ms.
		GetWatchdog().SetExpiration(100);
	}

	/**
	 * Drive left & right motors fo7r 2 seconds, enabled by a jumper (jumper
	 * must be in for autonomous to operate).
	 */
	void Autonomous(void)
	{
		GetWatchdog().Feed();
		GetWatchdog().SetEnabled(false);
		//Pull triger, fire rockets
		//{
		/*	myRobot->Drive(0.5, 0.0);			// drive forwards half speed
			Wait(2000);							//    for 2 seconds						//    for 2 seconds
			myRobot->Drive(-0.5, 0.0);			// reverse robot
			Wait(2000);
			myRobot->Drive(0.0, 0.0);			// stop robot
		//}*/
		GetWatchdog().SetEnabled(true);
	}

	/**
	 * Runs the motors under driver control with either tank or arcade steering selected
	 * by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis. 
	 */
	void OperatorControl(void)
	{
		GetWatchdog().Feed();
		while (true)
		{
			GetWatchdog().Feed();
			mecanum->SetSpeeds(rightStick->GetX(), rightStick->GetY(), leftStick->GetX());  //It works when I comment this out.
			GetWatchdog().Feed();
		}
	}
};

START_ROBOT_CLASS(DefaultRobot);

Mecanum.cpp

/*
 * Mecanum drive class
 * Team 2535
 * Theodore Dahlen
 * 2/1/2010
 */

class Mecanum
{
	
	Victor *MotorFrontLeft;
	Victor *MotorFrontRight;
	Victor *MotorBackLeft;
	Victor *MotorBackRight;
	
public:
	
	
	Mecanum(UINT32 slot,
			UINT32 FrontLeft,
			UINT32 FrontRight,
			UINT32 BackLeft,
			UINT32 BackRight)
	{
		MotorFrontLeft = new Victor(slot, FrontLeft);
		MotorFrontRight = new Victor(slot, FrontRight);
		MotorBackLeft = new Victor(slot, BackLeft);
		MotorBackRight = new Victor(slot, BackRight);
		
		
	}
	
	void SetSpeeds(float X, float Y, float Twist)
	{
		float *Wheels[4]; // 0 is FL, 1 is FR, 2 is BL, 3 is BR
		
		*Wheels[0] = Y + X + Twist;
		*Wheels[1] = Y - X - Twist;
		*Wheels[2] = Y - X + Twist;
		*Wheels[3] = Y + X - Twist;
		

		
		if(*Wheels[0] > 1 || *Wheels[0] < -1)
		{
			Divide(Wheels, 0);
		}
		else if(*Wheels[1] > 1 || *Wheels[1] < -1)
		{
			Divide(Wheels, 1);
		}
		else if(*Wheels[2] > 1 || *Wheels[2] < -1)
		{
			Divide(Wheels, 2);
		}
		else if(*Wheels[3] > 1 || *Wheels[3] < -1)
		{
			Divide(Wheels, 3);
		}
		else
		{
			//YOU MAY PROBLEMS! :D
		}
		
		Drive(Wheels);
	}
		
	
	void Divide(float *Wheels[4], int Divide)
	{
		float Divider = fabs(*Wheels[Divide]);
		
		*Wheels[0] = *Wheels[0] / Divider;
		*Wheels[1] = *Wheels[1] / Divider;
		*Wheels[2] = *Wheels[2] / Divider;
		*Wheels[3] = *Wheels[3] / Divider;
	}
	
	void Drive(float *Wheels[4])
	{
		MotorFrontLeft->Set(*Wheels[0]);
		MotorFrontRight->Set(*Wheels[1]);
		MotorBackLeft->Set(*Wheels[2]);
		MotorBackRight->Set(*Wheels[3]);
		
	}

};