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

	Robot()
	{
#if BUILD_VERSION == COMPETITION
		liftMotor = new CANTalon(CHAN_LIFT_MOTOR);
		liftMotor2 = new CANTalon(CHAN_LIFT_MOTOR2);
#else
		liftMotor = new CANJaguar(CHAN_LIFT_MOTOR);
		liftMotor2 = new CANJaguar(CHAN_LIFT_MOTOR2);
#endif
		liftEncoder = new Encoder(CHAN_LIFT_ENCODER_A, CHAN_LIFT_ENCODER_B, false, Encoder::EncodingType::k4X);
		liftEncoder->SetDistancePerPulse(LIFT_ENCODER_DIST_PER_PULSE);
		liftEncoder->SetPIDSourceParameter(liftEncoder->kDistance);
#if BUILD_VERSION == COMPETITION
		liftEncoder->SetReverseDirection(true);
#endif
		controlLift = new PIDController(PID_P, PID_I, PID_D, liftEncoder, liftMotor);
		controlLift->SetContinuous(true); //treat input to controller as continuous; true by default
		controlLift->SetOutputRange(PID_OUT_MIN, PID_OUT_MAX);
		controlLift->Disable(); //do not enable until in holding position mode
		controlLift2 = new PIDController(PID_P, PID_I, PID_D, liftEncoder, liftMotor2);
		controlLift2->SetContinuous(true); //treat input to controller as continuous; true by default
		controlLift2->SetOutputRange(PID_OUT_MIN, PID_OUT_MAX);
		controlLift2->Disable(); //do not enable until in holding position mode
		liftLimitSwitchMin = new DigitalInput(CHAN_LIFT_LOW_LS);
		liftLimitSwitchMax = new DigitalInput(CHAN_LIFT_HIGH_LS);
		joystick = new Joystick(CHAN_JS);
	}

	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()

	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 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;
		}
	}

 /**
  * 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
     }
 }

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

        DriveTrain(Vision* visionTracking) :
                IComponent(new string("DriveTrain")),
                leftDriveMaster(new CANTalon(1)),
                leftDriveSlave1(new CANTalon(3)),
                leftDriveSlave2(new CANTalon(5)),
                rightDriveMaster(new CANTalon(2)),
                rightDriveSlave1(new CANTalon(4)),
                rightDriveSlave2(new CANTalon(6)),
                shift(new Solenoid(4)),
                vision(visionTracking),
                visionPIDSource(new DrivePIDSource()),
                visionPIDOutput(new DrivePIDOutput()),
                visionPID(new PIDController(0.70f, 0, 0, visionPIDSource, visionPIDOutput)),
                angleETC(new ErrorTimeCubed(DRIVE_ANGLE_TOLERANCE, 45.0f, -180.0f, 180.0f)),
                crossTime(new Timer()),
                hasCrossed(false),
                crossState(0),
                isClimbing(true),
                driveTime(new Timer()),
                timedDriveState(0),
                shiftHigh(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BUMPER_RIGHT)),
                shiftLow(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BUMPER_LEFT)),
                stateUntoggle(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_BACK)),
                autoCrossToggle(new RobotButton(RobotButton::JoystickType::PRIMARY, NEW_JOYSTICK ? RobotButton::ControlTypes::AXIS : RobotButton::ControlTypes::KEY, JOYSTICK_TRIGGER_RIGHT)),
                reverseToggle(new RobotButton(RobotButton::JoystickType::PRIMARY, RobotButton::ControlTypes::KEY, JOYSTICK_X)),
                crossSpeedMultiplier(1.0f),
                crossingForward(true),
                leftSpeedCurrent(0),
                rightSpeedCurrent(0),
                targetDistance(0),
                crossReverse(false),
                reverse(true),
                primaryDriving(false),
                state(DriveState::NONE)
        {
            leftDriveMaster->SetControlMode(CANTalon::ControlMode::kPosition);
            leftDriveMaster->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
            leftDriveMaster->ConfigEncoderCodesPerRev(1024);
            leftDriveMaster->Enable();

            leftDriveSlave1->SetControlMode(CANTalon::ControlMode::kFollower);
            leftDriveSlave1->Enable();
            leftDriveSlave1->Set(1);

            leftDriveSlave2->SetControlMode(CANTalon::ControlMode::kFollower);
            leftDriveSlave2->Enable();
            leftDriveSlave2->Set(1);

            rightDriveMaster->SetControlMode(CANTalon::ControlMode::kPosition);
            rightDriveMaster->SetFeedbackDevice(CANTalon::FeedbackDevice::QuadEncoder);
            leftDriveMaster->ConfigEncoderCodesPerRev(1024);
            rightDriveMaster->Enable();

            rightDriveSlave1->SetControlMode(CANTalon::ControlMode::kFollower);
            rightDriveSlave1->Enable();
            rightDriveSlave1->Set(2);

            rightDriveSlave2->SetControlMode(CANTalon::ControlMode::kFollower);
            rightDriveSlave2->Enable();
            rightDriveSlave2->Set(2);

            visionPID->SetInputRange(-1, 1);
            visionPID->SetOutputRange(-1, 1);
            visionPID->SetContinuous(true);
            visionPID->SetAbsoluteTolerance(0.05);
            visionPID->Disable();
        }

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