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

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

	/**
	 * This autonomous (along with the chooser code above) shows how to select between different autonomous modes
	 * using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
	 * Dashboard, remove all of the chooser code and uncomment the GetString line to get the auto name from the text box
	 * below the Gyro
	 *
	 * You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
	 * If using the SendableChooser make sure to add them to the chooser code above as well.
	 */
	void AutonomousInit()
	{
		autoSelected = *((std::string*)chooser->GetSelected());
		//std::string autoSelected = SmartDashboard::GetString("Auto Selector", autoNameDefault);
		std::cout << "Auto selected: " << autoSelected << std::endl;

		rotation = 0.0;
				//*((double*)posChooser->GetSelected());

		//goal = *((std::string*)goalChooser->GetSelected());
		shoot = "No";
		//*((std::string*)shootChooser->GetSelected());

		defenseCrossed = false;
		done = false;


		std::cout << "Here" << std::endl;
		drive->SetMaxOutput(1.0);
		std::cout << "there" << std::endl;
		//Make sure to reset the encoder!
		leftEnc->Reset();
		rightEnc->Reset();
		gyro->Reset();
		autoCounter = 0;
		timer->Reset();
	}

	void RawControl::resetEncoders()
	{
		wheelEncoderFR->Reset();
		wheelEncoderFL->Reset();
		wheelEncoderBR->Reset();
		wheelEncoderBL->Reset();
		
	}

// Starts at the beginning of the autonomous period
void Robot::AutonomousInit() {
	autoLoopCounter = 0;

	encoder1.Reset();
	encoder2.Reset();
	lifterEncoder.Reset();
	ballManipulatorEncoder.Reset();
}

	void TeleopInit()
	{
		leftEnc->Reset();
		rightEnc->Reset();
		gyro->Reset();

		powerCounter = 0;
	}

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

	void Drive (float speed, int dist)
	{
		leftDriveEncoder.Reset();
		leftDriveEncoder.Start();
		
		int reading = 0;
		dist = abs(dist);
		
		// The encoder.Reset() method seems not to set Get() values back to zero,
		// so we use a variable to capture the initial value.
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "initial=%d\n", leftDriveEncoder.Get());
		dsLCD->UpdateLCD();

		// Start moving the robot
		robotDrive.Drive(speed, 0.0);
		
		while ((IsAutonomous()) && (reading <= dist))
		{
			reading = abs(leftDriveEncoder.Get());				
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "reading=%d\n", reading);
			dsLCD->UpdateLCD();
		}

		robotDrive.Drive(0.0, 0.0);
		
		leftDriveEncoder.Stop();
	}	

	// Runs during test mode
	// Test
	// * 
	void Test()
	{
		shifters.Set(DoubleSolenoid::kForward);

		leftDriveEncoder.Start();
		leftDriveEncoder.Reset();

		int start = leftDriveEncoder.Get();

		while (IsTest()) {
			if (rightStick.GetRawButton(7)) {
				robotDrive.ArcadeDrive(rightStick.GetY(), -rightStick.GetX());
			}
			else {
				robotDrive.ArcadeDrive(rightStick.GetY()/2, -rightStick.GetX()/2);
			}

			if (gamepad.GetEvent(4) == kEventClosed) {
				start = leftDriveEncoder.Get();
			}

			dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "lde: %d", leftDriveEncoder.Get() - start);
			dsLCD->UpdateLCD();

			gamepad.Update();
		}
	}

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

	void initRobot () {
			cerr << "running init\n";
			Dlf->EnableControl(0);
			Dlb->EnableControl(0);
			Drf->EnableControl(0);
			Drb->EnableControl(0);
			arm1->EnableControl();
			arm1_sec->EnableControl();
			arm2->EnableControl();
			
			
			
			Dlf->ConfigEncoderCodesPerRev(250);
			Dlf->SetPID(1,0,0);
			Dlb->ConfigEncoderCodesPerRev(250);
			Dlb->SetPID(1,0,0);
			Drf->ConfigEncoderCodesPerRev(250);
			Drf->SetPID(1,0,0);
			Drb->ConfigEncoderCodesPerRev(250);
			Drb->SetPID(1,0,0);
			Wait(.1);
			if(robotInted==false) {
				int count=220;
				arm2->Set(-.3);
				while(count-->0 && LimitClaw.Get() == 1) Wait(.005);
				arm2->Set(.15);
				while(count-->0 && LimitClaw.Get() == 0) Wait(.005);
				arm2->Set(0);
				if(count>0)
					EncClaw.Reset();
				arm1->Set(-.3);
				arm1_sec->Set(-.3);
				while(count-->0 && LimitArm.Get() == 1) Wait(.005);
				arm1->Set(.5);
				arm1_sec->Set(.5);
				while(count-->0 && LimitArm.Get() == 0) Wait(.005);
				if(count>0)
					EncArm.Reset();
				arm1->Set(0);
				arm1_sec->Set(0);
				
				robotInted = true;
			}
		}

	void Test() {
		robotDrive.SetSafetyEnabled(false);
		uint8_t toSend[10];//array of bytes to send over I2C
		uint8_t toReceive[10];//array of bytes to receive over I2C
		uint8_t numToSend = 1;//number of bytes to send
		uint8_t numToReceive = 0;//number of bytes to receive
		toSend[0] = 7; //send 0 to arduino
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);

		bool isSettingUp = true;

		pickup.setGrabber(-1);
		pickup.setLifter(1);
		while (isSettingUp) {
			isSettingUp = false;
			if (grabOuterLimit.Get() == false) {
				pickup.setGrabber(0);
			}
			else {
				isSettingUp = true;
			}

			if (liftLowerLimit.Get()) {
				pickup.setLifter(0);
			}
			else {
				isSettingUp = true;
			}
		}
		gyro.Reset();
		liftEncoder.Reset();
		grabEncoder.Reset();

		toSend[0] = 8;
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);

		while(IsTest() && IsEnabled());

		toSend[0] = 0;
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);
	}

	void OperatorControl(void) {
		char count=0;
		double target = 0, speed = 0;
		while(!IsDisabled()) {
			double tmpStick = -1*stick.GetRawAxis(2);
			if(tmpStick < .2 && tmpStick > -.2) tmpStick=0;
			target += tmpStick*1.5;
			int location = enc.GetRaw();
			if(stick.GetRawButton(5)) {
				up.Set(true);
				down.Set(false);
			}else if(stick.GetRawButton(7) && location > 0) {
				down.Set(true);
				up.Set(false);
			}else if(stick.GetRawButton(8)) {
				down.Set(true);
				up.Set(false);
			}else if(stick.GetRawButton(9)){
			
				speed = pid(target, location);
				if(speed > 1) {
					up.Set(true);
					down.Set(false);
				}else if(speed < -1) {
					up.Set(false);
					down.Set(true);
				}else{
					up.Set(false);
					down.Set(false);
				}
			}else if(stick.GetRawButton(10)) {
				enc.Reset();
			}else{
				up.Set(false);
				down.Set(false);
			}
			if(stick.GetRawButton(1))
				target = 2;
			if(stick.GetRawButton(4))
				target = 400;
			if(stick.GetRawButton(3))
				target = 200;
			if(stick.GetRawButton(2))
				target = 70;
				
			Wait(.02);
			while(count++%30==0) cerr << location << '\t' << target << '\t' << speed << endl;
		}
	}

	/**
	 * Runs during test mode	```````
	 */
	void Test() {
		TestMode tester(m_ds);
		driveDistanceRight.Reset();
		driveDistanceRight.Start();
		ballGrabber.resetSetPoint();
		shooter.motorShutOff();
		while (IsTest() && IsEnabled()){
			lcd->Clear();
			tester.PerformTesting(&gamePad, &driveDistanceRight, lcd, &rightJoyStick, &leftJoyStick,
								  &testSwitch, &testTalons, &frontUltrasonic, &backUltrasonic,
								  &ballGrabber.ballDetector, &analogTestSwitch,
								  &shooter, &ballGrabber
								  );
			lcd->UpdateLCD();
			Wait(0.1);
			
		}
		driveDistanceRight.Stop();
		}

	void DoAutonomousMoveStep(const step_speed *speeds, char * message)
	{
		leftDriveEncoder.Reset();
		double dist = speeds[0].distance;
		double reading;
		
		// Start moving the robot
		leftDriveMotor.Set(speeds->speed_left);
		rightDriveMotor.Set(speeds->speed_right);

		reading = absolute(leftDriveEncoder.GetDistance());
		
		while (dist > reading)
		{
			Wait(0.02);
			reading = absolute(leftDriveEncoder.GetDistance());
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "D: %5.0f R : %5.0f", dist, reading);
			dsLCD->UpdateLCD();
		}
		
		leftDriveMotor.Set(0.0);
		rightDriveMotor.Set(0.0);
	}

	void Autonomous()
	{
		Timer timer;
		float power = 0;
		bool isLifting = false;
		bool isGrabbing = false;
		double liftHeight = Constants::liftBoxHeight-Constants::liftBoxLip;
		double grabPower = Constants::grabAutoCurrent;
		bool backOut;

		uint8_t toSend[1];//array of bytes to send over I2C
		uint8_t toReceive[0];//array of bytes to receive over I2C
		uint8_t numToSend = 1;//number of bytes to send
		uint8_t numToReceive = 0;//number of bytes to receive
		toSend[0] = 2;//set the byte to send to 1
		i2c.Transaction(toSend, numToSend, toReceive, numToReceive);//send over I2C

		bool isSettingUp = true;

		//pickup.setGrabber(-1); //open grabber all the way
		pickup.setLifter(0.8);

		while (isSettingUp && IsEnabled() && IsAutonomous()) {
			isSettingUp = false;
			/*if (grabOuterLimit.Get() == false) {
				pickup.setGrabber(0); //open until limit
			}
			else {
				isSettingUp = true;
			}*/

			if (liftLowerLimit.Get()) {
				pickup.setLifter(0); //down till bottom
			}
			else {
				isSettingUp = true;
			}
		}

		gyro.Reset();
		liftEncoder.Reset();
		grabEncoder.Reset();

		if (grabStick.GetZ() > .8) {
			timer.Reset();
			timer.Start();
			while (timer.Get() < 1) {
				robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle());	// drive back
				if(power>-.4){
					power-=0.005;
					Wait(.005);
				}
			}
			robotDrive.MecanumDrive_Cartesian(0, 0, 0, gyro.GetAngle());	// STOP!!!
			timer.Stop();
			timer.Reset();
			Wait(1);
		}
		power = 0;

		while (isLifting && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		backOut = Constants::autoBackOut;
		pickup.grabberGrab(isGrabbing, grabPower, backOut, grabStick);
		Wait(.005);

		while (isGrabbing && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		liftHeight = 3*Constants::liftBoxHeight;
		Wait(.005);
		pickup.lifterPosition(liftHeight, isLifting, grabStick);
		Wait(.005);
		while (isLifting && IsEnabled() && IsAutonomous()) {
			Wait(.005);
		}

		while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12 < 2 && IsEnabled() && IsAutonomous());	// while the nearest object is closer than 2 feet

		timer.Start();

		while(prox.GetVoltage() * Constants::ultrasonicVoltageToInches  < Constants::autoBackupDistance && timer.Get() < Constants::autoMaxDriveTime && IsEnabled() && IsAutonomous()) {	// while the nearest object is further than 12 feet
			if (power < .45) { //ramp up the power slowly
				power += .00375;
			}
			robotDrive.MecanumDrive_Cartesian(0, power, 0, gyro.GetAngle());	// drive back
			float distance = prox.GetVoltage() * Constants::ultrasonicVoltageToInches / 12;	// distance from ultrasonic sensor
			SmartDashboard::PutNumber("Distance", distance);	// write stuff to smart dash
			SmartDashboard::PutNumber("Drive Front Left Current", pdp.GetCurrent(Constants::driveFrontLeftPin));
			SmartDashboard::PutNumber("Drive Front Right Current", pdp.GetCurrent(Constants::driveFrontRightPin));
			SmartDashboard::PutNumber("Drive Rear Left Current", pdp.GetCurrent(Constants::driveRearLeftPin));
			SmartDashboard::PutNumber("Drive Rear Right Current", pdp.GetCurrent(Constants::driveRearRightPin));
			SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle());
			SmartDashboard::PutNumber("Distance (in)", prox.GetVoltage() * Constants:: ultrasonicVoltageToInches);

			Wait(.005);
		}
//.........这里部分代码省略.........