C++ DriverStationLCD::Printf方法代码示例

 void TeleopPeriodic(void)
 {
         //Drive code
         leftFrontDrive->Set(-1 * leftStick->GetY());
         rightFrontDrive->Set(rightStick->GetY());
         leftRearDrive->Set(-1 * leftStick->GetY());
         rightRearDrive->Set(rightStick->GetY());
         
         //airCompressor->Start();
         // Simple Button Drive Forward
         if(rightStick->GetRawButton(3))
         {
         	leftFrontDrive->Set(-1);
         	rightFrontDrive->Set(1);
         	leftRearDrive->Set(-1);
         	rightRearDrive->Set(1);
         }
         
         // Simple Button Drive Backwards
         if(rightStick->GetRawButton(2))
         {
         	leftFrontDrive->Set(1);
         	rightFrontDrive->Set(-1);
         	leftRearDrive->Set(1);
         	rightRearDrive->Set(-1);
         }
         
         // Code to print to the user messages box in the Driver Station
         LCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hello World");
         LCD->Printf(DriverStationLCD::kUser_Line2,1,"Y Left: %f", leftStick->GetY());
         LCD->Printf(DriverStationLCD::kUser_Line3,1,"Y Right: %f", rightStick->GetY());
         LCD->UpdateLCD();
         Wait(0.2); 
 }

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			myRobot.ArcadeDrive(stick.getAxisLeftY(), stick.getAxisLeftX()); // drive with arcade style (use left stick)
			dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Move: %f4", stick.getAxisLeftY());
			dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "Rotate: %f4", stick.getAxisLeftX());
			dsLCD->UpdateLCD();
			Wait(0.005);				// wait for a motor update time
		}
	}

	void Print ()
		{
			if (PrintTime.Get() > PRINT_TIME)
			{
				lcd->Clear();
				lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Left Speed = %5.4f", PrimaryController.GetRawAxis(LEFT_JOYSTICK));
				lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Right Speed = %5.4f", PrimaryController.GetRawAxis(RIGHT_JOYSTICK));
				lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Charge State = %d", (int)Shooter.chargestate);
				//lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Collector speed= %d", Collector.CollectorSpeed());
				lcd->UpdateLCD();
				PrintTime.Reset();
				PrintTime.Start();
			}
		}

	void Disabled()
	{
		while(IsDisabled())
		{
			LEDLight->Set(Relay::kForward);
			rpi->Read();
			lcd->Clear();
			lcd->Printf(DriverStationLCD::kUser_Line3, 1, "R: %i", rpi->GetMissingPacketcount());
			lcd->Printf(DriverStationLCD::kUser_Line1, 1, "x: %i", rpi->GetXPos());
			lcd->Printf(DriverStationLCD::kUser_Line2, 1, "y: %i", rpi->GetYPos());

			lcd->UpdateLCD();
		}
	}

void DriverstationMessages::SendTextLines()
{
	DriverStationLCD *lcd =DriverStationLCD::GetInstance();
	for(size_t i = 0; i < LENGTH(text);i++){
			lcd->Printf((DriverStationLCD::Line)i, 1, "%s", (char *)text[i]);
	}
	lcd->UpdateLCD();	
}

void
DriverMessages::SendTextLines()
{
	DriverStationLCD *lcd =DriverStationLCD::GetInstance();
	
	for(int i = 0; i < 3;i++){
			lcd->Printf((DriverStationLCD::Line)i, 1, "%s", (char *)lineText[i]);
	}
	lcd->UpdateLCD();
	
}

	RobotDemo(void)
	{
		motor = new Jaguar(9);
		stick = new Joystick(1);
		compressor = new Compressor(1, 1);
		valve = new Solenoid(8);
		// Construct the dashboard sender object used to send hardware state
		// to the driver station
//		dds = new DashboardDataSender();
		dsLCD = DriverStationLCD::GetInstance();
		dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Plyboy test code: 6:46PM 1/2/2012");
		dsLCD->UpdateLCD();
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		Saftey->SetEnabled(false);
		//myRobot->SetSafetyEnabled(false);
		//myRobot->Drive(0.5, 0.0); 	// drive forwards half speed
		dsLCD = DriverStationLCD::GetInstance();
		dsLCD->Clear();
		//dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Hello World" );
		//dsLCD->UpdateLCD();
		
		Wait(0.5);
		
		ds = DriverStation::GetInstance();
		switch(ds->GetLocation())
		{
		case 1:
			//Execute Autonomous code #1
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 1");
			break;
		case 2:
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 2");
			//Execute Autonomous code #2
			break;
		case 3:
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Executing Autonomous 3");
			//Execute Autonomous code #3
			break;
			
		}
		dsLCD->UpdateLCD();
		
		Saftey->SetEnabled(false);
		Wait(0.5); 				//    for 2 seconds
		delete Jaguar1;
		delete Jaguar2;
		delete Saftey;
		delete dsLCD;
		delete ds;
	}

	void TeleopPeriodic(void) {
		// increment the number of teleop periodic loops completed
		m_telePeriodicLoops++;
		
		//Feed joystick inputs to each subsystem here
		
		
		//Using triggers to turn;
		ds->Printf(DriverStationLCD::kUser_Line1,0, "GetY: %f",DriveStick->GetY());
		ds->Printf(DriverStationLCD::kUser_Line2,0, "GetZ: %f",DriveStick->GetZ());
		MyRobot.DriveRobot(DriveStick->GetY(),(-DriveStick->GetZ()));
		//original:MyRobot.DriveRobot(DriveStick->GetY(),DriveStick->GetX());
		//MyRobot.DriveRobotTrig(DriveStick->GetY(),DriveStick->GetX());
		
	} // TeleopPeriodic(void)

		void OperatorControl (void) {
			GetWatchdog().SetEnabled(true); // We do want Watchdog in Teleop, though.
			
			DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
			
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
			dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
			
			SmartDashboard::GetInstance()->PutData("kinectMode?", kinectModeSelector);
			SmartDashboard::GetInstance()->PutData("demoMode?", demoModeSelector);
			SmartDashboard::GetInstance()->PutData("speedMode?", speedModeSelector);
			
			while (IsOperatorControl() && IsEnabled()) {
				GetWatchdog().Feed(); // Feed the Watchdog.
				
				kinectMode = (bool) kinectModeSelector->GetSelected();
				demoMode = (bool) demoModeSelector->GetSelected();
				speedModeMult = static_cast<double*>(speedModeSelector->GetSelected());
				
				if (kinectMode) {
					dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
					dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Kinect Mode");
					
					if (!demoMode) {
						if (kinectDrive.GetRawButton(KINECT_FORWARD_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_REVERSE_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_LEFT_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * -1 * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * -1 * *speedModeMult);
						} else if (kinectDrive.GetRawButton(KINECT_RIGHT_BUTTON)) {
							motorDriveLeft.Set(LEFT_DRIVE_POWER * *speedModeMult);
							motorDriveRight.Set(RIGHT_DRIVE_POWER * *speedModeMult);
						} else {
							motorDriveLeft.Set(0);
							motorDriveRight.Set(0);
						}
					}
			
					if (kinectManipulator.GetRawButton(KINECT_SHOOT_BUTTON)) {
						motorShooter.Set(SHOOTER_MOTOR_POWER);
						motorFeed.Set(FEED_MOTOR_POWER);
						motorPickup.Set(PICKUP_MOTOR_POWER);
					} else if (kinectManipulator.GetRawButton(KINECT_SUCK_BUTTON)) {
						motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
						motorFeed.Set(FEED_MOTOR_POWER * -1);
						motorPickup.Set(PICKUP_MOTOR_POWER * -1);
					} else {
						motorShooter.Set(0);
						motorFeed.Set(0);
						motorPickup.Set(0);
					}
				
					if (kinectManipulator.GetRawButton(KINECT_TURRET_LEFT_BUTTON)) {
						motorTurret.Set(TURRET_POWER);
					} else if(kinectManipulator.GetRawButton(KINECT_TURRET_RIGHT_BUTTON)) {
						motorTurret.Set(TURRET_POWER * -1);
					} else {
						motorTurret.Set(0);
					}
				} else {
					if (joystickDriveLeft.GetThrottle() == 0) {
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Xbox Mode");
						
						motorDriveLeft.Set(Deadband(joystickManipulator.GetRawAxis(2) * -1 * *speedModeMult));
						motorDriveRight.Set(Deadband(joystickManipulator.GetRawAxis(5) * *speedModeMult));
						
						if (joystickManipulator.GetRawButton(XBOX_SHOOT_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SHOOT_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER);
							motorFeed.Set(FEED_MOTOR_POWER);
							motorPickup.Set(PICKUP_MOTOR_POWER);
						} else if (joystickManipulator.GetRawButton(XBOX_SUCK_BUTTON) || (demoMode && joystickDriveRight.GetRawButton(XBOX_SUCK_BUTTON))) {
							motorShooter.Set(SHOOTER_MOTOR_POWER * -1);
							motorFeed.Set(FEED_MOTOR_POWER * -1);
							motorPickup.Set(PICKUP_MOTOR_POWER * -1);
						} else {
							motorShooter.Set(0);
							motorFeed.Set(0);
							motorPickup.Set(0);
						}
						
						if (joystickManipulator.GetRawAxis(3) > 0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) > 0.2)) {
							motorTurret.Set(TURRET_POWER);
						} else if(joystickManipulator.GetRawAxis(3) < -0.2 || (demoMode && joystickDriveRight.GetRawAxis(3) < -0.2)) {
							motorTurret.Set(TURRET_POWER * -1);
						} else {
							motorTurret.Set(0);
						}
					} else {
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "                     ");
						dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Joystick Mode");
						
						motorDriveLeft.Set(Deadband(joystickDriveLeft.GetY() * -1 * *speedModeMult));
						motorDriveRight.Set(Deadband(joystickDriveRight.GetY() * *speedModeMult));
						
						if (joystickManipulator.GetRawButton(MANIPULATOR_SHOOT_BUTTON)) {
//.........这里部分代码省略.........

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		double tm = GetTime();
		
		//AccelerationReset();
		
		AverageWindowFilter<double, 20> fx, fy;
		
		GetWatchdog().SetEnabled(false);
		
		double ax = 0, lastAx = 0;
		double ay = 0, lastAy = 0;
		
		for (int i = 0; i < 20; i++)
		{
			GetAcceleration(ax, ay);
			fx.AddPoint(ax);
			fy.AddPoint(ay);
			
			Wait(0.05);
		}
		
		avgX = fx.GetAverage();
		avgY = fy.GetAverage();
		
		double minX = 0, maxX = 0;
		double minY = 0, maxY = 0;
		
		GetWatchdog().SetEnabled(true);
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();
			myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
			
			if (GetTime() - tm > 0.05)
			{
				GetAcceleration(ax, ay);
				
				fx.AddPoint( ax - avgX );
				fy.AddPoint( ay - avgY );
				
				ax = fx.GetAverage();
				ay = fy.GetAverage();
				
				minX = min(fabs(ax - lastAx), minX);
				maxX = max(fabs(ax - lastAx), maxX);
				minY = min(fabs(ay - lastAy), minY);
				maxY = max(fabs(ay - lastAy), maxY);
				
				lastAx = ax;
				lastAy = ay;
				
				//AccelerationUpdate( ax, ay, .1);
				
				//get the filtered acceleration, velocity and position
				//GetAcceleration( &ax, &ay);
				
				
				// or
				// ax = (((axH / 0.01) - .5) * 8.0) * 9.81;
				// ay = (((ayH / 0.01) - .5) * 8.0) * 9.81;
								
				DriverStationLCD * lcd = DriverStationLCD::GetInstance();
				
				
				
				lcd->Printf(DriverStationLCD::kUser_Line3, 1, "ax: %f m/s^2            ", ax);
				lcd->Printf(DriverStationLCD::kUser_Line4, 1, "%.6f %.6f               ", minX, maxX);
				lcd->Printf(DriverStationLCD::kUser_Line5, 1, "ay: %f m/s^2            ", ay);
				lcd->Printf(DriverStationLCD::kUser_Line6, 1, "%.6f %.6f               ", minY, maxY);
				
				// euler' integration method.. bad bad bad
				//vx += ax / (0.01);
				//vy += ay / (0.01);
				//lcd->Printf(DriverStationLCD::kUser_Line4, 1, "vx: %.1f", vx);
				//lcd->Printf(DriverStationLCD::kUser_Line6, 1, "vy: %.1f", vy);
				
				lcd->UpdateLCD();
				tm = GetTime();
			}
		}
	}

	/**
	 * Autonomous code
	 */
	void Autonomous(void) {

		
		 //disable watchdog and start timer
		 GetWatchdog().SetEnabled(false);
		 gameTimer->Start();
		 gameTimer->Reset();
		
		 float speed = 0.15; //CHECK-> enough speed to get to the peg in time as shoulder rises slowly            
		 /*
		 //variables used to hold light sensors' values
		 rightSensor = 0;
		 leftSensor = 0;
		 middleSensor = 0;
		 */
		 while (IsAutonomous())
		  {
		    time_to_send++;
		    
		    if(time_to_send >50 )
		   {
		     time_to_send=0;
		     //Print a message to the Driver Station LCD
		     dsLCD->Printf(DriverStationLCD::kUser_Line,1, "Autonomous is running, RUN FOR COVER"); //Give output to dsLCD
		     dsLCD->UpdateLCD();
		   }

		   for(int x=0; x<4; x++) 
		     {
		        
		       if (x==1)
			 {
			   myRobot->Drive(0.0,0.0); //Stop the robot initially
			 }
		          
		       else if (x==2)
			 {
			   myRobot->Drive(speed,0.0); //Move the robot
			 }    
		       else if (x==3)
			 {
			   myRobot->Drive(0.5,0.); //Decrease the speed
			 }
		       else
			 {
			   myRobot->Drive(0.0,0.0); //If anything else happens, STOP the robot
			 }     
		     }
		   break;
		 }
		   



		 /*
		 float releaseVoltage;
		 bool reachedEndOfLine = false;
		 int followingLineNumber = 1; //2 is for the 'y' line; reading line number from left to right


		 //keep following line until robot reaches the 'T' - move shoulder to appropriate position simultaneously
		 while (reachedEndOfLine == false) {

		 //read the various sensors
		 //encoderReading = wheelEncoder->GetDistance();
		 shoulderPotentiometerReading = (5
		 -(shoulderPotentiometerChannel->GetVoltage())); // reads the potentiometer at channel 1


		 // LINE FOLLOWING CODE
		 leftSensor = left->Get() ? 1 : 0;
		 middleSensor = middle->Get() ? 1 : 0;
		 rightSensor = right->Get() ? 1 : 0;

		 if (leftSensor == 0 && middleSensor == 1 && rightSensor == 1) {
		 myRobot->Drive(speed, -0.5); // right and middle sensors are on line	

		 } else if (leftSensor == 1 && middleSensor == 1 && rightSensor == 0) {
		
	         myRobot->Drive(speed, 0.5); // left and middle sensors are on line
		
		 } else if ((leftSensor == 1 && middleSensor == 1 && rightSensor
		 == 1) || (leftSensor == 1 && middleSensor == 0
		 && rightSensor == 1)) {
		 //CHECK - add support for 'y' line, if necessary
		 reachedEndOfLine = true;

		 } else if (leftSensor == 0 && middleSensor == 0 && rightSensor == 0) {
		 //robot is off the line, stop
		 myRobot->Drive(speed, 0.0);

		 } else if (leftSensor == 0 && middleSensor == 1 && rightSensor == 0) {
		 myRobot->Drive(speed, 0.0); //only middle sensor is on line, go forward
		 }

		 //raise the shoulder while following line, depending on which line robot is following
		 if (followingLineNumber == 1 || followingLineNumber == 3) {
//.........这里部分代码省略.........

	/******************************************************
	 * Drive left & right motors for 2 seconds then stop  *
	 ******************************************************/
	void Autonomous(void)
	{
		//increase 2nd RPM
		compressor.Start(); // starts the compressor; 
		bool BRIDGE = bridge.Get();
		bool HIGH = high.Get();
		bool MIDDLE = middle.Get();
		bool TWOSEC = twosec.Get();
		bool FIVESEC = fivesec.Get();
		int highRPM = 1800; // 1st 1800 short about 5 ft
		int secondhighRPM = 1800; //1st 1400 (did not fire)
		int rpmForShooter;
		DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory
		char debugout [100];
	//	baddog.SetExpiration(30.0);
	//	baddog.Feed();
		dslcd->Clear(); 
		sprintf(debugout,"Bridge=%u",BRIDGE); 
		dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
		sprintf(debugout,"High=%u",HIGH); 
		dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
		sprintf(debugout,"Middle=%u",MIDDLE); 
		dslcd->Printf(DriverStationLCD::kUser_Line3,3,debugout);
		sprintf(debugout,"TwoSec=%u",TWOSEC); 
		dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
		sprintf(debugout,"FiveSec=%u",FIVESEC); 
		dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
		dslcd->UpdateLCD(); // update the Driver Station with the information in the code */
		if (BRIDGE == 0 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0)
			{
				myRobot.Drive(0.0, 0.0);
				Wait(10.0);
			}
		if (BRIDGE == 1 && HIGH == 0 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //1700 RPM
			{
				/*myRobot.Drive(-0.5, 0.0);
				Wait(3.0);
				*/
			
							flashring.Set(Relay::kForward);
							ShooterTimer.Reset();
							ShooterTimer.Start();
							while(ShooterTimer.Get()<7)
							{	
								shooter.setTargetRPM(1700);
								//wait-0.01
								Wait(0.005);
							}

							lynx.Set(-1.0);
							ShooterTimer.Reset();
							ShooterTimer.Start();
							while(ShooterTimer.Get()<1)
							{	
								shooter.setTargetRPM(1700);
								Wait(0.005);
							}
							turret.Set(-0.05);
							ShooterTimer.Reset();
							ShooterTimer.Start();
							while(ShooterTimer.Get()<0.2)
							{	
								shooter.setTargetRPM(1700);
								Wait(0.005);
							}
							turret.Set(0.0);
							ShooterTimer.Reset();
							ShooterTimer.Start();
							while(ShooterTimer.Get()<2.0)
							{	
								shooter.setTargetRPM(1700);
								Wait(0.005);
							}
			//				lynx.Set(0.0);
							shepard.Set(true);
							ShooterTimer.Reset();
							ShooterTimer.Start();
							while(ShooterTimer.Get()<2.0)
							{	 
								shooter.setTargetRPM(1700);
								Wait(0.005);
							}
							shepard.Set(false);
							shooter.setTargetRPM((int)0);
							flashring.Set(Relay::kOff);
							lynx.Set(0.0);
			}
		if (BRIDGE == 1 && HIGH == 1 && MIDDLE == 0 && TWOSEC == 0 && FIVESEC == 0) //main autonomous code-default
			{
		    	flashring.Set(Relay::kForward);
				ShooterTimer.Reset();
				ShooterTimer.Start();
				while(ShooterTimer.Get()<7)
				{	
					shooter.setTargetRPM((int)highRPM);
					//wait-0.01
					Wait(0.005);
//.........这里部分代码省略.........

void RobotDemo::DriverLCD()
{
	if (cycle_counter >= 50)
	{
		dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "RPS Back:%f  ",
				RPS_back);
		dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "RPS Front:%f  ",
				RPS_front);
		dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "RPS DRPS:%f  ",
				desired_RPS_control);
#if 0
		if (shooter_fire_piston_A->Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"Fire   ");
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"Retracting...   ");
		}
#endif
		//dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"TopLS:%i   BotLS:%i   ", top_claw_limit_switch->Get(),
		//	bottom_claw_limit_switch ->Get());
		if (top_claw_limit_switch->Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "!TOP");
		}
		else if (!bottom_claw_limit_switch->Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "!BOTTOM");
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Neither");
		}
		if (shooter_angle_1->Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Up   ");
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Down   ");
		}
		if (arcadedrive)
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Arcade   ");
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Tank   ");
		}
		dsLCD->UpdateLCD();
		//cycle_counter = 0;
	}
	//cycle_counter++;
}

//.........这里部分代码省略.........
						}
					}
					else
					{
						if (controller.GetRawAxis(4) <= 0)
							percent = ((acos(controller.GetRawAxis(3)) / pi));
						else if (controller.GetRawAxis(4) > 0)
							percent = ((acos(-controller.GetRawAxis(3)) / pi));
						fpotval = percent * 550 + 330;
						percent = (fpotval - 330) / 550;
						bpotval = percent * 500 + 245;
						
						if (fpot.GetValue() < fpotval)
							fchgval = -.5;
						
						else if (fpot.GetValue() > fpotval)
							fchgval = .5;
						
						if (fpot.GetValue() < fpotval + 10 && fpot.GetValue() > fpotval - 10)
							fchgval = 0;
			
						if (bpot.GetValue() > bpotval)
							bchgval = -.5;
									
						else if (bpot.GetValue() < bpotval)
							bchgval = .5;
									
						if (bpot.GetValue() < bpotval + 20 && bpot.GetValue() > bpotval - 20)
							bchgval = 0;
						
						frot.Set(fchgval);
						brot.Set(bchgval);
						
						if (pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2) > deadband && controller.GetRawButton(BTN_R1))
						{
							if (controller.GetRawAxis(4) > 0)
							{
								flmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								frmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								blmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								brmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
							}
							
							else if (controller.GetRawAxis(4) < 0)
							{
								flmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								frmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								blmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								brmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
							}
						}
						else
						{
							flmot.Set(0);
							frmot.Set(0);
							blmot.Set(0);
							brmot.Set(0);
						}
						contaxis4 = 0;
					}
				}
				else
				{
					if (contaxis4 == 0)
						contaxis4 = controller.GetRawAxis(4);
					if (controller.GetRawButton(BTN_R1))
					{
						if (contaxis4 > 0)
						{
							flmot.Set(-0.25);
							frmot.Set(0.25);
							blmot.Set(-0.25);
							brmot.Set(0.25);
						}
						else if (contaxis4 < 0)
						{
							flmot.Set(0.25);
							frmot.Set(-0.25);
							blmot.Set(0.25);
							brmot.Set(-0.25);
						}
					}	
				}
				if (float (fpot.GetValue()) < min)
					min = float (fpot.GetValue());
				
				else if (float (fpot.GetValue()) > max)
					max = float (fpot.GetValue());
				
				if (counter >= .1)
				{
					lcda->Clear();
					lcda->Printf(DriverStationLCD::kUser_Line3, 1, "FPot :: %d", fpotval);
					lcda->Printf(DriverStationLCD::kUser_Line4, 1, "BPot :: %d", bpotval);
					lcda->UpdateLCD();
					timer.Reset();
				
				}
		}
	}