C++ Encoder::SetDistancePerPulse方法代码示例

    BuiltinDefaultCode()	{

        m_robotDrive = new RobotDrive (m_lDrive, m_rDrive);
        m_driver = new Joystick (1);
        m_operator = new Joystick (2);
        m_rDrive = new Victor (1);
        m_lDrive = new Victor (2);
        m_climber = new Victor (3);
        m_plate1 = new Victor (8);
        m_plate2 = new Victor (10);
        m_launcher1 = new Victor (4);
        m_launcher2 = new Victor (5);
        m_launcher3 = new Victor (6);
        m_feeder = new Victor (7);
        m_ratchet = new Relay (1);
        m_left = new Encoder (1,2,true);
        m_left->SetDistancePerPulse(1);
        m_left->SetMaxPeriod(1.0);
        m_left->Start();
        m_right = new Encoder (3,4,false);
        m_right->SetDistancePerPulse(1);
        m_right->SetMaxPeriod(1.0);
        m_right->Start();
        m_plateh = new AnalogChannel (1);
        m_climberp = new AnalogChannel (2);
        m_dsLCD = DriverStationLCD::GetInstance();

    }

	RobotSystem(void):
		robotInted(false)
		,stick(1)		// as they are declared above.
		,stick2(2)
		,line1(10)
		,line2(11)
		,line3(12)
		//,camera(AxisCamera::GetInstance())
		,updateCAN("CANUpdate",(FUNCPTR)UpdateCAN)
		,cameraTask("CAMERA", (FUNCPTR)CameraTask)
		,compressor(14,1)
		,EncArm(2,3)
		,EncClaw(5,6)
		,PIDArm(.04,0,0) // .002, .033
		,PIDClaw(.014,.0000014,0)
		,LowArm(.1)
		/*
		,MiniBot1(4)
		,MiniBot2(2)
		,ClawGrip(3)
		*/
		,MiniBot1a(8,1)
		,MiniBot1b(8,2)
		,MiniBot2a(8,3)
		,MiniBot2b(8,4)
		,ClawOpen(8, 8)
		,ClawClose(8,7)
		,LimitClaw(7)
		,LimitArm(13)
	{
	//	myRobot.SetExpiration(0.1);
		GetWatchdog().SetEnabled(false);
		GetWatchdog().SetExpiration(1);
		compressor.Start();
		debug("Waiting to init CAN");
		Wait(2);
		
		Dlf = new CANJaguar(6,CANJaguar::kSpeed);
		Dlb = new CANJaguar(3,CANJaguar::kSpeed);
		Drf = new CANJaguar(7,CANJaguar::kSpeed);
		Drb = new CANJaguar(2,CANJaguar::kSpeed);
		arm1 = new CANJaguar(5);
		arm1_sec = new CANJaguar(8);
		arm2 = new CANJaguar(4);
		
		
		EncArm.SetDistancePerPulse(.00025);
		EncClaw.SetDistancePerPulse(.00025);
		EncClaw.SetReverseDirection(false);
		EncArm.SetReverseDirection(true);
		EncArm.Reset();
		EncClaw.Reset();
		
		
		updateCAN.Start((int)this);
		//cameraTask.Start((int)this);
		EncArm.Start();
		EncClaw.Start();
		debug("done initing");
	}

	RobotDemo()
	{
		//Initialize the objects for the drive train
		myRobot = new RobotDrive(1, 2);
		leftEnco = new Encoder(LEFT_ENCO_PORT_1, LEFT_ENCO_PORT_2);
		rightEnco = new Encoder(RIGHT_ENCO_PORT_1, RIGHT_ENCO_PORT_2);
		leftEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
		leftEnco->Start();
		rightEnco->SetDistancePerPulse((4 * 3.14159)/ENCO_PULSES_PER_REV);
		rightEnco->Start();
		leftStick = new Joystick(1);
		rightStick = new Joystick(2);

		//Initialize the gyro
		gyro = new Gyro(GYRO_PORT);
		gyro->Reset();

		//Initialize the manipulators
		intake = new Intake(INTAKE_ROLLER_PORT, BALL_SENSOR_PORT, LEFT_SERVO_PORT, RIGHT_SERVO_PORT);
		catapult = new Catapult(LOADING_MOTOR_PORT, HOLDING_MOTOR_PORT, LOADED_LIMIT_1_PORT,
				LOADED_LIMIT_2_PORT, HOLDING_LIMIT_PORT);

		//Initialize the objects needed for camera tracking
		rpi = new RaspberryPi("17140");
		LEDLight = new Relay(1);
		LEDLight->Set(Relay::kForward);

		//Set the autonomous modes
		autonMode = ONE_BALL_AUTON;
		autonStep = AUTON_ONE_SHOOT;

		//Initialize the lcd
		lcd = DriverStationLCD::GetInstance();
	}

  void RobotInit ()
  {
  lw = LiveWindow::GetInstance();
  CameraServer::GetInstance()->SetQuality(50);
   //the camera name (ex "cam0") can be found through the roborio web interface
  CameraServer::GetInstance()->StartAutomaticCapture("cam1");
  AutonState = 0;
  ballarm.Reset();
  ballarm.SetMaxPeriod(.01);
  ballarm.SetMinRate(.02);
  ballarm.SetDistancePerPulse(.9);
  gyroOne.Calibrate();
  UpdateActuatorCmnds(0,0,false,false,false,false,false,false,false,0,0,0,0,0);

  UpdateSmartDashboad(false,
                      false,
                      false,
                      false,
                      false,
                      0,
                      0,
                      0,
                      0,
                      0,
                      0,
                      0,
                      0,
                      0);
  }

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

    // WHen robot is enabled
    void init() {
        log->info("initializing");
        log->print();
        climber = NULL;
        leftDriveEncoder->Reset();
        leftDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
        leftDriveEncoder->Start();
		rightDriveEncoder->Reset();
        rightDriveEncoder->SetDistancePerPulse(DriveDistancePerPulse);
        rightDriveEncoder->Start();
        //leftClimber->motorController->Disable();
		leftClimber->encoder->Reset();
        leftClimber->encoder->Start();
        //rightClimber->motorController->Disable();
        rightClimber->encoder->Reset();
        rightClimber->encoder->Start();
        bcdValue = bcd->value();
        loadSwitchOldState = loaderSwitch->Get();
        #if 0
		bool leftDone = false;
		bool rightDone = false;
        // Only do this for some BCD values?
        while (!leftDone || !rightDone){
			if (!leftDone)
				leftDone = leftClimber->UpdateState(-1.0, -30);
			if (!rightDone)
				rightDone = rightClimber->UpdateState(-1.0, -30);
	        log->info("Wait: Ll Rl: %d %d",
	        		leftClimber->lowerLimitSwitch->Get(),
	        		rightClimber->lowerLimitSwitch->Get());
	        log->print();
		}
#endif
        climbState = NotInitialized;
        cameraElevateAngle =
        		(cameraElevateMotor->GetMaxAngle()-cameraElevateMotor->GetMinAngle()) * 2/3;
        cameraPivotAngle = 0;
        cameraPivotMotor->SetAngle(cameraPivotAngle);
        cameraElevateMotor->SetAngle(cameraElevateAngle);
        loading = false;
        loaderDisengageDetected = false;
        //This is a rough guess of motor power it should be based on voltage
        shooterMotorVolts = 8.0; // volts as a fraction of 12V
    	loadCount = 0;
    }

	C1983_Lift(Encoder *liftSensorChannel, Relay *brake)
	{
		m_liftSensor = liftSensorChannel;
		m_brake = brake;
		m_bIsBraking = true;
		
		m_brake->SetDirection(m_brake->kBothDirections);
		m_liftSensor->SetDistancePerPulse((float)LIFTDISTPERPULSE);
	}

        ControlledMotor(
        		Robot* r,
				const char* n,
				CANJaguar* m,
				Encoder* e,
				double dpp,
				DigitalInput* l) :
			robot(r), name(n), motor(m), encoder(e),
			distancePerPulse(dpp),lowerLimitSwitch(l),
			motorController()
		{
        	motorController = new PIDController(.1, .001, 0.0, encoder, motor);
        	encoder->SetDistancePerPulse(dpp);
		}

	BuiltinDefaultCode()	{
		//Initialze drive controllers
		m_rDrive1 = new Talon (1);
		m_rDrive2 = new Talon (2);
		m_lDrive1 = new Talon (3);
		m_lDrive2 = new Talon (4);

		//Initialize ramrod motor
		m_ramMotor = new Talon (5);

		//Initialize Arm
		m_arm = new ArmWrapper (7, 6, 5, 6, 10);
		m_arm->StartPID(0.0, 0.0, 0.0);

		//initialize bGrabber motor
		m_roller = new Talon (8);

		//Initialize ramrod servo
		m_ramServo = new Servo (9);

		//Initialize drive wrappers
		m_rDrive = new DriveWrapper (m_rDrive1, m_rDrive2);
		m_lDrive = new DriveWrapper (m_lDrive1, m_lDrive2);

		//Initialize robot drive
		m_robotDrive = new RobotDrive (m_lDrive, m_rDrive);

		//Initialize ramrod encoder
		m_ramEncoder = new Encoder (7,8,false);
		m_ramEncoder->SetDistancePerPulse(1);
		m_ramEncoder->SetMaxPeriod(1.0);
		m_ramEncoder->Start();

		//Initialize Compressor
		m_compressor = new Compressor(9, 1);

		//shifters
		m_shifters = new Solenoid(1);

		//Initialize bGrabber Solenoids
		m_bArm = new Solenoid (2);
		m_catch = new Solenoid (3);

		//Initialize joysticks
		m_driver = new JoystickWrapper (1);
		m_operator = new JoystickWrapper (2);

		//Grab driver station object
		m_dsLCD = DriverStationLCD::GetInstance();		
	}

	RobotDemo(void):
		leftDriveMotor(LEFT_DRIVE_PWM),
		rightDriveMotor(RIGHT_DRIVE_PWM),
		myRobot(&leftDriveMotor, &rightDriveMotor),	// these must be initialized in the same order
		stick(1),									// as they are declared above.
		stick2(2),
		gamepad(3),
		collectorMotor(PICKUP_PWM),
		indexerMotor(INDEX_PWM),
		shooterMotor(SHOOTER_PWM),
		armMotor (ARM_PWM),
		leftDriveEncoder(LEFT_DRIVE_ENC_A, LEFT_DRIVE_ENC_B),
		shifter(SHIFTER_A,SHIFTER_B),
		greenClaw(CLAW_1_LOCKED, CLAW_1_UNLOCKED),
		yellowClaw(CLAW_2_LOCKED, CLAW_2_UNLOCKED),
		potentiometer(ARM_ROTATION_POT),
		indexSwitch(INDEXER_SW),
		greenClawLockSwitch(CLAW_1_LOCK_SENSOR),
		yellowClawLockSwitch(CLAW_2_LOCK_SENSOR),
		compressor(COMPRESSOR_PRESSURE_SW, COMPRESSOR_SPIKE),
		jogTimer(),
		shooterTimer()
	{
		m_collectorMotorRunning = false;
		m_shooterMotorRunning   = false;
		m_jogTimerRunning       = false;
		m_shiftCount            = MAX_SHIFTS;
		
		dsLCD = DriverStationLCD::GetInstance();
		dsLCD->Clear();
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "2013 " NAME);
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, __DATE__ " "__TIME__);

		dsLCD->UpdateLCD();
		myRobot.SetExpiration(0.1);
		shifter.Set(DoubleSolenoid::kReverse);
		
		leftDriveEncoder.SetDistancePerPulse(DRIVE_ENCODER_DISTANCE_PER_PULSE);
		leftDriveEncoder.SetMaxPeriod(1.0);
		leftDriveEncoder.SetReverseDirection(true);  // change to true if necessary
		leftDriveEncoder.Start();

	}

	void TeleopInit()
	{
// Initialize the encoder
		sampleEncoder = new Encoder(0, 1, false, Encoder::EncodingType::k4X);
		sampleEncoder->SetMaxPeriod(.1);
		sampleEncoder->SetMinRate(10);
		sampleEncoder->SetDistancePerPulse(5);
		sampleEncoder->SetReverseDirection(true);
		sampleEncoder->SetSamplesToAverage(7);

// Initialize the joystick
		joystick = new Joystick(0);

// Initialize the motor
		motor = new Victor(9);

// Initialize the gear tooth counter
		toothTrigger = new AnalogTrigger(3);
		toothTrigger->SetLimitsRaw(250, 3600);
		gearToothCounter = new Counter(toothTrigger);
//		gearToothCounter->SetUpDownCounterMode();
	}

	FECLift()
	{
		theTimer = new Timer();
		//initialized the jag
		theJag = new Jaguar(JAGPORT);
		fakeEncoder = new Encoder(6,6,6,7,fakeEncoder->k4X);
		theEncoder = new Encoder(ENCODERSLOTA, ENCODERCHANNELA, ENCODERSLOTB, ENCODERCHANNELB, false, theEncoder->k4X);
#if RATE
		theEncoder->SetPIDSourceParameter(Encoder::kRate);
#else
		theEncoder->SetPIDSourceParameter(Encoder::kDistance);
#endif
		
		theEncoder->SetDistancePerPulse(DISTANCE_PER_PULSE);
		driverStation = DriverStation::GetInstance();
		theLift = new PIDController(P,I,D,theEncoder,theJag);
		GetWatchdog().SetExpiration(.3);
		
		decel = false;
		done = false;
		writing = true;
		closed = false;
		initFile();
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */ 
	void Autonomous()
	{
		init();
		lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Entered Autonomous");
		driveTrain.SetSafetyEnabled(false);
		bool checkBox1 = SmartDashboard::GetBoolean("Checkbox 1");
		m_FromAutonomous = true;
		//float rightDriveSpeed = -1.0;
		//float leftDriveSpeed = -1.0;
		//int rangeToWallClose = 60;
		//int rangeToWallFar = 120;
		//Initialize encoder.
		//int distanceToShoot = 133;
		//int shootDelay = 0;
		//ballGrabber.elevatorController.SetSetpoint(PHOENIX2014_INITIAL_AUTONOMOUS_ELEVATOR_ANGLE);
		//TODO Remove encoders from code??
		driveDistanceRight.Reset();
		driveDistanceLeft.Reset();
		driveDistanceRight.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_RIGHT);
		driveDistanceLeft.SetDistancePerPulse(PHOENIX2014_DRIVE_DISTANCE_PER_PULSE_LEFT);
		driveDistanceRight.Start();
		driveDistanceLeft.Start();
		//int printDelay = 0;
		float minDistance = 52.0;  // Closer to the wall than this is too close
		float maxDistance = 12.0*11.0; // Once at least this close, within shooting range
		//float closeDistance = maxDistance + 24.0;
		float autoDriveSpeed = 0.55;
		//float autoDriveSlowSpeed = 0.38;
		int time = 0;
		int maxDriveLoop = 4*200; // 4 seconds @200 times/sec

		bool shootingBall = false;
		bool wantFirstShot = true;

		if(checkBox1 == false){
			int printDelay = 0;
			//Ultrasonic Autonomous
			//bool isInRange = false;
			while(IsAutonomous() && IsEnabled())
			{
				float currentDistance = frontUltrasonic.GetAverageDistance();
				// Transition isInRange from false to true once
				if((minDistance < currentDistance) && (currentDistance < maxDistance)){
					//isInRange = true;
				}
				time++;
				bool motorDriveTimeExceeded = time > maxDriveLoop;
				bool motorMinMet = time > m_MinDriveLoop;
				if(/*isInRange ||*/ motorMinMet){
					driveTrain.TankDrive(0.0,0.0);
					if((ballGrabber.elevatorAngleSensor.GetVoltage() > PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE - 0.1) &&
							(ballGrabber.elevatorAngleSensor.GetVoltage() < PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE + 0.1)){
						//Enough to cover break release and start winding again.
						
						shootingBall = shooter.OperateShooter(wantFirstShot);
					}
					if(shootingBall == true){
						wantFirstShot = false;
					}
				}
				else if(motorDriveTimeExceeded){
					driveTrain.TankDrive(0.0,0.0);
				}
				else{
					//if((currentDistance < closeDistance) && motorMinMet){
					//	autoDriveSpeed = autoDriveSlowSpeed;
					//}
					driveTrain.TankDrive(-0.97 * autoDriveSpeed, -1.0 * autoDriveSpeed);//the DriveTrain is BACKWARD
				}
				ballGrabber.RunElevatorAutonomous(PHOENIX2014_AUTONOMOUS_ELEVATOR_ANGLE);
				printDelay = printDelay++;
				if(printDelay >= 200){
					lcd->Clear();
					lcd->PrintfLine(DriverStationLCD::kUser_Line1, "In Autonomous");
					shooter.DisplayDebugInfo(DriverStationLCD::kUser_Line2, lcd);
					shooter.PrintShooterState(DriverStationLCD::kUser_Line3, lcd);
					lcd->PrintfLine(DriverStationLCD::kUser_Line4, "Ulra=%f", frontUltrasonic.GetAverageDistance());
					//lcd->PrintfLine(DriverStationLCD::kUser_Line5, "CEV=%f", ballGrabber.elevatorAngleSensor.GetVoltage());
					//lcd->PrintfLine(DriverStationLCD::kUser_Line6, "DEV=%f", ballGrabber.m_desiredElevatorVoltage);
					lcd->UpdateLCD();
					printDelay = 0;
				}
				Wait(0.005);
			}
			lcd->Clear();
			lcd->UpdateLCD();
			lcd->PrintfLine(DriverStationLCD::kUser_Line2, "Exiting Autonomous");
		} else {
			//Timer Autonomous
			driveTrain.TankDrive(-0.5,-0.5);
			ballGrabber.DriveElevatorTestMode(-1.0);
			lcd->Clear();
			lcd->PrintfLine(DriverStationLCD::kUser_Line1, "Skip Auto");
			lcd->PrintfLine(DriverStationLCD::kUser_Line2, "CheckBox Checked");
			lcd->UpdateLCD();
			Wait(2.0);
			bool shooting = true;
//.........这里部分代码省略.........

	void RobotInit()
	{
		frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0);
		//camera.reset(new AxisCamera("axis-camera.local"));

		table = NetworkTable::GetTable("GRIP/myContoursReport");

		chooser = new SendableChooser();
		chooser->AddDefault(autoNameDefault, (void*)&autoNameDefault);
		for (std::map<std::string, bool (*)()>::const_iterator it = Autonomous::crossFunctions.begin(); it!= Autonomous::crossFunctions.end(); it++) {
			chooser->AddObject(it->first, (void*)&(it->first));
		}
		SmartDashboard::PutData("Auto Modes", chooser);

		posChooser = new SendableChooser();
		posChooser->AddDefault(posDefault, (void*)&posToDegrees[stoi(posDefault)]);
		for (int i = 1; i < 6; i++) {
			posChooser->AddObject(std::to_string(i), (void*)&posToDegrees[i]);
		}
		SmartDashboard::PutData("Position", posChooser);

		shootChooser = new SendableChooser();
		shootChooser->AddDefault(shootDefault, (void*)&shootDefault);
		shootChooser->AddObject("No", (void*)"No");

		SmartDashboard::PutData("Shoot", shootChooser);

		drive = new RobotDrive(2,3,0,1);
		//drive = new RobotDrive(frontLeft, backLeft, frontRight, backRight);

		drive->SetInvertedMotor(RobotDrive::MotorType::kFrontLeftMotor, true);
		drive->SetInvertedMotor(RobotDrive::MotorType::kRearLeftMotor, true);
		drive->SetInvertedMotor(RobotDrive::MotorType::kFrontRightMotor, true);
		drive->SetInvertedMotor(RobotDrive::MotorType::kRearRightMotor, true);
		drive->SetExpiration(0.2);

		drive->SetMaxOutput(0.5);

		driveStick = new Joystick(0);
		shootStick = new Joystick(1);

		launchPiston = new Solenoid(3);
		//tiltPiston = new DoubleSolenoid(0,1);
		defensePiston = new DoubleSolenoid(0,1);

		launch1 = new VictorSP(4);
		launch2 = new VictorSP(5);
		launch1->SetInverted(true);

		winch = new VictorSP(6);
		otherWinch = new Victor(7);

		gyro = new AnalogGyro(1);
		leftEnc = new Encoder(2, 3, false, Encoder::EncodingType::k1X);
		rightEnc = new Encoder(0,1, false, Encoder::EncodingType::k1X);
		//Configure for inches.t551
		leftEnc->SetDistancePerPulse(-0.06);
		rightEnc->SetDistancePerPulse(0.06);

		launcherSensor = new DigitalInput(9);

		Autonomous::init(drive, gyro, leftEnc, rightEnc);

		timer =  new Timer();
		defenseUp = false;
		debounce = false;
		//if (fork() == 0) {
		//            system("/home/lvuser/grip &");
		//}

		launcherDown = false;
	}

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

//.........这里部分代码省略.........