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

	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;

		}

	}

//.........这里部分代码省略.........
				{
					rightSP = 0;
					if(!(count%3) && !BACKWARDBUTTON)
					{
						PIDDriveRight->Reset();
						PIDDriveRight->Enable();
					}
				}
				else rightSP = stickY[1] * stickY[1] * (stickY[1]/stickYAbs[1]);
				
				if (BACKWARDBUTTON)
				{
					tempRightSP = rightSP;
					tempLeftSP = leftSP;
					rightSP = -tempLeftSP;
					leftSP = -tempRightSP; //This line and above line sets opposite values for the controller. ...Theoretically.
				}
				
				PIDDriveLeft->SetSetpoint(leftSP);
				PIDDriveRight->SetSetpoint(rightSP);
					
				
				leftSpeed = leftEncoder->GetRate();
				rightSpeed = rightEncoder->GetRate();
				if(!(count++ % 5))
				{
				printf("rate L: %2.2f R: %2.2f SP %2.4f %2.4f ERR %2.2f %2.2f Pow: %1.2f %1.2f\n", 
						leftPIDSource->PIDGet(), rightPIDSource->PIDGet(), leftSP, rightSP,
						PIDDriveLeft->GetError(), PIDDriveRight->GetError(), frontLeftMotor->Get(),
						frontRightMotor->Get());
						
				
				//printf("Throttle value: %f", stickR.GetThrottle());
				if(PIDDriveRight->OnTarget()) printf("Right on \n");
				if(PIDDriveLeft->OnTarget()) printf("Left on \n");
				}
					
				if(PIDRESETBUTTON)
				{
					//PIDDriveRight->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN); 
					//PIDDriveLeft->SetPID(stickR.GetThrottle()+1,DRIVEINTGAIN, DRIVEDERIVGAIN);
					PIDDriveLeft->Reset();
					PIDDriveRight->Reset();
					PIDDriveLeft->Enable();
					PIDDriveRight->Enable();
				}
			}
			else
			{
				if(PIDDriveLeft->IsEnabled()) PIDDriveLeft->Reset();
				if(PIDDriveRight->IsEnabled()) PIDDriveRight->Reset();
				if(DEMOSWITCH)
				{
					stickY[0] = stickY[0]*(1 - lift->getPosition()); //reduces power based on lift height
					stickY[1] = stickY[0]*(1 - lift->getPosition());
				}
				if(stickYAbs[0] > 0.05)
				{
					frontLeftMotor->Set(stickY[0]);
					rearLeftMotor->Set(stickY[0]);
				}
				else
				{
					frontLeftMotor->Set(0);
					rearLeftMotor->Set(0);
				}

	void TeleopPeriodic() override {
		float leftPower, rightPower; // Get the values for the main drive train joystick controllers
		leftPower = -leftjoystick->GetY();
		rightPower = -rightjoystick->GetY();

		float multiplier; // TURBO mode
		if (rightjoystick->GetRawButton(1))
		{
			multiplier = 1;
		} else {
			multiplier = 0.5;
		}

		// wtf is a setpoint - it's an angle to turn to
		if (leftjoystick->GetRawButton(6)) {
			turnController->Reset();
			turnController->SetSetpoint(0);
			turnController->Enable();
			ahrs->ZeroYaw();
			//ahrs->Reset();
		}

		// Press button to auto calculate angle to rotate bot to nearest ball
//		if(leftjoystick->GetRawButton(99))
//		{
//			ahrs->ZeroYaw();
//			turnController->Reset();
//			turnController->SetSetpoint(mqServer.GetDouble("angle"));
//			turnController->Enable();
//			aimState = 1;
//		}

		switch(aimState)
		{
		default:
		case 0: // No camera assisted turning
			//Drive straight with one controller, else: drive with two controllers
			if (leftjoystick->GetRawButton(1)) {
				drive->TankDrive(leftPower * multiplier, leftPower * multiplier,
						false);
			} else if (leftjoystick->GetRawButton(2)) {
				drive->TankDrive(leftPower * multiplier + rotateRate,
						leftPower * multiplier + -rotateRate, false);
			} else {
				drive->TankDrive(leftPower * multiplier, rightPower * multiplier,
						false);
			}
			break;
		case 1: // Camera assisted turning, deny input from controllers
			drive->TankDrive(rotateRate, -rotateRate, false);
			if(turnController->OnTarget() || leftjoystick->GetRawButton(97)) {
				aimState = 0; // Finished turning, auto assist off
				turnController->Disable();
				turnController->Reset();
			}
			break;
		}

		// That little flap at the bottom of the joystick
		float scaleIntake = (1 - (controlstick->GetThrottle() + 1) / 2);
		// Depending on the button, our intake will eat or shoot the ball
		if (controlstick->GetRawButton(2)) {
			intake->Set(-scaleIntake);
			shooter->Set(scaleIntake);
		} else if (controlstick->GetRawButton(1)) {
			intake->Set(scaleIntake);
			shooter->Set(-scaleIntake);
		} else {
			intake->Set(0);
			shooter->Set(0);
		}

		// Control the motor that lifts and descends the intake bar
		float intake_lever_power = 0;
		if (controlstick->GetRawButton(6)) {
			manual = true;
			intake_lever_power = .3;
//			intakeLever->Set(.30); // close
		} else if (controlstick->GetRawButton(4)) {
			manual = true;
			intake_lever_power = -.4;
//			intakeLever->Set(-.40); // open
		} else if (controlstick->GetRawButton(3)){
			manual = true;
			intake_lever_power = -scaleIntake;
//			intakeLever->Set(-scaleIntake);
		} else if (controlstick->GetRawButton(5)) {
			manual = true;
			intake_lever_power = scaleIntake;
//			intakeLever->Set(scaleIntake);
		} else {
			if (manual) {
				manual = false;
				lastLiftPos = intakeLever->GetEncPosition();
				intakeLever->SetControlMode(CANTalon::ControlMode::kPosition);
				intakeLever->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
				intakeLever->SetPID(1, 0.001, 0.0);
				intakeLever->EnableControl();
			}
			intake_hold = true;
//.........这里部分代码省略.........

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