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

	void Autonomous(void)
	{
		dsLCD->Clear();
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix");
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Autonomous Mode");
		dsLCD->UpdateLCD();
	}

	RobotDemo(void):
		myRobot(LEFT_DRIVE_PWM, RIGHT_DRIVE_PWM),	// these must be initialized in the same order
		stick(1),									// as they are declared above.
		stick2(2),
		gamepad(3),
		collectorMotor(PICKUP_PWM),
		indexerMotor(INDEX_PWM),
		shooterMotor(SHOOTER_PWM),
		armMotor (ARM_PWM),
		shifter(SHIFTER_A,SHIFTER_B),
		greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED),
		yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED),
		potentiometer(ARM_ROTATION_POT),
		indexSwitch(INDEXER_SW),
		compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE)
	{
		m_collectorMotorRunning = false;
		m_shooterMotorRunning   = false;
		
		dsLCD = DriverStationLCD::GetInstance();
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME);
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);

		dsLCD->UpdateLCD();
		myRobot.SetExpiration(0.1);
		shifter.Set(DoubleSolenoid::kReverse);
	}

	void output (void)
	{
		REDRUM;
		if (IsAutonomous())
			driverOut->PrintfLine(DriverStationLCD::kUser_Line1, "blaarag");
		else if (IsOperatorControl())
		{	
			REDRUM;
		}
		outputCounter++;
					
					if (outputCounter % 30 == 0){
						REDRUM;
					driverOut->PrintfLine(DriverStationLCD::kUser_Line2, "Top Shooter RPM: %f",(float)LTop.GetSpeed());
					driverOut->PrintfLine(DriverStationLCD::kUser_Line3, "Bot Shooter RPM: %f",(float)LBot.GetSpeed());
			//		driverOut->PrintfLine(DriverStationLCD::kUser_Line6, "Pilot Z-Axis: %f",pilot.GetZ());
					
					}
					
					if (outputCounter % 60 == 0){
						REDRUM;
					driverOut->PrintfLine(DriverStationLCD::kUser_Line4, "Top CANJag Temp: %f Celcius",LTop.GetTemperature()*(9/5) + 32);
					driverOut->PrintfLine(DriverStationLCD::kUser_Line5, "Bot CANJag Temp: %f Celcius",LBot.GetTemperature()*(9/5) + 32);
				    outputCounter = 1;
					}
		driverOut->UpdateLCD();
	}//nom nom nom

	void AutonomousStateMachine() {
		pneumaticsControl->initialize();
		shooterControl->initializeAuto();
		driveControl.initializeAuto();
		enum AutoState {
			AutoDrive, AutoSetup, AutoShoot
		};
		AutoState autoState;
		autoState = AutoDrive;

		bool feederState = false;
		bool hasFired = false;
		Timer feeder;

		while (IsAutonomous() && IsEnabled()) {
			GetWatchdog().Feed();
			switch (autoState) {//Start of Case Machine
			case AutoDrive://Drives the robot to set Destination 
				bool atDestination = driveControl.autoPIDDrive2();
				if (atDestination) {//If at Destination, Transition to Shooting Setup
					autoState = AutoSetup;
				}
				break;
			case AutoSetup://Starts the ballgrabber motors to place the ball and extends ballgrabber
				if (!pneumaticsControl->ballGrabberIsExtended()) {//extends ballgrabber if not already extended
					pneumaticsControl->ballGrabberExtend();
				}

				if (!feederState) {//Started the feeder timer once
					feederState = true;
					feeder.Start();
					autoState = AutoShoot;
				}

				break;
			case AutoShoot://Shoots the ball
				if (feeder.Get() < 2.0) {//Runs ballgrabber for 2.0 Seconds at most
					shooterControl->feed(true);
				} else if (feeder.Get() >= 2.0) {//Transition to Shooting
					shooterControl->feed(false);
					feeder.Stop();
				}
				
				if (pneumaticsControl->ballGrabberIsExtended() && !hasFired) {
					shooterControl->autoShoot();
					dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
							"The robot is(should be) shooting");
					if (shooterControl->doneAutoFire()) {//Returns true only after shoot is done firing
						hasFired = true;
					}
				} else if (hasFired) {//runs only after shoot is done
					dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
							"AutoMode Finished");
				}
				break;
			}
			dsLCD->UpdateLCD();

		}
	}

	void OperatorControl()
	{
		// Loop counter to ensure that the program is running (debug helper
		// that can be removed when things get more stable)
		int sanity, bigSanity = 0;
		
		gamepad.Update();

		while (IsOperatorControl() && IsEnabled())
		{
			controls = Controls::GetInstance();
			
			controls->SetSpeed(LEFT_DRIVE_PWM, -1.0 * gamepad.GetRightY());
			controls->SetSpeed(RIGHT_DRIVE_PWM, -1.0 * gamepad.GetRightY());
			
			gamepad.Update();
			
			dsLCD->Clear();
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Fix");
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Teleop Mode");
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "bigSanity: %d", sanity);
			dsLCD->UpdateLCD();
			sanity++;
			if (0 == sanity % 20)
			{
				bigSanity++;
			}

			Wait(0.05);				// wait for a motor update time
		}
	}

	void Drive (float speed, int dist)
	{
		leftDriveEncoder.Reset();
		leftDriveEncoder.Start();
		
		int reading = 0;
		dist = abs(dist);
		
		// The encoder.Reset() method seems not to set Get() values back to zero,
		// so we use a variable to capture the initial value.
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "initial=%d\n", leftDriveEncoder.Get());
		dsLCD->UpdateLCD();

		// Start moving the robot
		robotDrive.Drive(speed, 0.0);
		
		while ((IsAutonomous()) && (reading <= dist))
		{
			reading = abs(leftDriveEncoder.Get());				
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "reading=%d\n", reading);
			dsLCD->UpdateLCD();
		}

		robotDrive.Drive(0.0, 0.0);
		
		leftDriveEncoder.Stop();
	}	

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void) {

		while (IsOperatorControl()) {
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Voltage: %f",
					signal.GetVoltage());
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "CVoltage: %f",
					signalControlVoltage.GetVoltage());
			dsLCD->UpdateLCD();
			Wait(0.005); // wait for a motor update time
		}
	}

	void DriveThenShootAuto() {
		//initizlizes all parts of robot
		pneumaticsControl->initialize();
		shooterControl->initializeAuto();
		driveControl.initializeAuto();
		bool destinationPrevious = false;
		bool autoShot = false; //true if autoShoot

		//creates a timer for the ball grabber motors
		Timer feeding;
		bool started = false;
		while (IsAutonomous() && IsEnabled()) {
			GetWatchdog().Feed();
			//drives forward to shooting point
			bool atDestination = destinationPrevious || driveControl.autoPIDDrive2(); //autoDrive returns true when robot reached it's goal
			if (atDestination) {
				// The robot has reached the destination on the floor and is now ready to open and shoot
				if (!started) {
					started = true;
					destinationPrevious = true;
					//starts feeding-timer controls feeder motors so the ball doesn't get stuck
					feeding.Start();
				}

				pneumaticsControl->ballGrabberExtend();
				//waits for feeding to be done
				if (feeding.Get() < 2.0) {//3.0 was 
					shooterControl->feed(true);
				} else if (feeding.Get() >= 2.0) { // 3.0 was 
					shooterControl->feed(false);
					feeding.Stop();
				}

				if (pneumaticsControl->ballGrabberIsExtended() && !autoShot) {
					shooterControl->autoShoot();
					dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
							"The robot is(should be) shooting");
					if (shooterControl->doneAutoFire()) {//works only after shoot is done firing
						autoShot = true;
					}
				} else if (autoShot) {//runs only after shoot is done
					dsLCD->PrintfLine(DriverStationLCD::kUser_Line1,
							"AutoMode Finished");
				}

			}
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Feeder Time: %f",
					feeding.Get());
			dsLCD->UpdateLCD();
		}

	}

//should move this to helper function
void robot::feed()
{
	DriverStationLCD *lcd = DriverStationLCD::GetInstance();
	lcd->Clear();
	float th = gyro.getangle();
	a = qmod(th * dt + a, -180, 180);
	lcd->PrintfLine(DriverStationLCD::kUser_Line1, "BUILD: %i", BUILD);
	lcd->PrintfLine(DriverStationLCD::kUser_Line2, "%f", gyrob.GetAngle());
	lcd->PrintfLine(DriverStationLCD::kUser_Line3, "%f", arma.GetVoltage());
	lcd->PrintfLine(DriverStationLCD::kUser_Line4, "%f", armb.GetVoltage());
	//lcd->PrintfLine(DriverStationLCD::kUser_Line5, "%f", aa);
	lcd->UpdateLCD();
}

	void TeleopPeriodic(void) {
		// increment the number of teleop periodic loops completed
		m_telePeriodicLoops++;
		static AutoDrive *autoDrive = NULL;
		bool autoButton = DriveStick->GetButton(Joystick::kTriggerButton);
		if (autoButton)
		{
			if (autoDrive == NULL)
				autoDrive = new AutoDrive(m_Configuration->GetValue( m_Constant[ cAutoDrive]) * 100);
			autoDrive->Periodic(MyRobot, ds);
			ds->PrintfLine (DriverStationLCD::kUser_Line5, "autoDrive on");
		}
		else
		{
			ds->PrintfLine (DriverStationLCD::kUser_Line5, "autoDrive off");
			if (autoDrive != NULL)
			{
				MyRobot.ResetCounters();
				delete autoDrive;
				autoDrive = NULL;
			}

			if( !m_Configuration)
			{
				printf( "Configuration Initialize");
				InitializeConfiguration();
			}
			
			m_Configuration->Execute( DriveStick->GetRawButton( 2), DriveStick->GetZ(), ds);
			
			if(DriveStick->GetRawButton( 3))
			{
				ds->PrintfLine(DriverStationLCD::kUser_Line6, "Calculating distance...");
				Vision *vision = new Vision();
				double distance = vision->TakeDistancePicture( ds, m_Configuration->GetValue( m_Constant[ cHBottom]), m_Configuration->GetValue( m_Constant[ cHTop]), m_Configuration->GetValue( m_Constant[ cSBottom]), m_Configuration->GetValue( m_Constant[ cSTop]), m_Configuration->GetValue( m_Constant[ cVBottom]), m_Configuration->GetValue( m_Constant[ cVTop]));
				if( distance < 0.000001)
					ds->PrintfLine(DriverStationLCD::kUser_Line6, "No target found");
				else
					ds->PrintfLine(DriverStationLCD::kUser_Line6, "distance to target: %lf",distance);
				delete vision;
			}

			
			// Real teleop mode: use the JoySticks to drive
				MyRobot.DriveRobot(DriveStick->GetY(),(DriveStick->GetX()), ds);
		}

    	ds->UpdateLCD();
	} // TeleopPeriodic(void)

	void OperatorControl(void) {
		XboxController *xbox = XboxController::getInstance();

		bool isEndGame = false;
		GetWatchdog().SetEnabled(true);
		_driveControl.initialize();
		//_poleVaultControl.initialize();
		shooterControl.InitializeOperator();
		while (IsEnabled() && IsOperatorControl()) {
			GetWatchdog().Feed();
			dsLCD->Clear();
			if (xbox->isEndGame()) {
				isEndGame = !isEndGame;
			}
			if (!isEndGame) {
				shooterControl.Run();
				//_poleVaultControl.act();
				_driveControl.act();
				dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Normal");
				led0->Set((shooterControl.getLED1())?Relay::kOn: Relay::kOff);
				led1->Set((shooterControl.getLED2())?Relay::kOn: Relay::kOff);
			}

			else {
				shooterControl.RunEndGame();
				//_poleVaultControl.actEndGame();
				_driveControl.actEndGame();
				dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "End Game");
				
				flashCount--;
				
				if (flashCount<=0){
					isFlashing = !isFlashing;
					flashCount=FLASHTIME;
				
				}
				
				
				led0->Set((isFlashing)?Relay::kOn: Relay::kOff);
				led1->Set((isFlashing)?Relay::kOn: Relay::kOff);
			}
//			dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "Flash: %i", flashCount);
			dsLCD->UpdateLCD();
			Wait(WAIT_TIME); // wait for a motor update time

		}

		GetWatchdog().SetEnabled(false);
	}

 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); 
 }

	RobotDemo(void):
		gamepad(3)
	{
		dsLCD = DriverStationLCD::GetInstance();
		
		// Output the program name and build date/time in the hope that this will help
		// us catch cases where we are downloading a program other than the one
		// we think we are downloading. Keep in mind that if this source file
		// does not change (and you don't do a complete rebuild) the timestamp
		// will not change.
		
		dsLCD->Clear();
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 Test Menu");
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);
		dsLCD->UpdateLCD();
	}

void LcdDisplaySubsystem::UpdateLines()
{
	DriverStationLCD *lcd = DriverStationLCD::GetInstance();
	
	lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Tilt angle: %f", CommandBase::shooterTiltSubsystem->GetAngle());
	lcd->UpdateLCD();
}

	// Runs during test mode
	// Test
	// * 
	void Test()
	{
		shifters.Set(DoubleSolenoid::kForward);

		leftDriveEncoder.Start();
		leftDriveEncoder.Reset();

		int start = leftDriveEncoder.Get();

		while (IsTest()) {
			if (rightStick.GetRawButton(7)) {
				robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
			}
			else {
				robotDrive.ArcadeDrive(rightStick.GetY()/2, -rightStick.GetX()/2);
			}

			if (gamepad.GetEvent(4) == kEventClosed) {
				start = leftDriveEncoder.Get();
			}

			dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "lde: %d", leftDriveEncoder.Get() - start);
			dsLCD->UpdateLCD();

			gamepad.Update();
		}
	}