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

	void Disabled()
	{
		printf("AAAAAAAAAAAAAAAAAAAAAAAA");
		decel = false;
		done = false;
		theLift->Reset();
	}

	void Disabled(void)
	{
		bool stopped = false;
		while(!IsOperatorControl() && !IsAutonomous())
		{
			if(!stopped)
			{
				frontLeftMotor->Set(0.0);
				rearLeftMotor->Set(0.0);
				frontRightMotor->Set(0.0);
				rearRightMotor->Set(0.0);
				liftMotor1->Set(0.0);
				liftMotor2->Set(0.0);
				gripMotor1->Set(0.0);
				//gripMotor2->Set(0.0);
				PIDLift->Reset();
				PIDDriveLeft->Reset();
				PIDDriveRight->Reset();
				PIDGrip->Reset();
				stopped = true;
			}
		}
	}

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