C++ DriverStation类代码示例

void DataSending::SendTheData(){
	strIndex = 0;
	Dashboard &dash = DriverStation::GetInstance()->GetHighPriorityDashboardPacker();
	DriverStation *drive = DriverStation::GetInstance();
	Send(true);//yes this is the dataSending Code
	Send(drive->GetBatteryVoltage());//battery info
	Send((batteryCurrent->GetVoltage()-2.5)*AMPS_CONSTANT);
	GetDriveJoystickInfo();//joystick info
	GetOperatorJoystickInfo();//moar joystick info
	GetVicInfo();//victor info
	Send(RobotMap::launcherSolenoidLeft->Get());//solenoid info
	Send(RobotMap::launcherSolenoidRight->Get());
	Send(RobotMap::shifterDoubleSolenoid->Get());
	Send((bool)RobotMap::launcherCompressor->GetPressureSwitchValue());//sensor info
	Send(RobotMap::robotRangeFinderUltrasonicSensor->GetVoltage()/VoltsPerCM);
	Send(RobotMap::driveTrainGyro->GetAngle());
	Send(RobotMap::launcherPressureSwitch->GetVoltage()*PSI_CONSTANT);//transducer1
	Send(RobotMap::collectorLeftRoller->Get()*-1);//talon info
	Send(RobotMap::collectorRightRoller->Get());
	Send(count++);//number of packets
	Send(timesPerSecond);//every nth number data is sent from a 50 hz source
	//a.k.a 50 / above number = Hz
	Send(drive->GetMatchTime());
	Send(drive->IsEnabled());
	dash.AddString(strBuffer);
	dash.Finalize();
	UpdateUserLCD();
}

OI::OI() :
	pIO(NULL),
	driverStick(DRIVER_STICK),
	gunnerStick(GUNNER_STICK)
{
     DriverStation *pDS = DriverStation::GetInstance();
     pIO = &pDS->GetEnhancedIO();
}

MedicOperatorInterface::MedicOperatorInterface()
{
	joyDrive = new Joystick(1);//TODO:real value
	joyManip = new Joystick(2);//TODO:real value
	DriverStation *dsSimple = DriverStation::GetInstance();
	ds = &dsSimple->GetEnhancedIO();
	dsLCD = DriverStationLCD::GetInstance();	
	dashboard->init();
}

void DriverStation::MergeFrom(const DriverStation& from) {
  GOOGLE_CHECK_NE(&from, this);
  if (from._has_bits_[0 / 32] & (0xffu << (0 % 32))) {
    if (from.has_enabled()) {
      set_enabled(from.enabled());
    }
    if (from.has_state()) {
      set_state(from.state());
    }
  }
  mutable_unknown_fields()->MergeFrom(from.unknown_fields());
}

/**
 * Check if this motor has exceeded its timeout.
 * This method is called periodically to determine if this motor has exceeded its timeout
 * value. If it has, the stop method is called, and the motor is shut down until its value is
 * updated again.
 */
void MotorSafetyHelper::Check()
{
	DriverStation *ds = DriverStation::GetInstance();
	if (!m_enabled || ds->IsDisabled() || ds->IsTest()) return;

	::std::unique_lock<ReentrantMutex> sync(m_syncMutex);
	if (m_stopTime < Timer::GetFPGATimestamp())
	{
		char buf[128];
		char desc[64];
		m_safeObject->GetDescription(desc);
		snprintf(buf, 128, "%s... Output not updated often enough.", desc);
		wpi_setWPIErrorWithContext(Timeout, buf);
		m_safeObject->StopMotor();
	}
}

	void DashBoardInput() {
		int i = 0;
		GetWatchdog().SetEnabled(true);
		while (IsAutonomous() && IsEnabled()) {
			i++;
			GetWatchdog().Feed();
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "%f, %i",
					driverStation->GetAnalogIn(1), i);
			dsLCD->UpdateLCD();
		}
	}

	void RA14Robot::logging() {
		if (fout.is_open()) {
		fout << missionTimer->Get() << ",";
#ifndef DISABLE_SHOOTER
		myCam->log(fout);
#endif //Ends DISABLE_SHOOTER
		myDrive->log(fout);
		CurrentSensorSlot * slots[4] = { camMotor1Slot, camMotor2Slot,
				driveLeftSlot, driveRightSlot };
		
		DriverStation * ds = DriverStation::GetInstance();

		for (int i = 0; i < 4; ++i) {
			fout << slots[i]->Get() << ",";
		}

		fout << auto_case << "," << gyro->GetAngle() << "," << dropSensor->GetPosition() << "," << ds->GetBatteryVoltage() << ",";
		fout << target->IsValid() << "," << target->IsHot() << "," <<  target->GetDistance() << "," << target->GetX() << ",";
		fout << target->GetY() << "," << target->IsLeft() << "," << target->IsRight() << ",";
		fout << ds->GetMatchTime() << "," << auto_state << ",";
		fout << endl;
		}
	}

	/**
	 * 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;
	}

    /**
     * Robot-wide initialization code should go here.
     * 
     * Use this method for default Robot-wide initialization which will
     * be called when the robot is first powered on.  It will be called exactly 1 time.
     */
    void RobotInit()
    {
printf(">>> RobotInit\n");

	LogInit();

#ifdef HAVE_COMPRESSOR
	compressor  = new Compressor(1, 1);
#endif

#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
	topWheel1    = new CANJaguar(1);
	topWheel1->SetSafetyEnabled(false);	// motor safety off while configuring
	topWheel1->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
	topWheel1->ConfigEncoderCodesPerRev( 1 );
#endif
#ifdef HAVE_TOP_PWM1
	topWheel1    = new Victor(1);
	topWheel1->SetSafetyEnabled(false);	// motor safety off while configuring
#endif
#ifdef HAVE_TOP_CAN2
	topWheel2    = new CANJaguar(2);
	topWheel2->SetSafetyEnabled(false);	// motor safety off while configuring
	topWheel2->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
	topWheel2->ConfigEncoderCodesPerRev( 1 );
#endif
	topTach      = new Tachometer(2);
#endif

#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
	bottomWheel1 = new CANJaguar(3);
	bottomWheel1->SetSafetyEnabled(false);	// motor safety off while configuring
	bottomWheel1->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
	bottomWheel1->ConfigEncoderCodesPerRev( 1 );
#endif
#ifdef HAVE_BOTTOM_PWM1
	bottomWheel1 = new Victor(2);
	bottomWheel1->SetSafetyEnabled(false);	// motor safety off while configuring
#endif
#ifdef HAVE_BOTTOM_CAN2
	bottomWheel2 = new CANJaguar(4);
	bottomWheel2->SetSafetyEnabled(false);	// motor safety off while configuring
	bottomWheel2->SetSpeedReference( CANJaguar::kSpeedRef_Encoder );
	bottomWheel2->ConfigEncoderCodesPerRev( 1 );
#endif
	bottomTach   = new Tachometer(3);
#endif

#ifdef HAVE_ARM
	arm          = new DoubleSolenoid(2, 1);
#endif
#ifdef HAVE_INJECTOR
	injectorL    = new DoubleSolenoid(5, 3);
	injectorR    = new DoubleSolenoid(6, 4);
#endif
#ifdef HAVE_EJECTOR
	ejector      = new Solenoid(7);
#endif
#ifdef HAVE_LEGS
	legs         = new Solenoid(8);
#endif

	ds           = DriverStation::GetInstance();
	eio          = &ds->GetEnhancedIO();
	gamepad      = new Joystick(1);

	LiveWindow *lw = LiveWindow::GetInstance();
#ifdef HAVE_COMPRESSOR
	lw->AddActuator("K9", "Compressor", compressor);
#endif
#ifdef HAVE_TOP_WHEEL
#ifdef HAVE_TOP_CAN1
	lw->AddActuator("K9", "Top1",       topWheel1);
#endif
#ifdef HAVE_TOP_PWM1
	lw->AddActuator("K9", "Top1",       topWheel1);
#endif
#ifdef HAVE_TOP_CAN2
	lw->AddActuator("K9", "Top2",       topWheel2);
#endif
#endif
#ifdef HAVE_BOTTOM_WHEEL
#ifdef HAVE_BOTTOM_CAN1
	lw->AddActuator("K9", "Bottom1",    bottomWheel1);
#endif
#ifdef HAVE_BOTTOM_PWM1
	lw->AddActuator("K9", "Bottom1",    bottomWheel1);
#endif
#ifdef HAVE_BOTTOM_CAN2
	lw->AddActuator("K9", "Bottom2",    bottomWheel2);
#endif
#endif
//.........这里部分代码省略.........

	float Cypress::GetAnalogIn(int channel) {
		int getchan=abs(channel-4)+1;
		if (enhanced)
			return m_ds->GetEnhancedIO().GetAnalogIn(getchan);
		else
			return m_ds->GetAnalogIn(getchan);
	}

	void Cypress::SetDigitalOut(int channel, bool value) {
		int getchan=abs(channel-8)+1;
		if (enhanced) {
			getchan+=8;
			m_ds->GetEnhancedIO().SetDigitalOutput(getchan, value);
		} else
			m_ds->SetDigitalOut(getchan, value);
	}

	bool Cypress::GetDigitalIn(int channel)

	{
		int getchan=abs(channel-8)+1;
		if (enhanced)
			return !m_ds->GetEnhancedIO().GetDigital(getchan);
		else
			return m_ds->GetDigitalIn(getchan);
	}

	void DisabledInit() { 
		//Config loading
		try {
			cameraLight->Set(Relay::kOff);
			if (!Config::LoadFromFile("config.txt")) {
				cout << "Something happened during the load." << endl;
			}
			Config::Dump();
			
			myDrive->DisablePID();
			myDrive->ResetPID();
			
			if(fout.is_open() && !freshStart && !ds->IsFMSAttached()){
				fout.close();
			myShooter->ResetShooterProcess();
			
			lc->holdState(false);
			}
		} catch (exception ex) {
			cout << "Disabled exception. Trying again." << endl;
			cout << "Exception: " << ex.what() << endl;
		}
		
		//ResetShooterMotors();
		/*
		SmartDashboard::PutNumber("Target Info S",1741);
		cout<<SmartDashboard::GetNumber("Target Info S");
		*/
	}

	/**
	 * 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)
	{
		Victor armMotor(5);						// create arm motor instance
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();

			// determine if tank or arcade mode; default with no jumper is for tank drive
			if (ds->GetDigitalIn(ARCADE_MODE) == 0) {	
				myRobot->TankDrive(leftStick, rightStick);	 // drive with tank style
			} else{
				myRobot->ArcadeDrive(rightStick);	         // drive with arcade style (use right stick)
			}

			// Control the movement of the arm using the joystick
			// Use the "Y" value of the arm joystick to control the movement of the arm
			float armStickDirection = armStick->GetY();

			// if at a limit and telling the arm to move past the limit, don't drive the motor
			if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) {
				armStickDirection = 0;
			} else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) {
				armStickDirection = 0;
			}

			// Set the motor value 
			armMotor.Set(armStickDirection);
		}
	}

	// Test Autonomous
	void Autonomous()
	{
		robotDrive.SetSafetyEnabled(false);
		
		// STEP 1: Set all of the states.
		// SAFETY AND SANITY - SET ALL TO ZERO
		loaded = winchSwitch.Get();
		loading = false;
		intake.Set(0.0);
		rightWinch.Set(0.0);
		leftWinch.Set(0.0);
		
		// STEP 2: Move forward to optimum shooting position
		Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
		
		// STEP 3: Drop the arm for a clean shot
		arm.Set(DoubleSolenoid::kForward);
		Wait(1.0); // Ken
		
		// STEP 4: Launch the catapult
		LaunchCatapult();
		
		Wait (1.0); // Ken

		if (ds->GetDigitalIn(1))
		{
			// STEP 5: Start the intake motor and backup to our origin position to pick up another ball
			InitiateLoad();
			intake.Set(-INTAKE_COLLECT);
			while (CheckLoad());
			Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
			Wait(1.0); // For the ball to collect
			
			// STEP 6: Shut off the intake, bring up the arm and move to shooting position
			intake.Set(0.0);
			arm.Set(DoubleSolenoid::kReverse);
			Wait (1.0); // "Settle down"
			Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
			
			// Step 7: drop the arm for a clean shot and shoot
			arm.Set(DoubleSolenoid::kForward);
			
			Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
			
			// UNTESTED KICKED OFF FIELD
			Wait(1.0); // For arm to go down
			LaunchCatapult();
		}
		
		// Get us fully into the zone for 5 points
		Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);
		
		// SAFETY AND SANITY - SET ALL TO ZERO
		intake.Set(0.0);
		rightWinch.Set(0.0);
		leftWinch.Set(0.0);
	}