C++ Gyro::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();
	}

	RobotDemo()
	{
		//Initialize the objects for the drive train
		myRobot = new RobotDrive(1, 2);
		leftEnco = new Encoder(LEFT_ENCO_PORT_1, LEFT_ENCO_PORT_2);
		rightEnco = new Encoder(RIGHT_ENCO_PORT_1, RIGHT_ENCO_PORT_2);
		leftEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
		leftEnco->Start();
		rightEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
		rightEnco->Start();
		leftStick = new Joystick(1);
		rightStick = new Joystick(2);

		//Initialize the gyro
		gyro = new Gyro(GYRO_PORT);
		gyro->Reset();

		//Initialize the manipulators
		intake = new Intake(INTAKE_ROLLER_PORT, BALL_SENSOR_PORT, LEFT_SERVO_PORT, RIGHT_SERVO_PORT);
		catapult = new Catapult(LOADING_MOTOR_PORT, HOLDING_MOTOR_PORT, LOADED_LIMIT_1_PORT,
				LOADED_LIMIT_2_PORT, HOLDING_LIMIT_PORT);

		//Initialize the objects needed for camera tracking
		rpi = new RaspberryPi("17140");
		LEDLight = new Relay(1);
		LEDLight->Set(Relay::kForward);

		//Set the autonomous modes
		autonMode = ONE_BALL_AUTON;
		autonStep = AUTON_ONE_SHOOT;

		//Initialize the lcd
		lcd = DriverStationLCD::GetInstance();
	}

	void OperatorControl()
	{
		float xJoy, yJoy, gyroVal, angle = 0, turn = 0, angleDiff, turnPower;
		gyro.Reset();
		gyro.SetSensitivity(9.7);
		while (IsEnabled() && IsOperatorControl()) // loop as long as the robot is running
		{
			yJoy = xbox.getAxis(Xbox::L_STICK_V);
			xJoy = xbox.getAxis(Xbox::R_STICK_H);			
			gyroVal = gyro.GetAngle()/0.242*360;
			turn = 0.15;
			angle = angle - xJoy * xJoy * xJoy * turn;
			angleDiff = mod(angle - gyroVal, 360);
			turnPower = - mod(angleDiff / 180.0 + 1.0, 2) + 1.0;
			SmartDashboard::PutString("Joy1", vectorToString(xJoy, yJoy));
			SmartDashboard::PutNumber("Heading", mod(gyroVal, 360));
			SmartDashboard::PutNumber("Turn Power", turnPower);
			SmartDashboard::PutBoolean("Switch is ON:", dumperSwitch.Get());
			
			if (!xbox.isButtonPressed(Xbox::R))
			{
				drive.ArcadeDrive(yJoy * yJoy * yJoy, turnPower * fabs(turnPower), false);				
			}
		}
	}

	/**
	 * Initialization code for autonomous mode should go here.
	 * 
	 * Use this method for initialization code which will be called each time
	 * the robot enters autonomous mode.
	 */
	void RA14Robot::AutonomousInit() {
		Config::LoadFromFile("config.txt");
		auto_case = (int) Config::GetSetting("auto_case", 1);
		alreadyInitialized = true;
		auto_timer->Reset();
		auto_timer->Start();
		missionTimer->Start();
		myDrive->ResetOdometer();
		myCamera->Set(Relay::kForward);
		myCollection->ExtendArm();
		cout<<"Reseting Gyro"<<endl;
		gyro->Reset();
		//myOdometer->Reset();
		//myDrive->ShiftUp();
		myDrive->ShiftDown();
		//shift to high gear
		if (!fout.is_open()) {
			cout << "Opening logging.csv..." << endl;
			fout.open("logging.csv");
			logheaders();
		}
		auto_state = 0;
#ifndef DISABLE_SHOOTER
		myCam->Reset();
		myCam->Enable();
#endif //Ends DISABLE_SHOOTER
	}

	void TeleopInit()
	{
		leftEnc->Reset();
		rightEnc->Reset();
		gyro->Reset();

		powerCounter = 0;
	}

  virtual void SetUp() override {
    m_tilt = new Servo(TestBench::kCameraTiltChannel);
    m_pan = new Servo(TestBench::kCameraPanChannel);
    m_spiAccel = new ADXL345_SPI(SPI::kOnboardCS0);

    m_tilt->Set(kTiltSetpoint45);
    m_pan->SetAngle(0.0f);

    Wait(kServoResetTime);

    m_gyro->Reset();
  }

	void Test() {
		robotDrive.SetSafetyEnabled(false);
		uint8_t toSend[10];//array of bytes to send over I2C
		uint8_t toReceive[10];//array of bytes to receive over I2C
		uint8_t numToSend = 1;//number of bytes to send
		uint8_t numToReceive = 0;//number of bytes to receive
		toSend[0] = 7; //send 0 to arduino
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);

		bool isSettingUp = true;

		pickup.setGrabber(-1);
		pickup.setLifter(1);
		while (isSettingUp) {
			isSettingUp = false;
			if (grabOuterLimit.Get() == false) {
				pickup.setGrabber(0);
			}
			else {
				isSettingUp = true;
			}

			if (liftLowerLimit.Get()) {
				pickup.setLifter(0);
			}
			else {
				isSettingUp = true;
			}
		}
		gyro.Reset();
		liftEncoder.Reset();
		grabEncoder.Reset();

		toSend[0] = 8;
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);

		while(IsTest() && IsEnabled());

		toSend[0] = 0;
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
	}

	void RobotInit(void)
	{	
		//Pegs 1-6
		dseio.SetDigitalConfig(1,DriverStationEnhancedIO::kInputPullDown);
		dseio.SetDigitalConfig(2,DriverStationEnhancedIO::kInputPullDown);
		dseio.SetDigitalConfig(3,DriverStationEnhancedIO::kInputPullDown);
		dseio.SetDigitalConfig(4,DriverStationEnhancedIO::kInputPullDown);
		dseio.SetDigitalConfig(5,DriverStationEnhancedIO::kInputPullDown);
		dseio.SetDigitalConfig(6,DriverStationEnhancedIO::kInputPullDown);
		
		//pickup,retrieve,stow,stop
		dseio.SetDigitalConfig(7,DriverStationEnhancedIO::kInputPullDown);
		dseio.SetDigitalConfig(8,DriverStationEnhancedIO::kInputPullDown);
		dseio.SetDigitalConfig(9,DriverStationEnhancedIO::kInputPullDown);
		dseio.SetDigitalConfig(10,DriverStationEnhancedIO::kInputPullDown);
		
		LP1=false;
		LP2 =false;
		LP3=false;
		LP4 =false;
		LP5=false;
		LP6=false;
		LPStow=false;
		LPRetrieve=false;
		LPPickUp=false;
		LPStopArm=true;
		
		myarm->MoveClaw(-1);
		
		
		//start compressor
		robot_compressor->Start();
		
		mygyro->Reset();
	
	}

	/**
	 * Runs the motors with Mecanum drive.
	 */
	void OperatorControl()//teleop code
	{
		robotDrive.SetSafetyEnabled(false);
		gyro.Reset();
		grabEncoder.Reset();
		timer.Start();
		timer.Reset();
		double liftHeight = 0; //variable for lifting thread
		int liftHeightBoxes = 0; //another variable for lifting thread
		int liftStep = 0; //height of step in inches
		int liftRamp = 0; //height of ramp in inches
		double grabPower;
		bool backOut;
		uint8_t toSend[10];//array of bytes to send over I2C
		uint8_t toReceive[10];//array of bytes to receive over I2C
		uint8_t numToSend = 1;//number of bytes to send
		uint8_t numToReceive = 0;//number of bytes to receive
		toSend[0] = 1;//set the byte to send to 1
		i2c.Transaction(toSend, 1, toReceive, 0);//send over I2C
		bool isGrabbing = false;//whether or not grabbing thread is running
		bool isLifting = false;//whether or not lifting thread is running
		bool isBraking = false;//whether or not braking thread is running
		float driveX = 0;
		float driveY = 0;
		float driveZ = 0;
		float driveGyro = 0;
		bool liftLastState = false;
		bool liftState = false; //button pressed
		double liftLastTime = 0;
		double liftTime = 0;
		bool liftRan = true;
		Timer switchTimer;
		Timer grabTimer;
		switchTimer.Start();
		grabTimer.Start();


		while (IsOperatorControl() && IsEnabled())
		{
			// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
			// This sample does not use field-oriented drive, so the gyro input is set to zero.

			toSend[0] = 1;
			numToSend = 1;


			driveX = driveStick.GetRawAxis(Constants::driveXAxis);//starts driving code
			driveY = driveStick.GetRawAxis(Constants::driveYAxis);
			driveZ = driveStick.GetRawAxis(Constants::driveZAxis);
			driveGyro = gyro.GetAngle() + Constants::driveGyroTeleopOffset;


			if (driveStick.GetRawButton(Constants::driveOneAxisButton)) {//if X is greater than Y and Z, then it will only go in the direction of X
				toSend[0] = 6;
				numToSend = 1;

				if (fabs(driveX) > fabs(driveY) && fabs(driveX) > fabs(driveZ)) {
					driveY = 0;
					driveZ = 0;
				}
				else if (fabs(driveY) > fabs(driveX) && fabs(driveY) > fabs(driveZ)) {//if Y is greater than X and Z, then it will only go in the direction of Y
					driveX = 0;
					driveZ = 0;
				}
				else {//if Z is greater than X and Y, then it will only go in the direction of Z
					driveX = 0;
					driveY = 0;
				}
			}

			if (driveStick.GetRawButton(Constants::driveXYButton)) {//Z lock; only lets X an Y function
				toSend[0] = 7;
				driveZ = 0;//Stops Z while Z lock is pressed
			}

			if (!driveStick.GetRawButton(Constants::driveFieldLockButton)) {//robot moves based on the orientation of the field
				driveGyro = 0;//gyro stops while field lock is enabled
			}

			driveX = Constants::scaleJoysticks(driveX, Constants::driveXDeadZone, Constants::driveXMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveXDegree);
			driveY = Constants::scaleJoysticks(driveY, Constants::driveYDeadZone, Constants::driveYMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveYDegree);
			driveZ = Constants::scaleJoysticks(driveZ, Constants::driveZDeadZone, Constants::driveZMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveZDegree);
			robotDrive.MecanumDrive_Cartesian(driveX, driveY, driveZ, driveGyro);//makes the robot drive




			if (pdp.GetCurrent(Constants::grabPdpChannel) < Constants::grabManualCurrent) {
				pickup.setGrabber(Constants::scaleJoysticks(grabStick.GetX(), Constants::grabDeadZone, Constants::grabMax, Constants::grabDegree)); //defines the grabber
				if(grabTimer.Get() < 1) {
					toSend[0] = 6;
				}
			}
			else {
				pickup.setGrabber(0);
				grabTimer.Reset();
				toSend[0] = 6;
//.........这里部分代码省略.........

	void Autonomous()
	{
		Timer timer;
		float power = 0;
		bool isLifting = false;
		bool isGrabbing = false;
		double liftHeight = Constants::liftBoxHeight-Constants::liftBoxLip;
		double grabPower = Constants::grabAutoCurrent;
		bool backOut;

		uint8_t toSend[1];//array of bytes to send over I2C
		uint8_t toReceive[0];//array of bytes to receive over I2C
		uint8_t numToSend = 1;//number of bytes to send
		uint8_t numToReceive = 0;//number of bytes to receive
		toSend[0] = 2;//set the byte to send to 1
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);//send over I2C

		bool isSettingUp = true;

		//pickup.setGrabber(-1); //open grabber all the way
		pickup.setLifter(0.8);

		while (isSettingUp && IsEnabled() && IsAutonomous()) {
			isSettingUp = false;
			/*if (grabOuterLimit.Get() == false) {
				pickup.setGrabber(0); //open until limit
			}
			else {
				isSettingUp = true;
			}*/

			if (liftLowerLimit.Get()) {
				pickup.setLifter(0); //down till bottom
			}
			else {
				isSettingUp = true;
			}
		}

		gyro.Reset();
		liftEncoder.Reset();
		grabEncoder.Reset();

		if (grabStick.GetZ() > .8) {
			timer.Reset();
			timer.Start();
			while (timer.Get() < 1) {
				robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle());	// drive back
				if(power>-.4){
					power-=0.005;
					Wait(.005);
				}
			}
			robotDrive.MecanumDrive_Cartesian(0, 0, 0, gyro.GetAngle());	// STOP!!!
			timer.Stop();
			timer.Reset();
			Wait(1);
		}
		power = 0;

		while (isLifting && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		backOut = Constants::autoBackOut;
		pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);
		Wait(.005);

		while (isGrabbing && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		liftHeight = 3*Constants::liftBoxHeight;
		Wait(.005);
		pickup.lifterPosition(liftHeight, isLifting, grabStick);
		Wait(.005);
		while (isLifting && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12 < 2 && IsEnabled() && IsAutonomous());	// while the nearest object is closer than 2 feet

		timer.Start();

		while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches  < Constants::autoBackupDistance && timer.Get() < Constants::autoMaxDriveTime && IsEnabled() && IsAutonomous()) {	// while the nearest object is further than 12 feet
			if (power < .45) { //ramp up the power slowly
				power += .00375;
			}
			robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle());	// drive back
			float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12;	// distance from ultrasonic sensor
			SmartDashboard::PutNumber("Distance", distance);	// write stuff to smart dash
			SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
			SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
			SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
			SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
			SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle());
			SmartDashboard::PutNumber("Distance (in)", prox.GetVoltage() * Constants:: ultrasonicVoltageToInches);

			Wait(.005);
		}
//.........这里部分代码省略.........

/**
 * Reset the gyro.
 * Resets the gyro to a heading of zero. This can be used if there is significant
 * drift in the gyro and it needs to be recalibrated after it has been running.

 * @param slot The slot the analog module is connected to
 * @param channel The analog channel the gyro is plugged into
 */
void ResetGyro(UINT32 slot, UINT32 channel)
{
	Gyro *gyro = AllocateGyro(slot, channel);
	if (gyro) gyro->Reset();
}

	void Autonomous()
	{
		GetWatchdog().SetEnabled(false);
		Timer* hotGoalTimer = new Timer();
		Timer* reloadTimer = new Timer();
		Timer* intakeTimer = new Timer();
		Timer* intakeDropTimer = new Timer();
		bool goalFound = false;
		//bool rightSideHot = false;
		hotGoalTimer->Reset();
		hotGoalTimer->Start();
		gyro->Reset();
		leftEnco->Reset();
		rightEnco->Reset();
		LEDLight->Set(Relay::kForward);

		//Find out the type of autonomous we are using
		int autonType = (int)SmartDashboard::GetNumber(NUM_BALL_AUTO);
		if(autonType == 2)//Set the auton mode to two if we are doing a two ball auto
		{
			autonMode = TWO_BALL_AUTON;
			autonStep = AUTON_TWO_WAIT_FOR_INTAKE;
		}
		else//Set the auton to one if the value on SD is set to 1 or another random value
		{
			autonMode = ONE_BALL_AUTON;
			autonStep = AUTON_ONE_FIND_HOT;
		}

		//Bring the intake down
		intake->DropIntake();
		intakeDropTimer->Reset();
		intakeDropTimer->Start();

		while(IsAutonomous() && !IsDisabled())
		{
			rpi->Read();
			lcd->Printf(DriverStationLCD::kUser_Line1, 1, "L: %f", leftEnco->GetDistance());
			lcd->Printf(DriverStationLCD::kUser_Line2, 1, "R: %f", rightEnco->GetDistance());
			lcd->Printf(DriverStationLCD::kUser_Line3, 1, "T: %f", hotGoalTimer->Get());
			lcd->Printf(DriverStationLCD::kUser_Line4, 1, "i: %i", rpi->GetMissingPacketcount());
			lcd->Printf(DriverStationLCD::kUser_Line5, 1, "%i", rpi->GetXPos());
			lcd->Printf(DriverStationLCD::kUser_Line6, 1, "%i", rpi->GetYPos());
			lcd->UpdateLCD();
			LEDLight->Set(Relay::kForward);
			if(autonMode == ONE_BALL_AUTON)
			{
				switch(autonStep)
				{
				case AUTON_ONE_FIND_HOT:
					//Reload the catapult and find the hot goal
					rpi->Read();
					if(!goalFound)
					{
						//This is put into an if statement to protect against the 
						//rpi finding the hot goal and then losing it
						goalFound = ((rpi->GetXPos() != RPI_ERROR_VALUE) ||
								(rpi->GetYPos() != RPI_ERROR_VALUE));
					}
					//Wait for the goal to be hot and the intake to move down
					if(((goalFound) || (hotGoalTimer->Get() >= 6.75)) && intakeDropTimer->Get() >= INTAKE_DROP_WAIT)
					{
						autonStep = AUTON_ONE_SHOOT;
						catapult->StartRelease();
					}
					break;
				case AUTON_ONE_SHOOT:
					//Shoot the catapult
					if(!catapult->ReleaseHold())
					{
						//Move on to the next step when the catapult is released
						autonStep = AUTON_ONE_WAIT;
						//Start the wait timer
						reloadTimer->Reset();
						reloadTimer->Start();
					}
					break;
				case AUTON_ONE_WAIT:
					if(reloadTimer->Get() >= CATAPULT_WAIT_TIME)
					{
						autonStep = AUTON_ONE_DRIVE_FORWARDS;
						reloadTimer->Stop();
						//Start reloading the catapult
						catapult->StartLoad();
						gyro->Reset();
					}
					break;
				case AUTON_ONE_DRIVE_FORWARDS:
					//Drive forwards into the alliance zone and reload the catapult
					if((!GyroDrive(0, 0.75, 36)) && (!(bool)catapult->Load()))
					{
						autonStep = AUTON_END;
					}
					break;
				case AUTON_END:
					break;
				}
			}
			else if(autonMode == TWO_BALL_AUTON)
			{
//.........这里部分代码省略.........

	/****************************************
	 * Runs the motors with arcade steering.*
	 ****************************************/
	void OperatorControl(void)
	{
//TODO put in servo for lower camera--look in WPI to set	
//		Watchdog baddog;
		
	//	baddog.Feed();
		myRobot.SetSafetyEnabled(true);
		//SL Earth.Start(); // turns on Earth
//		SmartDashboard *smarty = SmartDashboard::GetInstance();
		//DriverStationLCD *dslcd = DriverStationLCD::GetInstance(); // don't press SHIFT 5 times; this line starts up driver station messages (in theory)
		//char debugout [100];
		compressor.Start();
		gyro.Reset(); // resets gyro angle
		int rpmForShooter;

		
		while (IsOperatorControl()) // while is the while loop for stuff; this while loop is for "while it is in Teleop"
		{ 
//			baddog.Feed();
			//myRobot.SetSafetyEnabled(true);
			//myRobot.SetExpiration(0.1);
			float leftYaxis = driver.GetY();
			float rightYaxis = driver.GetTwist();//RawAxis(5);
			myRobot.TankDrive(leftYaxis,rightYaxis); // drive with arcade style (use right stick)for joystick 1
			float random = gamecomponent.GetY();
			float lazysusan = gamecomponent.GetZ();
			//bool elevator = Frodo.Get();
			float angle = gyro.GetAngle();
			bool balance = Smeagol.Get();
			SmartDashboard::PutNumber("Gyro Value",angle);
			int NumFail = -1;
			//bool light = Pippin.Get();
			//SL float speed = Earth.GetRate();
			//float number = shooter.Get();
			//bool highspeed = button1.Get()
			//bool mediumspeed = button2.Get();
			//bool slowspeed = button3.Get();
			bool finder = autotarget.Get();
			//bool targetandspin = autodistanceandspin.Get();
			SmartDashboard::PutString("Targeting Activation","");
			//dslcd->Clear();
			//sprintf(debugout,"Number=%f",angle); 
			//dslcd->Printf(DriverStationLCD::kUser_Line2,2,debugout);
			//SL sprintf(debugout,"Number=%f",speed);
			//SL dslcd->Printf(DriverStationLCD::kUser_Line4,4,debugout);
			//sprintf(debugout,"Number=%f",number);
			//dslcd->Printf(DriverStationLCD::kUser_Line1,1,debugout);
			//sprintf(debugout,"Finder=%u",finder);
			//dslcd->Printf(DriverStationLCD::kUser_Line5,5,debugout);
			//dslcd->UpdateLCD(); // update the Driver Station with the information in the code
		    // sprintf(debugout,"Number=%u",maxi);
			// dslcd->Printf(DriverstationLCD::kUser_Line6,5,debugout)
						bool basketballpusher = julesverne.Get();
						bool bridgetipper = joystickbutton.Get();
						if (bridgetipper) // if joystick button 7 is pressed (is true)
						{	
							solenoid.Set(true); // then the first solenoid is on
						}
						
							else
							{
							//Wait(0.5); // and then the first solenoid waits for 0.5 seconds
							solenoid.Set(false); //and then the first solenoid turns off
						}
						if (basketballpusher) // if joystick button 6 is pressed (is true)
						{
							shepard.Set(true); // then shepard is on the run
							//Wait(0.5); // and shepard waits for 0.5 seconds
						}
							else
							{		
							shepard.Set(false); // and then shepard turns off
						} //10.19.67.9 IP address of computer;255.0.0.0 subnet mask ALL FOR WIRELESS CONNECTION #2			

			//}	
			//cheetah.Set(0.3*lazysusan);
//			smarty->PutDouble("pre-elevator",lynx.Get());
			lynx.Set(random);
//			smarty->PutDouble("elevator",lynx.Get());
			
//			smarty->PutDouble("joystick elevator",random);
			
			
			if (balance)						// this is the start of the balancing code
			{
				angle = gyro.GetAngle();
				myRobot.Drive(-0.03*angle, 0.0);
				Wait(0.005);
			}
			/*if (light)							//button 5 turns light on oand off on game controller
			     flashring.Set(Relay::kForward);
			     else
			            flashring.Set(Relay::kOff);
			 */           
			if (finder)
			{
				flashring.Set(Relay::kForward);
//.........这里部分代码省略.........

	/**
	 * Periodic code for autonomous mode should go here.
	 *
	 * Use this method for code which will be called periodically at a regular
	 * rate while the robot is in autonomous mode.
	 */
	void RA14Robot::AutonomousPeriodic() {
		StartOfCycleMaintenance();

		target->Parse(server->GetLatestPacket());
		float speed = Config::GetSetting("auto_speed", .5);
		cout<<"Auto Speed: "<<Config::GetSetting("auto_speed", 0)<<endl; // original .1
		float angle = gyro->GetAngle();
		float error = targetHeading - angle;
		float corrected = error * Config::GetSetting("auto_heading_p", .01);
		//float corrected = error * Config::GetSetting("auto_heading_p", .01);
		cout <<"Gyro angle: "<<angle<<endl;
		cout <<"Error: " << error << endl;
		//float lDrive = Config::GetSetting("auto_speed", -0.3) + (error * Config::GetSetting("auto_heading_p", .01));
		//float rDrive = Config::GetSetting("auto_speed", -0.3) - (error * Config::GetSetting("auto_heading_p", .01));
		// Reading p value from the config file does not appear to be working. When we get p from config, the math is not correct.
		float lDrive = Config::GetSetting("auto_speed", -0.3) + (error*0.01);
		float rDrive = Config::GetSetting("auto_speed", -0.3) - (error*0.01);
		cout << "Left: " << lDrive << endl;
		cout << "Right: " << rDrive << endl;
									
		
#ifndef DISABLE_AUTONOMOUS
		switch(auto_case)
		{
			case 0:
				// start master autonomous mode
				switch (auto_state) {
				case 0: // start
					auto_timer->Reset();
					auto_timer->Start();
					myCam->Process(false, false, false);
					break;
				case 1:
					myCam->Process(false, false, false);
					if (target->IsValid()) {
						auto_state = 2;
					} else if (auto_timer->Get() >= Config::GetSetting("auto_target_timeout", 1)) {
						auto_state = 10;
					}
					break;
				case 2:
					myCam->Process(false, false, false);
					if (target->IsHot()) {
						auto_state = 10;
					} else {
						if (auto_timer->Get() >= Config::GetSetting("auto_target_hot_timeout", 5)) {
							auto_state = 10;
						}
					}
					break;
				case 10:
					myCam->Process(true, false, false);
					if (myCam->IsReadyToRearm()) {
						auto_state = 11;
					}
					break;
				case 11:
					myCam->Process(false, false, false);
					myDrive->DriveArcade(corrected, speed);
					if (myDrive->GetOdometer() >= Config::GetSetting("auto_drive_distance", 100))
					{
						myDrive->Drive(0,0);
					}
					break;
				case 12:
					myDrive->Drive(0,0);
					break;
				default:
					cout << "Unknown state #" << auto_state << endl; 
					break;
				}
				// end master autonomous mode
				break;
			case 1:
				if( target->IsHot() && target->IsValid() )
				{
					cout << "Target is HOTTT taking the shot" << endl;
					//Drive forward and shoot right away
					//if( target->IsLeft() || target->IsRight() )
					//{
					if(myDrive->GetOdometer() <= 216 - Config::GetSetting("auto_firing_distance", 96)) //216 is distance from robot to goal
					{
						myDrive->Drive(corrected,speed);
					}
					else
					{
						myDrive->Drive(0,0);
						cout << "FIRING" << endl;
#ifndef DISABLE_SHOOTER
						myCam->Process(1,0,0);
#endif //Ends DISABLE_SHOOTER
					}
				/*}
				else
//.........这里部分代码省略.........