C++ SendableChooser::GetSelected方法代码示例

	void AutonomousInit(){
		SmartDashboard::PutString("AutonomousInit", "Autonomous Init Reached");
		autonomous->setSelected(*((std::string*)chooser->GetSelected()));
		//autoSelected = SmartDashboard::GetString("Auto Selector", *((std::string*)chooser->GetSelected()));
		autoSelected = *((std::string*)chooser->GetSelected());
		SmartDashboard::PutString("Auto Selected", autoSelected);
		//testSelected= SmartDashboard::GetString("Auto Selector", *((std::string*)chooser2->GetSelected()));
		testSelected = *((std::string*)chooser2->GetSelected());

		Drive->AutonomousInit();
		autonomous->startTimer();
		autonomous->reset();


	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void AutonomousInit()
	{
		autoSelected = *((std::string*)chooser->GetSelected());
		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;

		rotation = 0.0;
				//*((double*)posChooser->GetSelected());

		//goal = *((std::string*)goalChooser->GetSelected());
		shoot = "No";
		//*((std::string*)shootChooser->GetSelected());

		defenseCrossed = false;
		done = false;


		std::cout << "Here" << std::endl;
		drive->SetMaxOutput(1.0);
		std::cout << "there" << std::endl;
		//Make sure to reset the encoder!
		leftEnc->Reset();
		rightEnc->Reset();
		gyro->Reset();
		autoCounter = 0;
		timer->Reset();
	}

	virtual void TeleopInit() {
		// This makes sure that the autonomous stops running when
		// teleop starts running. If you want the autonomous to 
		// continue until interrupted by another command, remove
		// this line or comment it out.
		if (_autonomousCommand)
		{
			_autonomousCommand->Cancel();
		}
		
		clearLCD();
		
		_teleopCommand = (Command*)_teleopChooser->GetSelected();
//		_teleopCommand = (Command*)_noOpCmd;

		printCommandToLCD(_teleopCommand->GetName());
		
//		if (! CommandBase::azimuthSubsystem->IsCalibrated())
//		{
//			CommandBase::azimuthSubsystem->CalibrateAzimuth();
//		}
		_teleopCommand->Start();
//		_testRobotCmd->Start();
//		_cmd = new TestBridgeArmCmd(CommandBase::oi->driveTrigger);
//		_cmd->Start();
	}

 virtual void TeleopInit() {        
     CommandBase::turret->Reset();
     CommandBase::turret->Start();
     
     driveCommand = (Command*) driveStyle->GetSelected();
     driveCommand->Start();
 }

	void Autonomous(){
		Option *num = (Option *) chooser->GetSelected();
		myRobot->ResetDisplacement();
		Modes->SetMode(num->Get());
		Modes->Run();
		while(IsAutonomous() && IsEnabled()){
			Wait(0.05);
			Scheduler::GetInstance()->Run();
		}
	}

	// Start auto mode
	void AutonomousInit() override {
		autoSelected = *((int*) chooser.GetSelected()); // autonomous mode chosen in dashboard
		currentState = 1;
		ahrs->ZeroYaw();
		ahrs->GetFusedHeading();
		autoLength = SmartDashboard::GetNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
		autoSpeed = SmartDashboard::GetNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
		autoIntakeSpeed = SmartDashboard::GetNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
		liftdown->Set(false);
	}

	void TeleopPeriodic()
	{
		double throttle;

		bool BuddyBoxEnabled = BuddyBoxEnableChooser->GetSelected();
		bool SlaveInControl = MasterInterLink->GetCh5();
		SmartDashboard::PutBoolean( "Slave In Control", BuddyBoxEnabled ? SlaveInControl : false );
		bool SlaveControlsSpeed = SlaveSpeedControlChooser->GetSelected();

		if( BuddyBoxEnabled && SlaveInControl ) ActiveInterLink = SlaveInterLink;
		else ActiveInterLink = MasterInterLink;

		if( SlaveInControl && SlaveControlsSpeed ) throttle = SlaveInterLink->getCh6();
		else throttle = MasterInterLink->getCh6();

		double aile = ActiveInterLink->getAile();
		double elev = ActiveInterLink->getElev();
		double rudd = ActiveInterLink->getRudd();
		SmartDashboard::PutNumber( "Rudder", rudd );
		SmartDashboard::PutNumber( "Throttle", throttle );

		throScale = throttle + 1;

		double driveAngle = atan2( -aile*aileScale, elev*elevScale );
		SmartDashboard::PutNumber( "Drive Angle", driveAngle );

		double driveSpeed = sqrt( aile*aile + elev*elev );
		SmartDashboard::PutNumber( "Drive Speed", driveSpeed );

		frontLeft->Set( (float)(
				throScale * frontLeftSpeed * ( driveSpeed * sin( frontLeftAngle-driveAngle ) + ruddScale * rudd )
				) );
		backLeft->Set( (float)(
				throScale * backLeftSpeed * ( driveSpeed * sin( backLeftAngle-driveAngle ) + ruddScale * rudd )
				) );
		frontRight->Set( (float)(
				throScale * frontRightSpeed * ( driveSpeed * sin( frontRightAngle-driveAngle ) + ruddScale * rudd )
				) );
		backRight->Set( (float)(
				throScale * backRightSpeed * ( driveSpeed * sin( backRightAngle-driveAngle ) + ruddScale * rudd )
				) );
	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void AutonomousInit()
	{
		autoSelected = *((std::string*)chooser->GetSelected());
		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;

		if(autoSelected == autoNameCustom){
			//Custom Auto goes here
		} else {
			//Default Auto goes here
		}
	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString code to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional commands to the chooser code above (like the commented example)
	 * or additional comparisons to the if-else structure below with additional strings & commands.
	 */
	void AutonomousInit()
	{
		/* std::string autoSelected = SmartDashboard::GetString("Auto Selector", "Default");
		if(autoSelected == "My Auto") {
			autonomousCommand.reset(new MyAutoCommand());
		} else {
			autonomousCommand.reset(new ExampleCommand());
		} */

		autonomousCommand = (Command *)chooser->GetSelected();

		if (autonomousCommand != NULL)
			autonomousCommand->Start();
	}

	virtual void AutonomousInit() 
	{
		if (_teleopCommand)
		{
			_teleopCommand->Cancel();
		}

		clearLCD();

		_autonomousCommand = (Command*)_autoChooser->GetSelected();
//		_autonomousCommand = (Command*) _autoFireCmd;

		printCommandToLCD(_autonomousCommand->GetName());

		_autonomousCommand->Start();
	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *s
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void AutonomousInit() {
		autoSelected = *((std::string*) chooser->GetSelected());
		std::string autoSelected = SmartDashboard::GetString("Auto Selector",
				autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;


		//myRobot.SetSafetyEnabled(false);
		//myRobot.Drive(-0.5, 0.0);

		if (autoSelected == autoNameCustom) {
			//Custom Auto goes here
			//autoSpeed = 1;
			//Wait(2);
			//autoSpeed = 0;
		} else {
			//Default Auto goes here
		}
	}

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void Autonomous()
	{
		std::string autoSelected = *((std::string*)chooser->GetSelected());
		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;

		if(autoSelected == autoNameCustom){
			//Custom Auto goes here
			std::cout << "Running custom Autonomous" << std::endl;
			myRobot.SetSafetyEnabled(false);
			myRobot.Drive(-0.5, 1.0); 	// spin at half speed
			Wait(2.0); 				//    for 2 seconds
			myRobot.Drive(0.0, 0.0); 	// stop robot
		} else {
			//Default Auto goes here
			std::cout << "Running default Autonomous" << std::endl;
			myRobot.SetSafetyEnabled(false);
			myRobot.Drive(-0.5, 0.0); 	// drive forwards half speed
			Wait(2.0); 				//    for 2 seconds
			myRobot.Drive(0.0, 0.0); 	// stop robot
		}

	}

	/*Initializes objects necessary for teleop
	 * Adds options to sendable chooser on SmartDashboard
	 */
	void RobotInit(){
		/*if (fork()==0){
			system("/home/lvuser/start_vision &");
		}*/

		//creates DriveTrain and sendable choosers
		Drive= new DriveTrain();
		chooser = new SendableChooser();
		chooser2 = new SendableChooser();


		//adds options to both
		chooser->AddDefault(DoNothing, (void*)&DoNothing);
		chooser->AddObject(autoNameLeft, (void*)&autoNameLeft);
		chooser->AddObject(autoNameRight, (void*)&autoNameRight);
		chooser->AddObject(autoNameOver, (void*)&autoNameOver);
		chooser->AddObject(autoNameP1, (void*)&autoNameP1);
		chooser->AddObject(autoNameP2, (void*)&autoNameP2);
		chooser->AddObject(autoNameP3, (void*)&autoNameP3);
		chooser->AddObject(autoNameP4, (void*)&autoNameP4);
		chooser->AddObject(autoNameP5, (void*)&autoNameP5);
		chooser->AddObject(autoNameRW, (void*)&autoNameRW);
		SmartDashboard::PutData("Auto Modes", chooser);


		chooser2->AddDefault(autoNameTest1, (void*)&autoNameTest1);
		chooser2->AddObject(autoNameTest2, (void*)&autoNameTest2);
		chooser2->AddObject(autoNameTest3, (void*)&autoNameTest3);
		chooser2->AddObject(autoNameTest4, (void*)&autoNameTest4);
		SmartDashboard::PutData("Auto Modes 2", chooser2);
		Drive->AutonomousInit();
		autonomous = new Autonomous(Drive, *((std::string*)chooser->GetSelected()));

		//passes driveTrain and sendable chooser option to Autonomous class and creates it!

	}

		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)) {
//.........这里部分代码省略.........

	void AutonomousInit()
	{
		autonomousCommand = (Command *) chooser->GetSelected(); //Sends which autonomous was chosen
		autonomousCommand->Start();
	}