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

	Robot() :
            robotDrive(frontLeftChannel, rearLeftChannel,
                       frontRightChannel, rearRightChannel), // initialize variables in same
            stick(joystickChannel)                           // order as declared above.
    {
	    rotateToAngleRate = 0.0f;
        robotDrive.SetExpiration(0.1);
        robotDrive.SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);	// invert left side motors
        robotDrive.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);	// change to match your robot
        try {
            /* Communicate w/navX MXP via the MXP SPI Bus.                                       */
            /* Alternatively:  I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */
            /* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details.   */
            ahrs = new AHRS(SPI::Port::kMXP);
        } catch (std::exception ex ) {
            std::string err_string = "Error instantiating navX MXP:  ";
            err_string += ex.what();
            DriverStation::ReportError(err_string.c_str());
        }
        turnController = new PIDController(kP, kI, kD, kF, ahrs, this);
        turnController->SetInputRange(-180.0f,  180.0f);
        turnController->SetOutputRange(-1.0, 1.0);
        turnController->SetAbsoluteTolerance(kToleranceDegrees);
        turnController->SetContinuous(true);

        /* Add the PID Controller to the Test-mode dashboard, allowing manual  */
        /* tuning of the Turn Controller's P, I and D coefficients.            */
        /* Typically, only the P value needs to be modified.                   */
        LiveWindow::GetInstance()->AddActuator("DriveSystem", "RotateController", turnController);
        if ( ahrs ) {
            LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
        }
    }

	void resetPIDControllers() {
		delete launchController;
		delete gatherController;
		delete turnController;

		launchController = new PIDController(kLaunchP, kLaunchI, kLaunchD, kLaunchF, &launchPIDSource, &launchPIDOutput);
		launchController->SetInputRange(kLaunchMinAngle, kLaunchMaxAngle);
		launchController->SetOutputRange(-1.0, 1.0);
		launchController->SetAbsoluteTolerance(0.05);
		launchController->SetContinuous(false);

		gatherController = new PIDController(kGatherP, kGatherI, kGatherD, kGatherF, &gatherPIDSource, &gatherPIDOutput);
		gatherController->SetInputRange(0.0, 180.0);
		gatherController->SetOutputRange(-0.5, 0.5);
		gatherController->SetAbsoluteTolerance(0.1);
		gatherController->SetContinuous(false);

		turnController = new PIDController(kTurnP, kTurnI, kTurnD, kTurnF, turnPIDSource, &turnPIDOutput);
		turnController->SetInputRange(-160.0, 160.0);
		turnController->SetOutputRange(-0.4, 0.4);
		turnController->SetAbsoluteTolerance(0.05);
		turnController->SetContinuous(false);

	}

	/**
	 *  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 RobotInit() override {
		lw = LiveWindow::GetInstance();
		drive->SetExpiration(20000);
		drive->SetSafetyEnabled(false);
		//Gyroscope stuff
		try {
			/* Communicate w/navX-MXP via the MXP SPI Bus.                                       */
			/* Alternatively:  I2C::Port::kMXP, SerialPort::Port::kMXP or SerialPort::Port::kUSB */
			/* See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for details.   */
			ahrs = new AHRS(SPI::Port::kMXP);
		} catch (std::exception ex) {
			std::string err_string = "Error instantiating navX-MXP:  ";
			err_string += ex.what();
			//DriverStation::ReportError(err_string.c_str());
		}

		if (ahrs) {
			LiveWindow::GetInstance()->AddSensor("IMU", "Gyro", ahrs);
			ahrs->ZeroYaw();

			// Kp	  Ki	 Kd		Kf    PIDSource PIDoutput
			turnController = new PIDController(0.015f, 0.003f, 0.100f, 0.00f,
					ahrs, this);
			turnController->SetInputRange(-180.0f, 180.0f);
			turnController->SetOutputRange(-1.0, 1.0);
			turnController->SetAbsoluteTolerance(2); //tolerance in degrees
			turnController->SetContinuous(true);
		}
		chooser.AddDefault("No Auto", new int(0));
		chooser.AddObject("GyroTest Auto", new int(1));
		chooser.AddObject("Spy Auto", new int(2));
		chooser.AddObject("Low Bar Auto", new int(3));
		chooser.AddObject("Straight Spy Auto", new int(4));
		chooser.AddObject("Adjustable Straight Auto", new int(5));
		SmartDashboard::PutNumber(AUTO_LENGTH, AUTO_LENGTH_DEFAULT);
		SmartDashboard::PutNumber(AUTO_SPEED, AUTO_SPEED_DEFAULT);
		SmartDashboard::PutNumber(AUTO_INTAKE_SPEED, AUTO_INTAKE_SPEED_DEFAULT);
		SmartDashboard::PutData("Auto Modes", &chooser);
		liftdown->Set(false);
		comp->Start();
	}

        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 SetUp() override {
   pid = new PIDController(0.5, 0.0, 0.0, &inp, &out);
   pid->SetInputRange(-range / 2, range / 2);
 }