C++ PIDController::SetSetpoint方法代码示例

	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 Mecanum drive.
  */
 void OperatorControl()
 {
     robotDrive.SetSafetyEnabled(false);
     while (IsOperatorControl() && IsEnabled())
     {
         bool reset_yaw_button_pressed = stick.GetRawButton(1);
         if ( reset_yaw_button_pressed ) {
             ahrs->ZeroYaw();
         }
         bool rotateToAngle = false;
         if ( stick.GetRawButton(2)) {
             turnController->SetSetpoint(0.0f);
             rotateToAngle = true;
         } else if ( stick.GetRawButton(3)) {
             turnController->SetSetpoint(90.0f);
             rotateToAngle = true;
         } else if ( stick.GetRawButton(4)) {
             turnController->SetSetpoint(179.9f);
             rotateToAngle = true;
         } else if ( stick.GetRawButton(5)) {
             turnController->SetSetpoint(-90.0f);
             rotateToAngle = true;
         }
         double currentRotationRate;
         if ( rotateToAngle ) {
             turnController->Enable();
             currentRotationRate = rotateToAngleRate;
         } else {
             turnController->Disable();
             currentRotationRate = stick.GetTwist();
         }
         try {
             /* Use the joystick X axis for lateral movement,          */
             /* Y axis for forward movement, and the current           */
             /* calculated rotation rate (or joystick Z axis),         */
             /* depending upon whether "rotate to angle" is active.    */
             robotDrive.MecanumDrive_Cartesian(stick.GetX(), stick.GetY(),
                                               currentRotationRate ,ahrs->GetAngle());
         } catch (std::exception ex ) {
             std::string err_string = "Error communicating with Drive System:  ";
             err_string += ex.what();
             DriverStation::ReportError(err_string.c_str());
         }
         Wait(0.005); // wait 5ms to avoid hogging CPU cycles
     }
 }

	/**
	 *  Uses a PIDController and an array of setpoints to switch and maintain elevator positions.
     *  The elevator setpoint is selected by a joystick button.
	 */
	void OperatorControl() {
		pidController->SetInputRange(0, 5); //0 to 5V
		pidController->SetSetpoint(setPoints[0]); //set to first setpoint

		int index = 0;
		bool currentValue;
		bool previousValue = false;

		while (IsOperatorControl() && IsEnabled()) {
			pidController->Enable(); //begin PID control

			//when the button is pressed once, the selected elevator setpoint is incremented
			currentValue = joystick->GetRawButton(buttonNumber);
			if (currentValue && !previousValue) {
				pidController->SetSetpoint(setPoints[index]);
				index = (index + 1) % (sizeof(setPoints)/8); //index of elevator setpoint wraps around
			}
			previousValue = currentValue;
		}
	}

	void AutonomousGyroTurn() {
		switch (currentState) {
		case 1:

			timer->Reset();
			timer->Start();
			//State: Stopped
			//Transition: Driving State
			currentState = 2;
			break;
		case 2:
			//State: Driving
			//Stay in State until 2 seconds have passed--`
			//Transition: Gyroturn State
			drive->TankDrive(0.5, 0.5);
			if (timer->Get() >= 1) {
				drive->TankDrive(0.0, 0.0);
				ahrs->ZeroYaw();
				currentState = 3;
				turnController->SetSetpoint(90);
				turnController->Enable();
			}
			break;
		case 3:
			//State: Gyroturn
			//Stay in state until navx yaw has reached 90 degrees
			//Transition: Driving State
			drive->TankDrive(0.5 * rotateRate, -0.5 * rotateRate);
//			if (ahrs->GetYaw() >= 90) {
			if (turnController->OnTarget()) {
				drive->TankDrive(0.0, 0.0);
				currentState = 4;
				timer->Reset();
				timer->Start();
			}
			break;
		case 4:
			//State:Driving
			//Stay in state until 2 seconds have passed
			//Transition: State Stopped
			drive->TankDrive(0.5, 0.5);
			if (timer->Get() >= 1) {
				currentState = 5;
				timer->Stop();
			}
			break;
		case 5:
			//State: Stopped
			drive->TankDrive(0.0, 0.0);
			break;

		}

	}

    void SetMotorSpeedRight(float speed) {
    	char myString[64];

        if (pidDrive) {
            if (fabs(speed) < 0.5) {
                speed = 0;
            }
    		sprintf(myString, "pid SP R: %5.2f\n", speed);
    		SmartDashboard::PutString("DB/String 9", myString);
            controlRight->SetSetpoint(speed);
        } else {
    		sprintf(myString, "setpoint R: %5.2f\n", speed);
    		SmartDashboard::PutString("DB/String 9", myString);
            m_pscMotorRight->Set(speed);
        }
    }

    void SetMotorSpeedLeft(float speed) {
    	char myString[64];


        if (pidDrive) {
            if (fabs(speed) < 0.5) {
                speed = 0;
            }
    		sprintf(myString, "pid SP L: %5.2f\n", -speed);
    		SmartDashboard::PutString("DB/String 8", myString);
            controlLeft->SetSetpoint(-speed);
        } else {
    		sprintf(myString, "setpoint L: %5.2f\n", -speed);
    		SmartDashboard::PutString("DB/String 8", myString);
            m_pscMotorLeft->Set(-speed);
        }
    }

	void AutonomousAdjustableStraight() {
		switch (currentState) {
		case 1:
			timer->Reset();
			timer->Start();
			turnController->Reset();
			turnController->SetSetpoint(ahrs->GetYaw());
			turnController->Enable();
			currentState = 2;
			break;
		case 2:
			intakeLever->Set(autoIntakeSpeed);
			if (timer->Get() >= 1) {
				intakeLever->Set(0);
				currentState = 3;
				timer->Reset();
				timer->Start();
			}
			break;
		case 3:
			drive->TankDrive(autoSpeed, autoSpeed);
			intakeLever->Set(-0.1);
			if (timer->Get() >= autoLength) {
				intakeLever->Set(0.0);
				drive->TankDrive(0.0, 0.0);
				currentState = 4;
				timer->Reset();
				timer->Start();
			}
			break;
		case 4:
			intake->Set(0.5);
			shooter->Set(-0.5);
			if (timer->Get() >= 2) {
				currentState = 5;
			}
			break;
		case 5:
			intake->Set(0.0);
			shooter->Set(0.0);
			drive->TankDrive(0.0, 0.0);
			break;
		}
	}

	void stateAiming() {
		stateTimer++;
		if (stateTimer == 1) {
			// calculate launcher angle
			double angle = calcLaunchAngle(m_targets[0].block.y);
			LOGGER(INFO) << "[stateAiming] Setting Launch Angle: " << angle;

			if (angle > 46.0) {
				LOGGER(ERROR) << "[stateAiming] Target is out of range";
				robotState = kOperatorControl;
				return;
			}

			launchController->SetSetpoint(angle);
			launchController->Enable();
			return;
		} else if (stateTimer < 5) {
			return;
		} else if (stateTimer < 150) {
			LOGGER(DEBUG) << "[stateAiming] Timer: " << stateTimer << " SetPoint: " << launchController->GetSetpoint()
					      << " Angle: " << launchPIDSource.PIDGet() << " Correction: " << launchPIDOutput.correction;
			elevator->Set(-launchPIDOutput.correction);

			if ((fabs(launchPIDOutput.correction) < 0.2) &&
				(fabs(launchPIDSource.PIDGet()-launchController->GetSetpoint()) < 0.25)) {
				elevator->Set(0.0);
				launchController->Disable();
				robotState = kLaunching;
				stateTimer = 0;

				LOGGER(DEBUG) << "[stateAiming] Target x: " << m_targets[0].block.x << " y: " << m_targets[0].block.y
						      << " h: " << m_targets[0].block.height << " w: " << m_targets[0].block.width;
			}
		} else {
			launchController->Disable();
			elevator->Set(0.0);
			robotState = kOperatorControl;
			LOGGER(ERROR) << "[stateAiming] Aiming Failed, Timer: " << stateTimer << " Correction: "
					      << launchPIDOutput.correction;
		}
	}

	void AutonomousStraightSpy() {
		switch (currentState) {
		case 1:
			timer->Reset();
			timer->Start();
			turnController->Reset();
			turnController->SetSetpoint(ahrs->GetYaw());
			turnController->Enable();
			currentState = 2;
			break;
		case 2:
			intakeLever->Set(0.25);
			if (timer->Get() >= 1) {
				intakeLever->Set(0);
				currentState = 3;
				timer->Reset();
				timer->Start();
			}
			break;
		case 3:
			drive->TankDrive(0.5, 0.5);
			if (timer->Get() >= 5) {
				drive->TankDrive(0.0, 0.0);
				currentState = 4;
				timer->Reset();
				timer->Start();
			}
			break;
		case 4:
			intake->Set(0.5);
			if (timer->Get() >= 2) {
				currentState = 5;
			}
			break;
		case 5:
			intake->Set(0.0);
			drive->TankDrive(0.0, 0.0);
			break;
		}
	}

	void stateCentering() {
		stateTimer++;

		if (stateTimer == 1) {
			turnController->SetSetpoint(0.0);
			turnController->Enable();
			return;
		} else if (stateTimer < 10) {
			return;
	    } else if (stateTimer < 120) {
			if (!turnPIDSource->acquired()) {
				robotState = kOperatorControl;
				turnController->Disable();
				turnPIDSource->reset();
				LOGGER(ERROR) << "[stateCentering] No Target Found";
				return;
			}
			drive->ArcadeDrive(0.0, -turnPIDOutput.correction, false);
			LOGGER(DEBUG) << "[stateCentering] Timer: " << stateTimer << " SetPoint: " << turnController->GetSetpoint()
					      << " Offset: " << turnPIDSource->PIDGet() << " Correction: " << turnPIDOutput.correction;

			if ((fabs(turnPIDOutput.correction) < 0.10) && (fabs(turnPIDSource->PIDGet()) < 3)) {
				drive->ArcadeDrive(0.0, 0.0, false);
				turnController->Disable();
				robotState = kAiming;
				stateTimer = 0;
				turnPIDSource->reset();
			}
		} else {
			turnController->Disable();
			turnPIDSource->reset();
			drive->ArcadeDrive(0.0, 0.0, false);
			robotState = kOperatorControl;
			LOGGER(ERROR) << "[stateCentering] FAILED, Timer: " << stateTimer << " SetPoint: " << turnController->GetSetpoint()
				          << " Offset: " << turnPIDSource->PIDGet() << " Correction: " << turnPIDOutput.correction;
		}
	}

	void stateOperatorControl() {
		// DRIVING
		move = stick->GetRawAxis(1) * -1.0;
		rotate = stick->GetRawAxis(4) * -1.0;

		// Deadband
		if (fabs(move) < 0.1) {
			move = 0.0;
		}
		if (fabs(rotate) < 0.15) {
			rotate = 0.0;
		}
		drive->ArcadeDrive(move, rotate, false);

		// Joystick Buttons
		bool button4 = stick->GetRawButton(4);
		bool button1 = stick->GetRawButton(1);
		bool button5 = stick->GetRawButton(5);
		bool button6 = stick->GetRawButton(6);
		bool button3 = stick->GetRawButton(3);

		// Manual Gatherer
		if (stick->GetRawAxis(2) != 0) {
			gatherSpeed = stick->GetRawAxis(2);
			LOGGER(DEBUG) << "[stateOperatorControl] Gather Angle:" << gatherPIDSource.PIDGet();

		}
		else if (stick->GetRawAxis(3) != 0) {
			gatherSpeed = stick->GetRawAxis(3) * -1;
			LOGGER(DEBUG) << "[stateOperatorControl] Gather Angle:" << gatherPIDSource.PIDGet();
		}
		else {
			gatherSpeed = 0.0;
		}
		gatherer->Set(gatherSpeed);

		// Launch Angle
		double launcherAngle = launchPIDSource.PIDGet();
		if (button5 && !button6  && (launcherAngle < kLaunchMaxAngle)) {
			elevator->Set(-0.5); // Up
			LOGGER(DEBUG) << "[stateOperatorControl] Launcher Angle:" << launcherAngle;
		} else if (button6 && !button5 && (launcherAngle > kLaunchMinAngle)) {
			LOGGER(DEBUG) << "[stateOperatorControl] Launcher Angle:" << launcherAngle;
			elevator->Set(0.5); // Down
		} else {
			elevator->Set(0.0);
		}

		// Auto-Gather
		if (button3 && !lastButton3) {
			wheelsGathererIn = !wheelsGathererIn;
			gatherController->SetSetpoint(kGatherAngle);
			gatherController->Enable();
		}
		if (wheelsGathererIn) {
			gathererWheels->Set(1.0);
			gatherer->Set(gatherPIDOutput.correction);
			LOGGER(DEBUG) << "[stateOperatorControl] Gather Correction:" << gatherPIDOutput.correction
					      << " Gather Angle: " << gatherPIDSource.PIDGet();
		} else {
			gathererWheels->Set(0.0);
			gatherController->Disable();
		}

		if (button4 && !lastButton4) {
			stateTimer   = 0;
			robotState   = kCentering;
			shootingHigh = true;
		}
		if (button1 && !lastButton1) {
			stateTimer   = 0;
			robotState   = kLaunching;
			shootingHigh = true;
		}
		lastButton4 = button4;
		lastButton1 = button1;
		lastButton3 = button3;
	}

	void OperatorControl(void)
	{
		autonomous = false;
		//myRobot.SetSafetyEnabled(false);
		//myRobot.SetInvertedMotor(kFrontLeftMotor, true);
		//	myRobot.SetInvertedMotor(3, true);
		//variables for great pid
		double rightSpeed,leftSpeed;
		float rightSP, leftSP, liftSP, lastLiftSP, gripSP, tempLeftSP, tempRightSP;
		float stickY[2];
		float stickYAbs[2];
		bool lightOn = false;
		AxisCamera &camera = AxisCamera::GetInstance();
		camera.WriteResolution(AxisCameraParams::kResolution_160x120);
		camera.WriteMaxFPS(5);
		camera.WriteBrightness(50);
		camera.WriteRotation(AxisCameraParams::kRotation_0);
		rightEncoder->Start();
		leftEncoder->Start();
		liftEncoder->Start();
		rightEncoder->Reset();
		leftEncoder->Reset();
		liftEncoder->Reset();
		bool fastest = false; //transmission mode
		float reduction = 1; //gear reduction from 
		bool bDrivePID = false;
		//float maxSpeed = 500;
		float liftPower = 0;
		float liftPos = -10;
		bool disengageBrake = false;
		int count = 0;
		//int popCount = 0;
		double popStart = 0;
		double popTime = 0;
		int popStage = 0;
		int liftCount = 0;
		int liftCount2 = 0;
		const float LOG17 = log(17.61093344);
		float liftPowerAbs = 0;
		float gripError = 0;
		float gripErrorAbs = 0;
		float gripPropMod = 0;
		float gripIntMod = 0;
		bool shiftHigh = false;
		leftEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //6-inch wheels, 1000 raw counts per revolution,
		rightEncoder->SetDistancePerPulse((6 * PI / 1000)/reduction); //about 1:1 gear ratio
		DriverStationEnhancedIO &myEIO = driverStation->GetEnhancedIO();
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(0.3);

		PIDDriveLeft->SetOutputRange(-1, 1);
		PIDDriveRight->SetOutputRange(-1, 1);
		//PIDDriveLeft->SetInputRange(-244,244);
		//PIDDriveRight->SetInputRange(-244,244);
		PIDDriveLeft->SetTolerance(5);
		PIDDriveRight->SetTolerance(5);
		PIDDriveLeft->SetContinuous(false);
		PIDDriveRight->SetContinuous(false);
		//PIDDriveLeft->Enable();
		//PIDDriveRight->Enable();
		PIDDriveRight->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN);
		PIDDriveLeft->SetPID(DRIVEPROPGAIN, DRIVEINTGAIN, DRIVEDERIVGAIN);
		
		liftSP = 0;
		PIDLift->SetTolerance(10);
		PIDLift->SetContinuous(false);
		PIDLift->SetOutputRange(-0.75, 1.); //BUGBUG
		PIDLift->Enable();
		
		gripSP = 0;
		float goalGripSP = 0;
		bool useGoalSP = true;
		bool gripPIDOn = true;
		float gripP[10];
		float gripI[10];
		float gripD[10];
		PIDGrip->SetOutputRange(-0.5, 0.28); //Negative goes up
		PIDGrip->SetTolerance(5);
		PIDGrip->SetSetpoint(0);
		PIDGrip->Enable();
		miniDep->Set(miniDep->kForward);
		int i = 0;
		while(i < 10)
		{
			gripP[i] = GRIPPROPGAIN;
			i++;
		}
		i = 0;
		while(i < 10)
		{
			gripI[i] = GRIPINTGAIN;
			i++;
		}
		i = 0;
		while(i < 10)
		{
			gripD[i] = GRIPDERIVGAIN;
			i++;
		}

//.........这里部分代码省略.........

	void Autonomous(void)
	{
#if 1
		/*int autoMode = 0;
		autoMode |= bcd1->Get();
		autoMode <<= 1;
		autoMode |= bcd2->Get();
		autoMode <<= 1;
		autoMode |= bcd3->Get()
		;*/
		//double ignoreLineStart = 0;
		GetWatchdog().SetEnabled(true);
		GetWatchdog().SetExpiration(0.2);		
		float liftSP = 0;
		PIDLift->SetTolerance(10);
		PIDLift->SetContinuous(false);
		PIDLift->SetOutputRange(-0.75, 1); //BUGBUG
		PIDLift->SetPID(LIFTPROPGAIN, LIFTINTGAIN, LIFTDERIVGAIN);
		PIDLift->Enable();
		PIDGrip->SetSetpoint(0);
		PIDGrip->Enable();
		stopCount = 0;

		float reduction;
		int counts = 0;
		leftEncoder->Start();
		rightEncoder->Start();
		leftEncoder->Reset();
		rightEncoder->Reset();
		liftEncoder->Start();
		liftEncoder->Reset();
		leftEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
		rightEncoder->SetDistancePerPulse((6 * PI / 500)/reduction);
		double avgEncoderCount;
		float leftSpeed = .2, rightSpeed = .2;
		short int lsL,lsM,lsR;
		lineFollowDone = false;
		counts = 0;
		//int fancyWaiter = 0;
		int popstage = 0;
		goPop = false;
		double backStart = 0;
		double backTime = 0;
		double popStart = 0;
		double popTime = 0;
		bool backDone = false;
		miniDep->Set(miniDep->kForward);
		
		int liftCount = 0;
		bool disengageBrake = false;
		float lastLiftSP = 0;
		
		gripOpen1->Set(true);
		gripOpen2->Set(false);
		
		gripLatch1->Set(true);
		gripLatch2->Set(false);
		
		
		while(IsAutonomous())
		{
			if(!(counts % 100))printf("%2.2f \n",getDistance());
			if(backStart) backTime = GetClock();
			if(popStart) popTime = GetClock();
			
			//if(!ignoreLineStart)ignoreLineStart = GetClock();
			
			if(!compSwitch->Get()) compressor->Set(compressor->kReverse);
			else compressor->Set(compressor->kOff);
			
			if(counts%3 == 0)
			{
				leftValue = lsLeft->Get();
				middleValue = lsMiddle->Get();
				rightValue = lsRight->Get();
			}
			counts++;
			GetWatchdog().Feed();
			//avgEncoderCount = (leftEncoder->Get() + rightEncoder->Get())/2;
			//myRobot.SetLeftRightMotorOutputs(.2,.2);
			
			//All three/five autonomous programs will do the same thing up until 87 inches from the wall
			
			if (counts % 100 == 0){//TESTING
				if(lsLeft->Get()){
					lsL = 1;
				}else{
					lsL = 0;
				}
				if(lsRight->Get()){
					lsR = 1;
				}else{
					lsR = 0;
				}
				if(lsMiddle->Get()){
					lsM = 1;
				}else{
					lsM = 0;
				}
				//printf("Encoder: %2.2f \n", (float)avgEncoderCount);
//.........这里部分代码省略.........

	void TeleopPeriodic()
	{
		char myString [STAT_STR_LEN];

		if (running)
		{
			enterHoldCommand = joystick->GetRawButton(BUT_JS_ENT_POS_HOLD);
			exitHoldCommand = joystick->GetRawButton(BUT_JS_EXIT_POS_HOLD);

			switch (liftState)
			{
				case raising:
					if (GetLiftLimitSwitchMax())
					{
						SetLiftMotor(MOTOR_SPEED_STOP);
						if(!liftEncFullRanged)
						{
							maxLiftEncDist = liftEncoder->GetDistance();
							liftEncFullRanged = true;
						}
						motorSpeed = -MOTOR_SPEED_DOWN;
						liftState = lowering;
						SetLiftMotor(motorSpeed);
					}

					if (enterHoldCommand && liftEncZeroed && liftEncFullRanged)
					{
						liftState = holding;
					}

					break;

				case lowering:
					if (GetLiftLimitSwitchMin())
					{
						SetLiftMotor(MOTOR_SPEED_STOP);
						if(!liftEncZeroed)
						{
							liftEncoder->Reset();
							liftEncZeroed = true;
						}
						motorSpeed=MOTOR_SPEED_UP;
						liftState = raising;
						SetLiftMotor(motorSpeed);
					}

					if (enterHoldCommand && liftEncZeroed && liftEncFullRanged)
					{
						liftState = holding;
					}
					break;

				case holding:
					if(!(controlLift->IsEnabled()))
					{
						pidPosSetPoint = SP_RANGE_FRACTION*maxLiftEncDist; //go to the midpoint of the range
						controlLift->SetSetpoint(pidPosSetPoint);
#if BUILD_VERSION == COMPETITION
						controlLift2->SetSetpoint(pidPosSetPoint);
#endif
						controlLift->Enable();
#if BUILD_VERSION == COMPETITION
						controlLift2->Enable();
#endif
					}

					if(exitHoldCommand)
					{
						controlLift->Disable();
#if BUILD_VERSION == COMPETITION
						controlLift2->Disable();
#endif
						motorSpeed = -MOTOR_SPEED_DOWN;
						liftState = lowering;
						SetLiftMotor(motorSpeed);
					}
				break;
			}
		}

		//status
		sprintf(myString, "running: %d\n", running);
		SmartDashboard::PutString("DB/String 0", myString);
		sprintf(myString, "State: %d\n", liftState);
		SmartDashboard::PutString("DB/String 1", myString);
		sprintf(myString, "motorSpeed: %f\n", motorSpeed);
		SmartDashboard::PutString("DB/String 2", myString);
		sprintf(myString, "lift encoder zeroed: %d\n", liftEncZeroed);
		SmartDashboard::PutString("DB/String 3", myString);
		sprintf(myString, "max enc set: %d\n", liftEncFullRanged);
		SmartDashboard::PutString("DB/String 4", myString);
		sprintf(myString, "maxLiftEncDist: %f\n", maxLiftEncDist);
		SmartDashboard::PutString("DB/String 5", myString);
		sprintf(myString, "enc dist: %f\n", liftEncoder->GetDistance());
		SmartDashboard::PutString("DB/String 6", myString);
		sprintf(myString, "pid: %d\n", controlLift->IsEnabled());
		SmartDashboard::PutString("DB/String 7", myString);
		sprintf(myString, "dist to sp : %f\n", DistToSetpoint());
		SmartDashboard::PutString("DB/String 8", myString);
		sprintf(myString, "at sp : %d\n", AtSetpoint());
//.........这里部分代码省略.........

	void AutonomousSpy() {
//		Strategy 1 - start as spy with a boulder, score in lower goal. Starts with intake facing low goal
//		-------------------------------------------------------------------------------------------------------------------
		switch (currentState) {
		case 1:
			//		-State: stopped
			timer->Reset();
			timer->Start();
			ahrs->ZeroYaw();
			currentState = 2;
			break;

//		--transition: state Driving Forward
		case 2:
			//		-State: Driving Forward
			//		--wait until lined up with low goal
			//		--transition: State stopped
			drive->TankDrive(0.5, 0.5);
			if (timer->Get() >= 1) { // NEEDS TO BE SET
				//		-State: stopped
				//		--wait until stopped
				drive->TankDrive(0.0, 0.0);
				currentState = 3;
				timer->Reset();
				timer->Start();
			}
			break;
			//		--transition: State Shooting
		case 3:
//		-State: Shooting
//		--wait until shooting complete
			intake->Set(-.5);
			if (timer->Get() >= .7) { //Find Out Actual Time
				intake->Set(0);
				timer->Reset();
				timer->Start();
				currentState = 4;
			}
			break;
			//		--transition: State Backing Up
		case 4:
			//		-State: Backing Up
			//		--wait until off tower ramp
			drive->TankDrive(-0.5, -0.5);
			if (timer->Get() > 1) {
				drive->TankDrive(0.0, 0.0);
				ahrs->ZeroYaw();
				ahrs->Reset();
				currentState = 5;
				turnController->SetSetpoint(-65.5);
				turnController->Enable();
			}
			break;

//		--transition: Turning
		case 5:
			//		-State: Turning Left
			//		--wait until 65 degrees has been reached to line up with low bar
			drive->TankDrive(-0.5, 0.5);
			if (turnController->OnTarget()) {
				drive->TankDrive(0.0, 0.0);
				timer->Reset();
				timer->Start();
				currentState = 6;
			}
			break;
//		--transition: Backing Up
		case 6:
			//		-State backing Up
			//		--wait until near guard wall
			drive->TankDrive(-0.5, -0.5);
			if (timer->Get() >= 1) {
				drive->TankDrive(0.0, 0.0);
				ahrs->ZeroYaw();
				ahrs->Reset();
				currentState = 7;
				turnController->SetSetpoint(-24.5);
				turnController->Enable();
			}
			break;
//		--transition: Turn Left
		case 7:
//		-State: Turn Right
//		--wait until 25 degree turn has been made to line with low bar
			drive->TankDrive(-0.5, 0.5);
			if (turnController->OnTarget()) {
				drive->TankDrive(0.0, 0.0);
				timer->Reset();
				timer->Start();
				currentState = 8;
			}
			break;
//		--transition: Back Up
		case 8:
//		-State: Backing Up
//		--wait until backed through low bar
			drive->TankDrive(-0.5, -0.5);
			if (timer->Get() >= 1) { // NeedTo Update Value
				timer->Stop();
				currentState = 9;
//.........这里部分代码省略.........