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

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

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

	}

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

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

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