C++ Victor::Set方法代码示例

void RobotDemo::MecanumDrive(float x, float y, float rotation, float gyroAngle)
{
	const float EPSILONFL=1.f, EPSILONFR=1.f, EPSILONBL=1.f, EPSILONBR=1.f;
		
	double xIn = x;
	double yIn = y;
	// Negate y for the joystick.
	yIn = -yIn;
	// Compenstate for gyro angle.
	RotateVector(xIn, yIn, gyroAngle);

	double wheelSpeeds[kMaxNumberOfMotors];
	wheelSpeeds[0] = xIn + yIn + rotation;
	wheelSpeeds[1] = -xIn + yIn - rotation;
	wheelSpeeds[2] = -xIn + yIn + rotation;
	wheelSpeeds[3] = xIn + yIn - rotation;

	Normalize(wheelSpeeds);

	uint8_t syncGroup = 0x80;

	victorFL->Set(EPSILONFL*wheelSpeeds[0] * 1, syncGroup);
	victorFR->Set(EPSILONFR*wheelSpeeds[1] * -1, syncGroup);
	victorBL->Set(EPSILONBL*wheelSpeeds[2] * 1, syncGroup);
	victorBR->Set(EPSILONBR*wheelSpeeds[3] * -1, syncGroup);

	CANJaguar::UpdateSyncGroup(syncGroup);
}

void RobotDemo::dumb_climber_code()
{
	//printf("Top:%i   Bottom:%i  " , top_claw_limit_switch->Get() , bottom_claw_limit_switch->Get());
	if (drive_stick_prim ->GetRawButton(climber_hold_up))
	{
		if (top_claw_limit_switch->Get() == 1)
		{
			climbing_motor->Set(0.0);
			//printf("STOPPED\n");
		}
		else
		{
			climbing_motor->Set(1);
			//printf("GOING UP\n");
		}
	} // not climber hold up

	else if (drive_stick_prim->GetRawButton(climber_hold_down))
	{
		if (bottom_claw_limit_switch->Get() == 0)
		{
			climbing_motor->Set(0.0);
			//printf("STOPED\n");
		}
		else
		{
			climbing_motor->Set(-1);
			//printf("GOING DOWN\n");
		}
	} // hold down button
	else
	{ // no js buttons pushed
		climbing_motor->Set(0.0);
	}
}

	void TeleopPeriodic() {
		if(m_driver->GetRawButton(BUTTON_LB)) {
			// PYRAMID
			m_PIDController->SetSetpoint(PLATE_PYRAMID_THREE_POINT);
			m_PIDController->Enable();
		} else if(m_driver->GetRawButton(BUTTON_RB)) {
			// FEEDER
			m_PIDController->SetSetpoint(PLATE_FEEDER_THREE_POINT);
			m_PIDController->Enable();
		} else if(m_driver->GetRawAxis(TRIGGERS) > 0.5) {
			m_PIDController->SetSetpoint(PLATE_TEN_POINT_CLIMB);
			m_PIDController->Enable();
		} else {
			// MANUAL CONTROL
			m_PIDController->Disable();
			
			m_plate1->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
			m_plate2->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
		}
		
		// ----- PRINT -----
		SmartDashboard::PutNumber("Plate Position: ", m_plateSensor->GetVoltage());
		SmartDashboard::PutNumber("PID GET: ", m_plateSensor->PIDGet());
		
	} // TeleopPeriodic()

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		while (IsOperatorControl())
		{
			left.Set(cont.GetRawAxis(2));
			right.Set(cont.GetRawAxis(4));
		}
	}

 void AutonomousPeriodic(void)
 {
 	// Basic motor test code 
 	leftFrontDrive->Set(1);
 	leftRearDrive->Set(1);
 	
 	rightRearDrive->Set(1);
 	rightFrontDrive->Set(1);
 }

void RobotDemo::dumb_drive_code()
{
#if DUMB_DRIVE_CODE
	left_drive_motor_A->Set(-drive_stick_sec ->GetY());
	left_drive_motor_B->Set(-drive_stick_sec->GetY());
	right_drive_motor_A->Set(drive_stick_prim->GetY());
	right_drive_motor_B->Set(drive_stick_prim->GetY());
#endif
}

	/**
	 * This method is called every 20ms to update the robots components during autonomous.
	 * There should be no blocking calls in this method (connection requests, large data collection, etc),
	 * failing to comply with this could result in inconsistent robot behavior
	 */
	void AutonomousPeriodic()
	{
		//In the first two seconds of auto, drive forwardat half power
		if(Timer::GetFPGATimestamp() - autoTimer >= 0 && Timer::GetFPGATimestamp() < 2)
		{
			rd->SetLeftRightMotorOutputs(.5, .5);
		}
		else if(Timer::GetFPGATimestamp() - autoTimer >= 2 && Timer::GetFPGATimestamp() < 4)
		{
			//2 to 4 seconds, stop drivetrain and spin shooter wheels
			rd->SetLeftRightMotorOutputs(0, 0);
			shoot1->Set(1);
			shoot2->Set(1);
		}
		else if(Timer::GetFPGATimestamp() - autoTimer >= 4 && Timer::GetFPGATimestamp() < 8 && !kickerSwitch->Get())
		{
			//4 to 8 seconds and while kicker switch is not true, spin shooter wheels and kicker
			shoot1->Set(1);
			shoot2->Set(1);
			kicker->Set(-1);
		}
		else
		{
			//After all that, stop everything
			rd->SetLeftRightMotorOutputs(0, 0);
			shoot1->Set(0);
			shoot2->Set(0);
			kicker->Set(0);
		}
	}

 void Reset() {
   m_talonCounter->Reset();
   m_victorCounter->Reset();
   m_jaguarCounter->Reset();
   m_talon->Set(0.0f);
   m_victor->Set(0.0f);
   m_jaguar->Set(0.0f);
 }

	/**
	 * This method is called every 20ms to update the robots components during teleop.
	 * There should be no blocking calls in this method (connection requests, large data collection, etc),
	 * failing to comply with this could result in inconsistent robot behavior
	 */
	void TeleopPeriodic()
	{
		//Make the robot move like an arcade stick controlled device
		rd->ArcadeDrive(j1);

		//If button 1 on the joystick is pressed, spin the shooter wheels up,
		//otherwise stop them
		if(j1->GetRawButton(1))
		{
			shoot1->Set(1);
			shoot2->Set(2);
		}
		else
		{
			shoot1->Set(0);
			shoot2->Set(0);
		}

		//If button 2 on the joystick is pressed, spin the kicker, otherwise stop the kicker
		if(j1->GetRawButton(2))
		{
			kicker->Set(-1);
		}
		else
		{
			kicker->Set(0);
		}
	}

	void ShootModeSet()
	{
		if (controller.GetRawButton(BTN_MODE_RELOAD) == true && ShootMode == eShoot)
		{
			ShootMode = eReload;
			spinspeed = 0;
			shootertime.Stop();
			shootertime.Reset();
			shottime.Stop();
			shottime.Reset();
		}
		else if (controller.GetRawButton(BTN_MODE_SHOOT) == true && ShootMode == eReload)
		{
			ShootMode = eShoot;
			spinspeed = 1; // CHANGE BACK TO 1.  40% IS BUTTERZONE FOR CHILDREN
			shootertime.Start();
			shottime.Start();
		}
		spin1.Set(-spinspeed);
	}

	void TeleopPeriodic()
	{
		char myString[64];
		float   y;


// get the joystick position and filter the deadband
		y = -joystick->GetY();

		if (y > -0.1 && y < 0.1)
			y = 0;

		sprintf(myString, "joystick setting: %f\n", y);
		SmartDashboard::PutString("DB/String 1", myString);
		motor->Set(y, 0); // set the speed of the motor
		sprintf(myString, "gear count: %d\n", gearToothCounter->Get());
		SmartDashboard::PutString("DB/String 2", myString);
		sprintf(myString, "gear stopped: %d\n", gearToothCounter->GetStopped());
		SmartDashboard::PutString("DB/String 3", myString);

//		sprintf(myString, "In val: %d\n", toothInput->GetValue());
//		SmartDashboard::PutString("DB/String 2", myString);
	}

	void Disabled(void)
	{
		bool stopped = false;
		while(!IsOperatorControl() && !IsAutonomous())
		{
			if(!stopped)
			{
				frontLeftMotor->Set(0.0);
				rearLeftMotor->Set(0.0);
				frontRightMotor->Set(0.0);
				rearRightMotor->Set(0.0);
				liftMotor1->Set(0.0);
				liftMotor2->Set(0.0);
				gripMotor1->Set(0.0);
				//gripMotor2->Set(0.0);
				PIDLift->Reset();
				PIDDriveLeft->Reset();
				PIDDriveRight->Reset();
				PIDGrip->Reset();
				stopped = true;
			}
		}
	}

	void OperatorControl(void)
	{
		// Teleoperated Code.
		/*double WaitDash = 0.0;
		double FireDash = 0.0;
		double intPause = 0.0;
		
		SmartDashboard::PutNumber("P", 3.0);
		SmartDashboard::PutNumber("W", 0.0);
		SmartDashboard::PutNumber("A", 0.0);
		
		WaitDash = SmartDashboard::GetNumber("P");
		FireDash = SmartDashboard::GetNumber("W");
		intPause = SmartDashboard::GetNumber("A");
		
		SmartDashboard::PutNumber("P", WaitDash);
		SmartDashboard::PutNumber("W", FireDash);
		SmartDashboard::PutNumber("A", WaitDash);
		*/	
		// Enable and start the compressor.
		//compressor->Enabled();
		compressor->Start();

		// Enable drive motor safety timeout.
		myRobot.SetSafetyEnabled(true);

		// Enable watchdog and initial feed.
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(1);
		GetWatchdog().Feed();

		// Set robot in low gear by default. Not active.
		//s[0]->Set(false);
		GetWatchdog().Feed();

		//bool blnShoot = false;
		bool blnLowHang = false;
		bool blnShift = false;

		GetWatchdog().Feed();

		bool blnShooterSpd = false;
		bool blnReverse = false;

		float fltShoot;
		float fltSpeed = 1;

		int intFail = 0;
		
		timerLowHang.Reset();
		timerShift.Reset();
		timerFire.Reset();
		timerShooter.Reset();
		timerDriveCtrl.Reset();
		timerCamera.Reset();
		timerReverse.Reset();

		timerLowHang.Start();
		timerShift.Start();
		timerFire.Start();
		timerShooter.Start();
		timerDriveCtrl.Start();
		timerCamera.Start();
		timerReverse.Start();
		
		GetWatchdog().Feed();

		//sd->sendIOPortData();

		// Local variables.
		//float fltStick1X, fltStick1Y;

		while (IsOperatorControl())
		{
			if(timerReverse.Get() > 0.5)
			{
				if(stick1->GetRawButton(8) && blnReverse == false )
				{
					blnReverse = true;
					GetWatchdog().Feed();
					timerReverse.Reset();
					timerReverse.Start();
				}
				else if(stick1->GetRawButton(9) && blnReverse == true)
				{
					blnReverse = false;
					GetWatchdog().Feed();
					timerReverse.Reset();
					timerReverse.Start();
				}
			}
			if(blnReverse == false)
			{
				fltStick1Y = stick1->GetY();
				fltStick1X = stick1->GetX();
				SmartDashboard::PutBoolean("Reverse",false);
				GetWatchdog().Feed();
			}
			else if(blnReverse == true)
			{
//.........这里部分代码省略.........

void RobotDemo::climber_code()
{
	if (drive_stick_prim ->GetRawButton(climber_off_button))
	{
		climbing_motor-> Set(0.0);
		claw_go_down = false;
		claw_ = false;

	}
	else
	{
		if (drive_stick_prim ->GetRawButton(climber_hold_up))
		{
			claw_go_down = false;
			claw_ = false;
			if (top_claw_limit_switch->Get())
			{
				climbing_motor->Set(0.0);
			}
			else
			{
				climbing_motor->Set(1);
			}

		}// hold up
		else
		{
			if (drive_stick_prim ->GetRawButton(climber_hold_down))
			{
				claw_go_down = false;
				claw_ = false;
				if (bottom_claw_limit_switch->Get() == false)
				{
					climbing_motor ->Set(0.0);
				}
				else
				{
					climbing_motor ->Set(-1);
				}
			} // hold down
			else if (drive_stick_sec ->GetRawButton(climber_send_top))
			{
				claw_ = true;
				claw_go_down = false;
			}
			else if (drive_stick_sec ->GetRawButton(climber_send_bottom))
			{
				claw_go_down = true;
				claw_ = false;
			}
			else
			{ // no holding or sending
				if ((claw_ == false) && (claw_go_down == false))
				{
					climbing_motor ->Set(0.0);
				}
			}

		} // no climber hold up button

	} // no climber off button

	if (claw_go_down)
	{
		if (bottom_claw_limit_switch->Get() == 1)
		{
			climbing_motor ->Set(-1);
		}
		else
		{
			climbing_motor ->Set(0.0);
		}

	}
	if (claw_)
	{
		if (top_claw_limit_switch->Get() == 0)
		{
			climbing_motor->Set(1);
		}
		else
		{
			climbing_motor ->Set(0.0);
		}
	}
}

	void Autonomous(void)
	{
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(1);
		GetWatchdog().Feed();
		s[0]->Set(true);
		s[1]->Set(true);
		SmartDashboard::PutString("Gear","High");
		compressor->Start();
		myRobot.SetSafetyEnabled(true);


		int WaitDash = 0;
		int FireDash = 0;
		int intPause = 0;
		GetWatchdog().Feed();

		// Incase we need to recreate the controls for Autonomous.
		//SmartDashboard::PutNumber("fire_wait", 3);
		//SmartDashboard::PutNumber("fire_amount", 3);
		//SmartDashboard::PutNumber("fire_pause", 5);

		WaitDash = static_cast<int>(SmartDashboard::GetNumber("fire_wait"));
		FireDash = static_cast<int>(SmartDashboard::GetNumber("fire_amount"));
		intPause = static_cast<int>(SmartDashboard::GetNumber("fire_pause"));
		GetWatchdog().Feed();


		/* CAMERA THRESHOLD AND DECLARATIONS, UNCOMMENT IF CAMERA READDED TO AUTONOMOUS
		//Their threshold values suck DDDD, from the NIvision assistant will be below.
		//Threshold threshold(60, 100, 90, 255, 20, 255);	//HSV threshold criteria, ranges are in that order ie. Hue is 60-100
		//Threshold threshold(100, 255, 230, 255, 140, 255); //New threshold values
		Threshold threshold(0, 255, 0, 255, 221, 255);
		
		ParticleFilterCriteria2 criteria[] = {
				{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}
		};												//Particle filter criteria, used to filter out small particles
		// AxisCamera &camera = AxisCamera::GetInstance();	//To use the Axis camera uncomment this line
				
		*/
		while (IsAutonomous() && IsEnabled())
		{

			myShooter1.Set(-1);
			myShooter2.Set(-1);
			GetWatchdog().Feed();


			GetWatchdog().Feed();

			// Wait before firing.
			for(intAutoCtrl = 0; intAutoCtrl < (1);intAutoCtrl++)
			{
				GetWatchdog().Feed();
				for(intDelay = 0; intDelay< (((intPause)*2)+1);intDelay++)
				{
					GetWatchdog().Feed();
					Wait(0.5);
				}
				GetWatchdog().Feed();


				// Fire a number of frisbees as set in the dashboard.
				//for(intCount = 0; intCount < (FireDash+1); intCount++)
			//	{
					GetWatchdog().Feed();
					s[2]->Set(true);

					// Delay between firing each frisbees.
					for(int wait = 0; wait<((WaitDash)*2);wait++)
					{
						GetWatchdog().Feed();
						Wait(0.5);
					}
					s[2]->Set(false);
					GetWatchdog().Feed();
					for(intSecondWait = 0;intSecondWait < 3;intSecondWait++)
					{
						GetWatchdog().Feed();
						Wait(0.5);
					}
					GetWatchdog().Feed();
				}
				GetWatchdog().Feed();
			}

			myShooter1.Set(0);
			myShooter2.Set(0);
			GetWatchdog().Feed();
		//}

	}