Wierd Robot Problems

So our team has been experiencing very weird problems with our code. Whenever we try and drive the robot, wheels will spin the wrong way, for seemingly no reason. We think that it may be cause by using the gyro for our field-centric steering, however we are not sure, as this is part of WPILib. Another problem we are having is that several of our functions just dont seem to be running. For inastance, we have a function called speedcontrol, which simply changes the speed of our shooter depending on which button is pressed, then outputs the speed, however the output always remains at 0. Below are 4 copies of the code. One copy is our regular code, one is the code without the deadzone, one is without field-centric steering, and finally, one is without either the deadzone or the field-centric steering. Help us CD, you are our only hope.

/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
#include "WPILib.h"
#include <math.h>


class RobotDemo : public SimpleRobot
{



Victor laquanda;				//front left motor:V1
Victor anabel;					//back left motor:V2
Victor rotciv;					//front right motor:V3
Victor shanaynay;				//back right motor:V4

RobotDrive rDrive;				//declare robot drive

Victor shooterVictor;				//declare victor for shooter
Victor shooterVictor2;
Victor shooterVictor3;

Solenoid loaderArm;				//declare servo for loader arm

Joystick driverStick;			//declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;

Victor climbingArm1;			//declare victors for climbing arms
Victor climbingArm2;

Solenoid SC1;				//declares solenoids for climbing arm
Solenoid SC2;

DigitalInput Disk1; 			//declares limit switches for hopper
DigitalInput Disk2;
DigitalInput Disk3;

Gyro g1;



public:


	RobotDemo(void):

	laquanda(1),			//assign drive motors PWM ports
	anabel(2),
	rotciv(3),
	shanaynay(4),

	rDrive(laquanda, anabel, rotciv, shanaynay),			//assigns robot drive talons theiw PWM ports

	shooterVictor(6),		//assigns victor its PWM port
	shooterVictor2(7),
	shooterVictor3(8),

	loaderArm(3),			//assign servo its PWM port

	driverStick(1),			//assign joysticks their USB ports
	driverStick2(2),
	shooterStick(3),

	climbingArm1(8),		//assigns climbing victors PWM ports
	climbingArm2(9),

	SC1(1),				//assigns climbing pneumatics digital output ports
	SC2(2),

	Disk1(5),			//assigns limit switches digital input ports
	Disk2(6),
	Disk3(7),

	g1(1)
	{


	Compressor *c = new Compressor(4, 2);
	c->Start();

	Watchdog().SetEnabled(false);

	rDrive.SetExpiration(0.1);
	rDrive.SetSafetyEnabled(true);			//sets motor saftey on for all motors

	shooterVictor.SetExpiration(0.1);
	shooterVictor.SetSafetyEnabled(true);

	shooterVictor2.SetExpiration(0.1);
	shooterVictor2.SetSafetyEnabled(true);

	shooterVictor3.SetExpiration(0.1);
	shooterVictor3.SetSafetyEnabled(true);

	climbingArm1.SetExpiration(0.1);
	climbingArm1.SetSafetyEnabled(false);

	climbingArm2.SetExpiration(0.1);
	climbingArm2.SetSafetyEnabled(false);

	}



	void Autonomous(void)
	{

	g1.Reset();

	DriverStationLCD *ds = DriverStationLCD::GetInstance();

	ds->Clear();

	double AutoTime = 0;			//used to measure time during autonomous


	rDrive.SetSafetyEnabled(false);				//turn of motor saftey timers during autonomous for convience

	shooterVictor.SetSafetyEnabled(false);
	shooterVictor2.SetSafetyEnabled(false);
	shooterVictor3.SetSafetyEnabled(false);

	ds->PrintfLine(DriverStationLCD::kUser_Line1, "Program Started Successfully!");
	ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is in Autonomous");

	ds->UpdateLCD();

	while(IsAutonomous()){

		//top autonomous
		if(AutoTime>=0 && AutoTime<0.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.5, 0.0);
		}
		if(AutoTime>=0.5 && AutoTime<1.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
		}
		if(AutoTime>=1.0 && AutoTime<1.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
			shooterVictor.Set(1.0);
			shooterVictor2.Set(1.0);
			shooterVictor3.Set(1.0);
			loaderArm.Set(true);
		}
		if(AutoTime>=1.5 && AutoTime<2.0){
			loaderArm.Set(false);
		}
		if(AutoTime>=2.0 && AutoTime<2.5){
			loaderArm.Set(true);
		}
		if(AutoTime>=2.5 && AutoTime<3.0){
			loaderArm.Set(false);
		}
		if(AutoTime>=3.0 && AutoTime<3.5){
			loaderArm.Set(true);
		}
		if(AutoTime>=3.5 && AutoTime<4.0){
			loaderArm.Set(false);
			shooterVictor.Set(0.0);
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
		}
		if(AutoTime>=4.0 && AutoTime<5.0){
			rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start
		}
		if(AutoTime>=5.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start
		}


		//start at back autonomous
		/*
		if(AutoTime>=0 && AutoTime<0.5){
			rDrive.MecanumDrive_Cartesian(0.5, 0.0, 0.0);  //0.5 for right start, -0.5 for left start
		}
		if(AutoTime>=0.5 && AutoTime<1.0){
			rDrive.MecanumDrive_Cartesian(0.0, 1.0, 0.0);
		}
		if(AutoTime>=1.0 && AutoTime<1.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
		}

		if(AutoTime>=1.5 && AutoTime<2.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
			shooterVictor.Set(1.0);
			shooterVictor2.Set(1.0);
			shooterVictor3.Set(1.0);
			loaderArm.Set(true);
		}
		if(AutoTime>=2.0 && AutoTime<2.5){
			loaderArm.Set(false);
		}
		if(AutoTime>=2.5 && AutoTime<3.0){
			loaderArm.Set(true);
		}
		if(AutoTime>=3.0 && AutoTime<3.5){
			loaderArm.Set(false);
		}
		if(AutoTime>=3.5 && AutoTime<4.0){
			loaderArm.Set(true);
		}
		if(AutoTime>=4.0 && AutoTime<4.5){
			loaderArm.Set(false);
			shooterVictor.Set(0.0);
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
		}
		if(AutoTime>=4.5 && AutoTime<5.0){
			rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start
		}
		if(AutoTime>=5.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start
		}
		*/
		AutoTime=AutoTime+0.005;
		Wait(0.005);
		}
	}


	void OperatorControl(void){

	g1.Reset();

	rDrive.SetSafetyEnabled(true);				//turn motor safety timers back on

	shooterVictor.SetSafetyEnabled(true);
	shooterVictor2.SetSafetyEnabled(true);
	shooterVictor3.SetSafetyEnabled(true);

	bool gyroe = true;

	float shootspeed=0.25;

	DriverStationLCD *ds = DriverStationLCD::GetInstance();

	ds->PrintfLine(DriverStationLCD::kUser_Line6, "Pinkie Pie Is Best Pony");

	ds->UpdateLCD();

		while (IsOperatorControl()){

		ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is In TeleOp");

		gyroe=gyroc(gyroe);

		shootspeed=speedcontrol(shootspeed);

		gc();

		driving(gyroe);   			//runs drive control function

		ShooterControl(shootspeed);		//runs shooter control function

		LoaderControl();		//runs servo control function

		Climbing();			//runs pryamid climbing function



		//DiskCount();			//runs diskcount function

		ds->UpdateLCD();

		Wait(0.005);			//waits 0.02 seconds
			}
		}


	void gc(){
		if(driverStick.GetRawButton(2)){
			g1.Reset();
		}
	}

	float speedcontrol(float s){
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		if (shooterStick.GetRawButton(6)){
			s=0.25;
		}
		else if (shooterStick.GetRawButton(7)){
			s=0.5;
		}
		else if (shooterStick.GetRawButton(10)){
			s=0.75;
		}
		else if (shooterStick.GetRawButton(11)){
			s=1.0;
		}
		ds->PrintfLine(DriverStationLCD::kUser_Line1, "Shooter Speed: %d", s);
		return s;

	}


	bool gyroc(bool gyroe){
		DriverStationLCD *ds = DriverStationLCD::GetInstance();

		if (driverStick.GetRawButton(4)){

	}

		ds->PrintfLine(DriverStationLCD::kUser_Line4, "Gyro On: %d", gyroe);
		ds->PrintfLine(DriverStationLCD::kUser_Line5, " %d", g1.GetAngle());
		ds->UpdateLCD();
		return gyroe;
	}

	void driving(bool gyroe){
		double y = driverStick.GetY();           //variable for forward/backward movement
		double x = driverStick.GetX();           //variable for side to side movement
		double turn = driverStick2.GetX() ;        //variable for turning movement
		double deadzone = 0.2;	//variable for amount of deadzone
		if(fabs(y)>deadzone) {
			float yo = y;
			y=fabs(y);
			y=1-y;
			y=y*.1;
			y=y*2.5;
			y=yo-y;
			if(driverStick.GetY()<0){
				y=-y;
			}
		}
		if(fabs(x)>deadzone) {
			float xo = x;
			x=fabs(x);
			x=1-x;
			x=x*.1;
			x=x*2.5;
			x=xo-x;
			if(driverStick.GetX()<0){
				x=-x;
			}
		}
		if(fabs(turn)>deadzone) {
			float turno = turn;
			turn=fabs(turn);
			turn=1-turn;
			turn=turn*.1;
			turn=turn*2.5;
			turn=turno-turn;
			if(driverStick2.GetX()<0){
				turn=-turn;
			}
		}
		if (gyroe=true){
		rDrive.MecanumDrive_Cartesian(x, y, -turn, g1.GetAngle());
		}
		else{
		rDrive.MecanumDrive_Cartesian(x, y, -turn);
		}
	}


	void ShooterControl(float shootspeed){			//shooter control function
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		if(shooterStick.GetRawButton(1)){
			shooterVictor.Set(shootspeed);			//if trigger is pressed, turn on shooter
			shooterVictor2.Set(shootspeed);
			shooterVictor3.Set(shootspeed);
			ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is On");
			}
		else{
			shooterVictor.Set(0.0);			//if trigger isnt pressed, turn off shooter
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is Off");
			}
		}


	void LoaderControl(){			//servo control function
		if(shooterStick.GetRawButton(3)){
			loaderArm.Set(true);			//if middle button is pressed, extend servo
		}
		else{
			loaderArm.Set(false);				//if middle button isnt pressed, retract servo
			}
		}


	void Climbing(){						//pyramid climbing function, happens automatically
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		if( shooterStick.GetRawButton(4)){		//if button 6 pressed on both sticks, starts climb

		ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Up");
		SC1.Set(true);
		SC2.Set(true);
		}
		if(shooterStick.GetRawButton(5)){
			ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Down");
			SC1.Set(false);
			SC2.Set(false);
		}
	}



	/*
	void DiskCount(){			//function that counts disk in the hopper and displays it
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		int disknum = 0;		//variable that counts disks in hopper

		if(Disk1.Get()){
			disknum=disknum+1;	//disk has activated a limit switch, add 1 to # of disks
			}
		if(Disk2.Get()){
			disknum=disknum+1;
			}
		if(Disk3.Get()){
			disknum=disknum+1;
			}

		ds->PrintfLine(DriverStationLCD::kUser_Line5, "Disk's in Hopper: %d", disknum);	//print out the number of disks in hopper


		}
*/
	};

START_ROBOT_CLASS(RobotDemo);		//starts the class running

/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
//no deadzone
#include "WPILib.h"
#include <math.h>


class RobotDemo : public SimpleRobot
{



Victor laquanda;				//front left motor:V1
Victor anabel;					//back left motor:V2
Victor rotciv;					//front right motor:V3
Victor shanaynay;				//back right motor:V4

RobotDrive rDrive;				//declare robot drive

Victor shooterVictor;				//declare victor for shooter
Victor shooterVictor2;
Victor shooterVictor3;

Solenoid loaderArm;				//declare servo for loader arm

Joystick driverStick;			//declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;

Victor climbingArm1;			//declare victors for climbing arms
Victor climbingArm2;

Solenoid SC1;				//declares solenoids for climbing arm
Solenoid SC2;

DigitalInput Disk1; 			//declares limit switches for hopper
DigitalInput Disk2;
DigitalInput Disk3;

Gyro g1;



public:


	RobotDemo(void):

	laquanda(1),			//assign drive motors PWM ports
	anabel(2),
	rotciv(3),
	shanaynay(4),

	rDrive(laquanda, anabel, rotciv, shanaynay),			//assigns robot drive talons theiw PWM ports

	shooterVictor(6),		//assigns victor its PWM port
	shooterVictor2(7),
	shooterVictor3(8),

	loaderArm(3),			//assign servo its PWM port

	driverStick(1),			//assign joysticks their USB ports
	driverStick2(2),
	shooterStick(3),

	climbingArm1(8),		//assigns climbing victors PWM ports
	climbingArm2(9),

	SC1(1),				//assigns climbing pneumatics digital output ports
	SC2(2),

	Disk1(5),			//assigns limit switches digital input ports
	Disk2(6),
	Disk3(7),
	
	g1(1)
	{


	Compressor *c = new Compressor(4, 2);
	c->Start();

	Watchdog().SetEnabled(false);

	rDrive.SetExpiration(0.1);
	rDrive.SetSafetyEnabled(true);			//sets motor saftey on for all motors

	shooterVictor.SetExpiration(0.1);
	shooterVictor.SetSafetyEnabled(true);

	shooterVictor2.SetExpiration(0.1);
	shooterVictor2.SetSafetyEnabled(true);

	shooterVictor3.SetExpiration(0.1);
	shooterVictor3.SetSafetyEnabled(true);

	climbingArm1.SetExpiration(0.1);
	climbingArm1.SetSafetyEnabled(false);

	climbingArm2.SetExpiration(0.1);
	climbingArm2.SetSafetyEnabled(false);

	}
	 
	 

	void Autonomous(void)
	{

	g1.Reset();	
		
	DriverStationLCD *ds = DriverStationLCD::GetInstance();

	ds->Clear();
	
	double AutoTime = 0;			//used to measure time during autonomous


	rDrive.SetSafetyEnabled(false);				//turn of motor saftey timers during autonomous for convience

	shooterVictor.SetSafetyEnabled(false);
	shooterVictor2.SetSafetyEnabled(false);
	shooterVictor3.SetSafetyEnabled(false);

	ds->PrintfLine(DriverStationLCD::kUser_Line1, "Program Started Successfully!");
	ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is in Autonomous");

	ds->UpdateLCD();

	while(IsAutonomous()){
		
		//top autonomous
		if(AutoTime>=0 && AutoTime<0.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.5, 0.0);
		}
		if(AutoTime>=0.5 && AutoTime<1.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
		}
		if(AutoTime>=1.0 && AutoTime<1.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
			shooterVictor.Set(1.0);
			shooterVictor2.Set(1.0);
			shooterVictor3.Set(1.0);
			loaderArm.Set(true);
		}
		if(AutoTime>=1.5 && AutoTime<2.0){
			loaderArm.Set(false);
		}
		if(AutoTime>=2.0 && AutoTime<2.5){
			loaderArm.Set(true);
		}
		if(AutoTime>=2.5 && AutoTime<3.0){
			loaderArm.Set(false);
		}
		if(AutoTime>=3.0 && AutoTime<3.5){
			loaderArm.Set(true);
		}	
		if(AutoTime>=3.5 && AutoTime<4.0){
			loaderArm.Set(false);
			shooterVictor.Set(0.0);
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
		}
		if(AutoTime>=4.0 && AutoTime<5.0){
			rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start	
		}	
		if(AutoTime>=5.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start	
		}	
		
		
		//start at back autonomous
		/*
		if(AutoTime>=0 && AutoTime<0.5){
			rDrive.MecanumDrive_Cartesian(0.5, 0.0, 0.0);  //0.5 for right start, -0.5 for left start
		}
		if(AutoTime>=0.5 && AutoTime<1.0){
			rDrive.MecanumDrive_Cartesian(0.0, 1.0, 0.0);
		}
		if(AutoTime>=1.0 && AutoTime<1.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
		}

		if(AutoTime>=1.5 && AutoTime<2.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
			shooterVictor.Set(1.0);
			shooterVictor2.Set(1.0);
			shooterVictor3.Set(1.0);
			loaderArm.Set(true);
		}
		if(AutoTime>=2.0 && AutoTime<2.5){
			loaderArm.Set(false);
		}
		if(AutoTime>=2.5 && AutoTime<3.0){
			loaderArm.Set(true);
		}
		if(AutoTime>=3.0 && AutoTime<3.5){
			loaderArm.Set(false);
		}
		if(AutoTime>=3.5 && AutoTime<4.0){
			loaderArm.Set(true);
		}	
		if(AutoTime>=4.0 && AutoTime<4.5){
			loaderArm.Set(false);
			shooterVictor.Set(0.0);
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
		}
		if(AutoTime>=4.5 && AutoTime<5.0){
			rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start	
		}	
		if(AutoTime>=5.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start	
		}	
		*/
		AutoTime=AutoTime+0.005;
		Wait(0.005);
		}
	}


	void OperatorControl(void){

	g1.Reset();		
		
	rDrive.SetSafetyEnabled(true);				//turn motor safety timers back on

	shooterVictor.SetSafetyEnabled(true);
	shooterVictor2.SetSafetyEnabled(true);
	shooterVictor3.SetSafetyEnabled(true);

	bool gyroe = true;

	float shootspeed=0.25;
	
	DriverStationLCD *ds = DriverStationLCD::GetInstance();

	ds->PrintfLine(DriverStationLCD::kUser_Line6, "Pinkie Pie Is Best Pony");

	ds->UpdateLCD();

		while (IsOperatorControl()){

		ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is In TeleOp");
		
		gyroe=gyroc(gyroe);
		
		shootspeed=speedcontrol(shootspeed);
		
		gc();
		
		driving(gyroe);   			//runs drive control function
		
		ShooterControl(shootspeed);		//runs shooter control function

		LoaderControl();		//runs servo control function

		Climbing();			//runs pryamid climbing function

		
		
		//DiskCount();			//runs diskcount function

		ds->UpdateLCD();

		Wait(0.005);			//waits 0.02 seconds
			}
		}

	
	void gc(){
		if(driverStick.GetRawButton(2)){
			g1.Reset();
		}
	}
	
	float speedcontrol(float s){
		DriverStationLCD *ds = DriverStationLCD::GetInstance();	
		if (shooterStick.GetRawButton(6)){
			s=0.25;
		}
		else if (shooterStick.GetRawButton(7)){
			s=0.5;
		}
		else if (shooterStick.GetRawButton(10)){
			s=0.75;
		}
		else if (shooterStick.GetRawButton(11)){
			s=1.0;
		}
		ds->PrintfLine(DriverStationLCD::kUser_Line1, "Shooter Speed: %d", s);
		return s;
		
	}
	
	 
	bool gyroc(bool gyroe){
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		
		if (driverStick.GetRawButton(3)){
			if(gyroe==true){
				gyroe=false;
			}
			else{
				gyroe=true;
			}
	}

		ds->PrintfLine(DriverStationLCD::kUser_Line4, "Gyro On: %d", gyroe);
		ds->PrintfLine(DriverStationLCD::kUser_Line5, " %d", g1.GetAngle());
		ds->UpdateLCD();
		return gyroe;
	}	
	
	void driving(bool gyroe){
		double y = driverStick.GetY();           //variable for forward/backward movement
		double x = driverStick.GetX();           //variable for side to side movement
		double turn = driverStick2.GetX() ;        //variable for turning movement


		if (gyroe=true){
		rDrive.MecanumDrive_Cartesian(x, y, -turn, g1.GetAngle());
		}
		else{
		rDrive.MecanumDrive_Cartesian(x, y, -turn);
		}
	}


	void ShooterControl(float shootspeed){			//shooter control function
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		if(shooterStick.GetRawButton(1)){
			shooterVictor.Set(shootspeed);			//if trigger is pressed, turn on shooter
			shooterVictor2.Set(shootspeed);
			shooterVictor3.Set(shootspeed);
			ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is On");
			}
		else{
			shooterVictor.Set(0.0);			//if trigger isnt pressed, turn off shooter
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is Off");
			}
		}


	void LoaderControl(){			//servo control function
		if(shooterStick.GetRawButton(3)){
			loaderArm.Set(true);			//if middle button is pressed, extend servo
			}
		else{
			loaderArm.Set(false);				//if middle button isnt pressed, retract servo
			}
		}


	void Climbing(){						//pyramid climbing function, happens automatically
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		if( driverStick.GetRawButton(4)){		//if button 6 pressed on both sticks, starts climb

		ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Up");
		SC1.Set(true);
		SC2.Set(true);
		}
		if(driverStick.GetRawButton(5)){
			ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Down");
			SC1.Set(false);
			SC2.Set(false);
		}
	}
		


	/*
	void DiskCount(){			//function that counts disk in the hopper and displays it
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		int disknum = 0;		//variable that counts disks in hopper

		if(Disk1.Get()){
			disknum=disknum+1;	//disk has activated a limit switch, add 1 to # of disks
			}
		if(Disk2.Get()){
			disknum=disknum+1;
			}
		if(Disk3.Get()){
			disknum=disknum+1;
			}

		ds->PrintfLine(DriverStationLCD::kUser_Line5, "Disk's in Hopper: %d", disknum);	//print out the number of disks in hopper


		}
*/
	};

START_ROBOT_CLASS(RobotDemo);		//starts the class running



/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
//no field centric
#include "WPILib.h"
#include <math.h>


class RobotDemo : public SimpleRobot
{



Victor laquanda;				//front left motor:V1
Victor anabel;					//back left motor:V2
Victor rotciv;					//front right motor:V3
Victor shanaynay;				//back right motor:V4

RobotDrive rDrive;				//declare robot drive

Victor shooterVictor;				//declare victor for shooter
Victor shooterVictor2;
Victor shooterVictor3;

Solenoid loaderArm;				//declare servo for loader arm

Joystick driverStick;			//declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;

Victor climbingArm1;			//declare victors for climbing arms
Victor climbingArm2;

Solenoid SC1;				//declares solenoids for climbing arm
Solenoid SC2;

DigitalInput Disk1; 			//declares limit switches for hopper
DigitalInput Disk2;
DigitalInput Disk3;

Gyro g1;



public:


	RobotDemo(void):

	laquanda(1),			//assign drive motors PWM ports
	anabel(2),
	rotciv(3),
	shanaynay(4),

	rDrive(laquanda, anabel, rotciv, shanaynay),			//assigns robot drive talons theiw PWM ports

	shooterVictor(6),		//assigns victor its PWM port
	shooterVictor2(7),
	shooterVictor3(8),

	loaderArm(3),			//assign servo its PWM port

	driverStick(1),			//assign joysticks their USB ports
	driverStick2(2),
	shooterStick(3),

	climbingArm1(8),		//assigns climbing victors PWM ports
	climbingArm2(9),

	SC1(1),				//assigns climbing pneumatics digital output ports
	SC2(2),

	Disk1(5),			//assigns limit switches digital input ports
	Disk2(6),
	Disk3(7),

	g1(1)
	{


	Compressor *c = new Compressor(4, 2);
	c->Start();

	Watchdog().SetEnabled(false);

	rDrive.SetExpiration(0.1);
	rDrive.SetSafetyEnabled(true);			//sets motor saftey on for all motors

	shooterVictor.SetExpiration(0.1);
	shooterVictor.SetSafetyEnabled(true);

	shooterVictor2.SetExpiration(0.1);
	shooterVictor2.SetSafetyEnabled(true);

	shooterVictor3.SetExpiration(0.1);
	shooterVictor3.SetSafetyEnabled(true);

	climbingArm1.SetExpiration(0.1);
	climbingArm1.SetSafetyEnabled(false);

	climbingArm2.SetExpiration(0.1);
	climbingArm2.SetSafetyEnabled(false);

	}



	void Autonomous(void)
	{

	g1.Reset();

	DriverStationLCD *ds = DriverStationLCD::GetInstance();

	ds->Clear();

	double AutoTime = 0;			//used to measure time during autonomous


	rDrive.SetSafetyEnabled(false);				//turn of motor saftey timers during autonomous for convience

	shooterVictor.SetSafetyEnabled(false);
	shooterVictor2.SetSafetyEnabled(false);
	shooterVictor3.SetSafetyEnabled(false);

	ds->PrintfLine(DriverStationLCD::kUser_Line1, "Program Started Successfully!");
	ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is in Autonomous");

	ds->UpdateLCD();

	while(IsAutonomous()){

		//top autonomous
		if(AutoTime>=0 && AutoTime<0.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.5, 0.0);
		}
		if(AutoTime>=0.5 && AutoTime<1.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
		}
		if(AutoTime>=1.0 && AutoTime<1.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
			shooterVictor.Set(1.0);
			shooterVictor2.Set(1.0);
			shooterVictor3.Set(1.0);
			loaderArm.Set(true);
		}
		if(AutoTime>=1.5 && AutoTime<2.0){
			loaderArm.Set(false);
		}
		if(AutoTime>=2.0 && AutoTime<2.5){
			loaderArm.Set(true);
		}
		if(AutoTime>=2.5 && AutoTime<3.0){
			loaderArm.Set(false);
		}
		if(AutoTime>=3.0 && AutoTime<3.5){
			loaderArm.Set(true);
		}
		if(AutoTime>=3.5 && AutoTime<4.0){
			loaderArm.Set(false);
			shooterVictor.Set(0.0);
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
		}
		if(AutoTime>=4.0 && AutoTime<5.0){
			rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start
		}
		if(AutoTime>=5.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start
		}


		//start at back autonomous
		/*
		if(AutoTime>=0 && AutoTime<0.5){
			rDrive.MecanumDrive_Cartesian(0.5, 0.0, 0.0);  //0.5 for right start, -0.5 for left start
		}
		if(AutoTime>=0.5 && AutoTime<1.0){
			rDrive.MecanumDrive_Cartesian(0.0, 1.0, 0.0);
		}
		if(AutoTime>=1.0 && AutoTime<1.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
		}

		if(AutoTime>=1.5 && AutoTime<2.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
			shooterVictor.Set(1.0);
			shooterVictor2.Set(1.0);
			shooterVictor3.Set(1.0);
			loaderArm.Set(true);
		}
		if(AutoTime>=2.0 && AutoTime<2.5){
			loaderArm.Set(false);
		}
		if(AutoTime>=2.5 && AutoTime<3.0){
			loaderArm.Set(true);
		}
		if(AutoTime>=3.0 && AutoTime<3.5){
			loaderArm.Set(false);
		}
		if(AutoTime>=3.5 && AutoTime<4.0){
			loaderArm.Set(true);
		}
		if(AutoTime>=4.0 && AutoTime<4.5){
			loaderArm.Set(false);
			shooterVictor.Set(0.0);
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
		}
		if(AutoTime>=4.5 && AutoTime<5.0){
			rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start
		}
		if(AutoTime>=5.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start
		}
		*/
		AutoTime=AutoTime+0.005;
		Wait(0.005);
		}
	}


	void OperatorControl(void){

	g1.Reset();

	rDrive.SetSafetyEnabled(true);				//turn motor safety timers back on

	shooterVictor.SetSafetyEnabled(true);
	shooterVictor2.SetSafetyEnabled(true);
	shooterVictor3.SetSafetyEnabled(true);

	bool gyroe = true;

	float shootspeed=0.25;

	DriverStationLCD *ds = DriverStationLCD::GetInstance();

	ds->PrintfLine(DriverStationLCD::kUser_Line6, "Pinkie Pie Is Best Pony");

	ds->UpdateLCD();

		while (IsOperatorControl()){

		ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is In TeleOp");

		shootspeed=speedcontrol(shootspeed);


		driving(gyroe);   			//runs drive control function

		ShooterControl(shootspeed);		//runs shooter control function

		LoaderControl();		//runs servo control function

		Climbing();			//runs pryamid climbing function



		//DiskCount();			//runs diskcount function

		ds->UpdateLCD();

		Wait(0.005);			//waits 0.02 seconds
			}
		}



	float speedcontrol(float s){
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		if (shooterStick.GetRawButton(6)){
			s=0.25;
		}
		else if (shooterStick.GetRawButton(7)){
			s=0.5;
		}
		else if (shooterStick.GetRawButton(10)){
			s=0.75;
		}
		else if (shooterStick.GetRawButton(11)){
			s=1.0;
		}
		ds->PrintfLine(DriverStationLCD::kUser_Line1, "Shooter Speed: %d", s);
		return s;

	}





	void driving(bool gyroe){
		double y = driverStick.GetY();           //variable for forward/backward movement
		double x = driverStick.GetX();           //variable for side to side movement
		double turn = driverStick2.GetX() ;        //variable for turning movement
		double deadzone = 0.2;	//variable for amount of deadzone
			if(fabs(y)>deadzone) {
			float yo = y;
			y=fabs(y);
			y=1-y;
			y=y*.1;
			y=y*2.5;
			y=yo-y;
			if(driverStick.GetY()<0){
				y=-y;
			}
		}
		if(fabs(x)>deadzone) {
			float xo = x;
			x=fabs(x);
			x=1-x;
			x=x*.1;
			x=x*2.5;
			x=xo-x;
			if(driverStick.GetX()<0){
				x=-x;
			}
		}
		if(fabs(turn)>deadzone) {
			float turno = turn;
			turn=fabs(turn);
			turn=1-turn;
			turn=turn*.1;
			turn=turn*2.5;
			turn=turno-turn;
			if(driverStick2.GetX()<0){
				turn=-turn;
			}
		}

		rDrive.MecanumDrive_Cartesian(x, y, -turn);

	}


	void ShooterControl(float shootspeed){			//shooter control function
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		if(shooterStick.GetRawButton(1)){
			shooterVictor.Set(shootspeed);			//if trigger is pressed, turn on shooter
			shooterVictor2.Set(shootspeed);
			shooterVictor3.Set(shootspeed);
			ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is On");
			}
		else{
			shooterVictor.Set(0.0);			//if trigger isnt pressed, turn off shooter
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is Off");
			}
		}


	void LoaderControl(){			//servo control function
		if(shooterStick.GetRawButton(3)){
			loaderArm.Set(true);			//if middle button is pressed, extend servo
			}
		else{
			loaderArm.Set(false);				//if middle button isnt pressed, retract servo
			}
		}


	void Climbing(){						//pyramid climbing function, happens automatically
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		if( driverStick.GetRawButton(4)){		//if button 6 pressed on both sticks, starts climb

		ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Up");
		SC1.Set(true);
		SC2.Set(true);
		}
		if(driverStick.GetRawButton(5)){
			ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Down");
			SC1.Set(false);
			SC2.Set(false);
		}
	}



	/*
	void DiskCount(){			//function that counts disk in the hopper and displays it
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		int disknum = 0;		//variable that counts disks in hopper

		if(Disk1.Get()){
			disknum=disknum+1;	//disk has activated a limit switch, add 1 to # of disks
			}
		if(Disk2.Get()){
			disknum=disknum+1;
			}
		if(Disk3.Get()){
			disknum=disknum+1;
			}

		ds->PrintfLine(DriverStationLCD::kUser_Line5, "Disk's in Hopper: %d", disknum);	//print out the number of disks in hopper


		}
*/
	};

START_ROBOT_CLASS(RobotDemo);		//starts the class running

/*
Minotaur FRC 1369
2013 Robot Code
Lead Cracker: Graham Peterson
Lead Lead: Daniel Brown
*Insert Important Stuff Here*
*/
//nothing
#include "WPILib.h"
#include <math.h>


class RobotDemo : public SimpleRobot
{



Victor laquanda;				//front left motor:V1
Victor anabel;					//back left motor:V2
Victor rotciv;					//front right motor:V3
Victor shanaynay;				//back right motor:V4

RobotDrive rDrive;				//declare robot drive

Victor shooterVictor;				//declare victor for shooter
Victor shooterVictor2;
Victor shooterVictor3;

Solenoid loaderArm;				//declare servo for loader arm

Joystick driverStick;			//declares all 3 joysticks
Joystick driverStick2;
Joystick shooterStick;

Victor climbingArm1;			//declare victors for climbing arms
Victor climbingArm2;

Solenoid SC1;				//declares solenoids for climbing arm
Solenoid SC2;

DigitalInput Disk1; 			//declares limit switches for hopper
DigitalInput Disk2;
DigitalInput Disk3;

Gyro g1;



public:


	RobotDemo(void):

	laquanda(1),			//assign drive motors PWM ports
	anabel(2),
	rotciv(3),
	shanaynay(4),

	rDrive(laquanda, anabel, rotciv, shanaynay),			//assigns robot drive talons theiw PWM ports

	shooterVictor(6),		//assigns victor its PWM port
	shooterVictor2(7),
	shooterVictor3(8),

	loaderArm(3),			//assign servo its PWM port

	driverStick(1),			//assign joysticks their USB ports
	driverStick2(2),
	shooterStick(3),

	climbingArm1(8),		//assigns climbing victors PWM ports
	climbingArm2(9),

	SC1(1),				//assigns climbing pneumatics digital output ports
	SC2(2),

	Disk1(5),			//assigns limit switches digital input ports
	Disk2(6),
	Disk3(7),
	
	g1(1)
	{


	Compressor *c = new Compressor(4, 2);
	c->Start();

	Watchdog().SetEnabled(false);

	rDrive.SetExpiration(0.1);
	rDrive.SetSafetyEnabled(true);			//sets motor saftey on for all motors

	shooterVictor.SetExpiration(0.1);
	shooterVictor.SetSafetyEnabled(true);

	shooterVictor2.SetExpiration(0.1);
	shooterVictor2.SetSafetyEnabled(true);

	shooterVictor3.SetExpiration(0.1);
	shooterVictor3.SetSafetyEnabled(true);

	climbingArm1.SetExpiration(0.1);
	climbingArm1.SetSafetyEnabled(false);

	climbingArm2.SetExpiration(0.1);
	climbingArm2.SetSafetyEnabled(false);

	}
	 
	 

	void Autonomous(void)
	{

	g1.Reset();	
		
	DriverStationLCD *ds = DriverStationLCD::GetInstance();

	ds->Clear();
	
	double AutoTime = 0;			//used to measure time during autonomous


	rDrive.SetSafetyEnabled(false);				//turn of motor saftey timers during autonomous for convience

	shooterVictor.SetSafetyEnabled(false);
	shooterVictor2.SetSafetyEnabled(false);
	shooterVictor3.SetSafetyEnabled(false);

	ds->PrintfLine(DriverStationLCD::kUser_Line1, "Program Started Successfully!");
	ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is in Autonomous");

	ds->UpdateLCD();

	while(IsAutonomous()){
		
		//top autonomous
		if(AutoTime>=0 && AutoTime<0.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.5, 0.0);
		}
		if(AutoTime>=0.5 && AutoTime<1.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
		}
		if(AutoTime>=1.0 && AutoTime<1.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
			shooterVictor.Set(1.0);
			shooterVictor2.Set(1.0);
			shooterVictor3.Set(1.0);
			loaderArm.Set(true);
		}
		if(AutoTime>=1.5 && AutoTime<2.0){
			loaderArm.Set(false);
		}
		if(AutoTime>=2.0 && AutoTime<2.5){
			loaderArm.Set(true);
		}
		if(AutoTime>=2.5 && AutoTime<3.0){
			loaderArm.Set(false);
		}
		if(AutoTime>=3.0 && AutoTime<3.5){
			loaderArm.Set(true);
		}	
		if(AutoTime>=3.5 && AutoTime<4.0){
			loaderArm.Set(false);
			shooterVictor.Set(0.0);
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
		}
		if(AutoTime>=4.0 && AutoTime<5.0){
			rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start	
		}	
		if(AutoTime>=5.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start	
		}	
		
		
		//start at back autonomous
		/*
		if(AutoTime>=0 && AutoTime<0.5){
			rDrive.MecanumDrive_Cartesian(0.5, 0.0, 0.0);  //0.5 for right start, -0.5 for left start
		}
		if(AutoTime>=0.5 && AutoTime<1.0){
			rDrive.MecanumDrive_Cartesian(0.0, 1.0, 0.0);
		}
		if(AutoTime>=1.0 && AutoTime<1.5){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, -0.2);//-0.2 for right start, 0.2 for left start
		}

		if(AutoTime>=1.5 && AutoTime<2.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);
			shooterVictor.Set(1.0);
			shooterVictor2.Set(1.0);
			shooterVictor3.Set(1.0);
			loaderArm.Set(true);
		}
		if(AutoTime>=2.0 && AutoTime<2.5){
			loaderArm.Set(false);
		}
		if(AutoTime>=2.5 && AutoTime<3.0){
			loaderArm.Set(true);
		}
		if(AutoTime>=3.0 && AutoTime<3.5){
			loaderArm.Set(false);
		}
		if(AutoTime>=3.5 && AutoTime<4.0){
			loaderArm.Set(true);
		}	
		if(AutoTime>=4.0 && AutoTime<4.5){
			loaderArm.Set(false);
			shooterVictor.Set(0.0);
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.2); // opposite of above turn
		}
		if(AutoTime>=4.5 && AutoTime<5.0){
			rDrive.MecanumDrive_Cartesian(1.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start	
		}	
		if(AutoTime>=5.0){
			rDrive.MecanumDrive_Cartesian(0.0, 0.0, 0.0);	//1.0 for right start, -1.0 for left start	
		}	
		*/
		AutoTime=AutoTime+0.005;
		Wait(0.005);
		}
	}


	void OperatorControl(void){

	g1.Reset();		
		
	rDrive.SetSafetyEnabled(true);				//turn motor safety timers back on

	shooterVictor.SetSafetyEnabled(true);
	shooterVictor2.SetSafetyEnabled(true);
	shooterVictor3.SetSafetyEnabled(true);

	bool gyroe = true;

	float shootspeed=0.25;
	
	DriverStationLCD *ds = DriverStationLCD::GetInstance();

	ds->PrintfLine(DriverStationLCD::kUser_Line6, "Pinkie Pie Is Best Pony");

	ds->UpdateLCD();

		while (IsOperatorControl()){

		ds->PrintfLine(DriverStationLCD::kUser_Line2, "Robot Is In TeleOp");
		
		shootspeed=speedcontrol(shootspeed);
		
		
		driving(gyroe);   			//runs drive control function
		
		ShooterControl(shootspeed);		//runs shooter control function

		LoaderControl();		//runs servo control function

		Climbing();			//runs pryamid climbing function

		
		
		//DiskCount();			//runs diskcount function

		ds->UpdateLCD();

		Wait(0.005);			//waits 0.02 seconds
			}
		}

	

	float speedcontrol(float s){
		DriverStationLCD *ds = DriverStationLCD::GetInstance();	
		if (shooterStick.GetRawButton(6)){
			s=0.25;
		}
		else if (shooterStick.GetRawButton(7)){
			s=0.5;
		}
		else if (shooterStick.GetRawButton(10)){
			s=0.75;
		}
		else if (shooterStick.GetRawButton(11)){
			s=1.0;
		}
		ds->PrintfLine(DriverStationLCD::kUser_Line1, "Shooter Speed: %d", s);
		return s;
		
	}
	
	 


	
	void driving(bool gyroe){
		double y = driverStick.GetY();           //variable for forward/backward movement
		double x = driverStick.GetX();           //variable for side to side movement
		double turn = driverStick2.GetX() ;        //variable for turning movement
		

		rDrive.MecanumDrive_Cartesian(x, y, -turn);
		
	}


	void ShooterControl(float shootspeed){			//shooter control function
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		if(shooterStick.GetRawButton(1)){
			shooterVictor.Set(shootspeed);			//if trigger is pressed, turn on shooter
			shooterVictor2.Set(shootspeed);
			shooterVictor3.Set(shootspeed);
			ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is On");
			}
		else{
			shooterVictor.Set(0.0);			//if trigger isnt pressed, turn off shooter
			shooterVictor2.Set(0.0);
			shooterVictor3.Set(0.0);
			ds->PrintfLine(DriverStationLCD::kUser_Line3, "Shooter is Off");
			}
		}


	void LoaderControl(){			//servo control function
		if(shooterStick.GetRawButton(3)){
			loaderArm.Set(true);			//if middle button is pressed, extend servo
			}
		else{
			loaderArm.Set(false);				//if middle button isnt pressed, retract servo
			}
		}


	void Climbing(){						//pyramid climbing function, happens automatically
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		if( driverStick.GetRawButton(4)){		//if button 6 pressed on both sticks, starts climb

		ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Up");
		SC1.Set(true);
		SC2.Set(true);
		}
		if(driverStick.GetRawButton(5)){
			ds->PrintfLine(DriverStationLCD::kUser_Line4, "Arms Down");
			SC1.Set(false);
			SC2.Set(false);
		}
	}
		


	/*
	void DiskCount(){			//function that counts disk in the hopper and displays it
		DriverStationLCD *ds = DriverStationLCD::GetInstance();
		int disknum = 0;		//variable that counts disks in hopper

		if(Disk1.Get()){
			disknum=disknum+1;	//disk has activated a limit switch, add 1 to # of disks
			}
		if(Disk2.Get()){
			disknum=disknum+1;
			}
		if(Disk3.Get()){
			disknum=disknum+1;
			}

		ds->PrintfLine(DriverStationLCD::kUser_Line5, "Disk's in Hopper: %d", disknum);	//print out the number of disks in hopper


		}
*/
	};

START_ROBOT_CLASS(RobotDemo);		//starts the class running

We had a similar problem a few weeks ago with our mecanum drive. Turns out our drive motors were inverted in the wiring. You can either fix that in the wiring (which I guess you can’t do right now), or you can simply invert the motors in the code.

And at the end of your driving() routine, the line:
if (gyroe=true){

likely doesn’t do what you want…it does not test to see if the value of “gyroe” is “true”, but assigns the value “true” to the variable “gyroe”, then evaluates the result – which will ALWAYS be true. What you want is:
if (gyroe==true){

You can catch these errors at compile time if you write it this way:
if (true == gyroe){

because then if you forget one of the equal signs, then you are attempting to assign a value to the constant “true”, which the compiler will flag as an error. I learned that the hard way. More than once. :slight_smile:

Most modern compilers (though I don’t recall whether the gcc used by Wind River is one of them) will give you a warning when you do “if (x = 1)” instead of “if (x == 1)”. It is generally considered good practice to have code that doesn’t generate any compiler warnings, and you will avoid errors such as these. I generally turn the warning level all the up as well, to avoid even more errors.

WindRiver ships a (probably modified by them) 3.4.4 release of gcc. That bugfix version was released on May 18, 2005. The minor version, 3.4.0 (the last one to have new features, like warnings), was released April 20, 2004.

Hilariously, 3.4.4 isn’t even the newest bugfix release (3.4.6 exists)

I wouldn’t count it as modern. That said, I haven’t actually tested it to see if it emits a warning related to = in if statements.