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

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

	float RawControl::averageEncoders()
	{
		float traveled;
		traveled=(((abs(wheelEncoderFR->Get()))+(abs(wheelEncoderFL->Get()))+(abs(wheelEncoderBR->Get()))+(abs(wheelEncoderBL->Get())))/4);
		traveled/=250;
		traveled=traveled * 8 * 3.14;	
		traveled=fabs(traveled);
		return traveled;
	}

	//void StartAutomaticCapture(std::shared_ptr<USBCamera>cam0);
	void TeleopPeriodic() {
//		ui.GetData(&wui);
//		m_tank.Drive(wui.LeftSpeed, wui.RightSpeed);
//
//		m_shooter.Rotate(wui.RotateSpeed*3); //70 degrees per second at full value
//		m_shooter.Lift(wui.LiftSpeed*1.193); //4 seconds for 180 degree revolution
//		if(wui.SpinUp) {
//			m_shooter.Spinup(1);
//		}
//		if(wui.Shoot) {
//			m_shooter.Shoot();
//		}
//		if(wui.Pickup) {
//			m_shooter.Pickup();
//		}
//
//		m_suspension.SetFrontLeft(wui.DropFL);
//		m_suspension.SetBackLeft(wui.DropBL);
//		m_suspension.SetFrontRight(wui.DropFR);
//		m_suspension.SetBackRight(wui.DropBR);

//		m_leddar.GetDetections();
//		m_shooter.Update();
		//float RTrigger = m_lStick->GetRawAxis(3);
		//float LTrigger = m_lStick->GetRawAxis(2);

		//if (m_PWMTalonLeftFrontTop == .5)
		//if (abs(RTrigger) < 0.2)
			//RTrigger = 0;
		//if (abs(LTrigger) < 0.2)
			//LTrigger = 0;
		float leftSpeed = m_lStick->GetRawAxis(1);
		float rightSpeed = m_lStick->GetRawAxis(5);
		if (abs(leftSpeed) < 0.2)
			leftSpeed = 0;
		if (abs(rightSpeed) < 0.2)
			rightSpeed = 0;
		//float LTrigger = m_lStick->GetRawAxis(3);
		//float RTrigger = m_lStick->GetRawAxis(2);
		SmartDashboard::PutNumber("Left Stick", leftSpeed);
		SmartDashboard::PutNumber("Right Stick", rightSpeed);
		//SmartDashboard::PutNumber("L Trigger", LTrigger);
		//SmartDashboard::PutNumber("R Trigger", RTrigger);
		SmartDashboard::PutNumber("Left Encoder", leftEncoder->Get());
		SmartDashboard::PutNumber("Right Encoder", rightEncoder->Get());
		drive->TankDrive(leftSpeed, rightSpeed, true);
		//drive->TankDrive(RTrigger, LTrigger, true);
		LEFTDRIVE1->Set(leftSpeed);
		LEFTDRIVE2->Set(leftSpeed);
		RIGHTDRIVE1->Set(rightSpeed);
		RIGHTDRIVE2->Set(rightSpeed);
		//m_PWMTalonLeftFrontTop->Set(RTrigger);
		//m_PWMTalonRightFrontTop->Set(RTrigger);
		//m_PWMTalonRightRearTop->Set(LTrigger);
		//m_PWMTalonLeftRearTop->Set(LTrigger);
	}

// Called repeatedly when this Command is scheduled to run
void RPMHoldingCommand::Execute() {
	double countsPerRevolution = 360;

	double currentTime = timer->Get();
	double timeDifference = currentTime - lastTime;
	lastTime = currentTime;
	
	Encoder* motorEncoder = prototypingsubsystem->GetMotorEncoder();
	int currentCount = motorEncoder->Get();
	int countDifference = currentCount - lastCount;
	double rawRPM = countDifference * (60.0 / countsPerRevolution) / timeDifference;
	double rpm = GetRPM(rawRPM);
	printf("rawRPM %f rpm %f\n", rawRPM, rpm);
	rpmSource->inputRPM(rpm);
	//rpmSource->inputRPM(rpm);
	
	printf("setpoint %f RPM %f result %f error %f \n", controller->GetSetpoint(), rpm, controller->Get(), controller->GetError());
	
	SmartDashboard *sd = oi->getSmartDashboard();

	oi->DisplayEncoder(motorEncoder);
	oi->DisplayPIDController(controller);
	
	sd->PutDouble("Count Difference", countDifference);
	sd->PutDouble("Current count", currentCount);
	sd->PutDouble("Time difference", timeDifference);
	sd->PutDouble("Calculated RPM", rpm);
	sd->PutDouble("Motor speed", prototypingsubsystem->GetMotor()->Get());
	
	sd->PutDouble("Counts per revolution", countsPerRevolution);
	
	lastCount = currentCount;

}

/**
 * Gets the current count.
 * Returns the current count on the Encoder.
 * This method compensates for the decoding type.
 * 
 * @deprecated Use GetEncoderDistance() in favor of this method.  This returns unscaled pulses and GetDistance() scales using value from SetEncoderDistancePerPulse().
 *
 * @return Current count from the Encoder.
 * 
 * @param aSlot The digital module slot for the A Channel on the encoder
 * @param aChannel The channel on the digital module for the A Channel of the encoder
 * @param bSlot The digital module slot for the B Channel on the encoder
 * @param bChannel The channel on the digital module for the B Channel of the encoder
 */
INT32 GetEncoder(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 bChannel)
{
	Encoder *encoder = AllocateEncoder(aSlot, aChannel, bSlot, bChannel);
	if (encoder != NULL)
		return encoder->Get();
	else
		return 0;
}

	void TeleopPeriodic()
	{
		drive_mode_t new_mode = drive_mode_chooser.GetSelected();
		SmartDashboard::PutString("current mode", new_mode == TANK_DRIVE ? "Tank" : "Arcade");
		if (new_mode != drive_mode)
			SetDriveMode(new_mode);
		if (drive_mode == TANK_DRIVE) {
			left_speed = accel(left_speed, pilot->LeftY(), TICKS_TO_ACCEL);
			right_speed = accel(right_speed, pilot->RightY(), TICKS_TO_ACCEL);
			drive->TankDrive(left_speed, right_speed);
		}
		else {
			rot_speed = accel(rot_speed, pilot->RightX(), TICKS_TO_ACCEL);
			SmartDashboard::PutNumber("rotation speed", rot_speed);
			rot_speed = pilot->RightX();
			move_speed = accel(move_speed, pilot->LeftY(), TICKS_TO_ACCEL);
			drive->ArcadeDrive(move_speed * MOVE_SPEED_LIMIT, -rot_speed * MOVE_SPEED_LIMIT, false);
		}
		SmartDashboard::PutBoolean("clamp open", clamp->isOpen());
		SmartDashboard::PutBoolean("sword in", clamp->isSwordIn());

		SmartDashboard::PutData("gyro", gyro);

//		for (uint8 i = 0; i <= 15; ++i)
//			SmartDashboard::PutNumber(std::string("current #") + std::to_string(i), pdp->GetCurrent(i));
		SmartDashboard::PutNumber("Current", pdp->GetTotalCurrent());

		if (pilot->ButtonState(GamepadF310::BUTTON_A)) {
			clamp->open();
		}
		else if (pilot->ButtonState(GamepadF310::BUTTON_B)){
			clamp->close();
		}

		clamp->update();

		SmartDashboard::PutNumber("accelerometer Z", acceler->GetZ());

		SmartDashboard::PutNumber("Encoder", encoder->Get());

		flywheel->Set(pilot->RightTrigger());

		if (pilot->LeftTrigger() != 0)
			flywheel->Set(-pilot->LeftTrigger());


		SmartDashboard::PutNumber("Left Trigger:", pilot->LeftTrigger());

		if (pilot->ButtonState(GamepadF310::BUTTON_X)) {
			cameraFeeds-> changeCam(cameraFeeds->kBtCamFront);
		}
		if (pilot->ButtonState(GamepadF310::BUTTON_Y)){
			cameraFeeds-> changeCam(cameraFeeds->kBtCamBack);
		}

		cameraFeeds->run();

	}

	void Claw(double joy) {
		if(joy < 10) joy = 10;
		if(joy > 230) joy = 230;
		int location = EncClaw.Get();
		double speed = PIDClaw(joy, location);
		//if(location < 15) speed *= .2;
		if(speed > .32) speed = .32;
		if(speed < -.32) speed = -.32;
		if(speed < .1 && speed > -.1) speed = 0;
		arm2->Set(speed,2);
	}

inline void lifterPositionTaskFunc(uint32_t joystickPtr, uint32_t liftTalonPtr, uint32_t liftEncoderPtr, uint32_t liftUpperLimitPtr, uint32_t liftLowerLimitPtr, uint32_t pdpPtr, uint32_t heightPtr, uint32_t isLiftingPtr ...) {//uint is a pointer and not an integer
	double *height = (double *) heightPtr;//initializes double
	Joystick *joystick = (Joystick *) joystickPtr;
	Talon *liftTalon = (Talon *) liftTalonPtr;
	Encoder *liftEncoder = (Encoder *) liftEncoderPtr;
	Switch *liftLowerLimit = (Switch *) liftLowerLimitPtr;
	Switch *liftUpperLimit = (Switch *) liftUpperLimitPtr;
	PowerDistributionPanel *pdp = (PowerDistributionPanel *) pdpPtr;
	bool *isLifting = (bool *) isLiftingPtr;

	*isLifting = true;//tells robot.cpp that thread is running

	if (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height) {//checks to see if encoder is higher than it's supposed to be
		if (liftLowerLimit->Get() == false) {//starts to spin motor to pass startup current
			liftTalon->Set(1);//move down
			Wait(Constants::liftDelay);
		}
		while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) > *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftLowerLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too high and hasn't hit a limit switch or been cancelled
			SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder in SmartDashboard
			liftTalon->Set(.7);//move down
		}
	}
	else {
		if (liftUpperLimit->Get() == false) {//starts to spin motor to pass startup current
			liftTalon->Set(-1);//move up
			Wait(Constants::liftDelay);
		}
		while (Constants::encoderToDistance(liftEncoder->Get(),Constants::liftEncoderTicks, Constants::liftEncoderBase, Constants::liftEncoderRadius) < *height && pdp->GetCurrent(Constants::liftPdpChannel) < Constants::liftCurrent && liftUpperLimit->Get() == false && joystick->GetRawButton(Constants::pickupCancelButton) == false) {//while it is too low and hasn't hit a limit switch or been cancelled
			SmartDashboard::PutNumber("Pretend Encoder",liftEncoder->Get());//displays number of ticks of encoder on SmartDashboard
			liftTalon->Set(-1);//move up
		}
	}

	liftTalon->Set(0);//stop
	*isLifting = false;//tells robot.cpp that thread is finished
}

inline void lifterBrakeTaskFunc(uint32_t liftTalonPtr, uint32_t liftEncoderPtr, uint32_t liftUpperLimitPtr, uint32_t liftLowerLimitPtr, uint32_t isBrakingPtr ...) {//uint is a pointer and not an integer
	Talon *liftTalon = (Talon *) liftTalonPtr;
	Encoder *liftEncoder = (Encoder *) liftEncoderPtr;
	Switch *liftLowerLimit = (Switch *) liftLowerLimitPtr;
	Switch *liftUpperLimit = (Switch *) liftUpperLimitPtr;
	bool *isBraking = (bool *) isBrakingPtr;
	PIDController pid(Constants::liftBrakeP, Constants::liftBrakeI, Constants::liftBrakeD, liftEncoder, liftTalon);
	Timer timer;

	//TODO enable and test out brake
	if (!Constants::liftBrakeIsEnabled) {
		return;
	}

	timer.Start();

	while (timer.Get() < Constants::liftBrakeUpTime) {
		liftTalon->Set(Constants::liftBrakeUpPower);
	}

	liftTalon->Set(0);

	pid.Enable();
	pid.SetSetpoint(liftEncoder->Get());

	while (*isBraking) {
		if ((pid.Get() > 0 && liftLowerLimit->Get()) || (pid.Get() < 0 && liftUpperLimit->Get())){
			liftTalon->Set(0);
		}
		else {
			liftTalon->Set(pid.Get());
		}
		SmartDashboard::PutBoolean("Braking", true);


	}

	pid.Disable();
	SmartDashboard::PutBoolean("Braking", false);
	liftTalon->Set(0);
}

	void Arm(double joy) {
		int location = EncArm.Get();
		/*
		if(location < 10 && joy < 0) joy = 0;
		if(location > 110 && joy > 0) joy = 0;
		arm1->Set(joy);
		arm1_sec->Set(joy);
		return;
		*/
		
		if(joy < -10) joy = -10;
		if(joy > 110) joy = 110;
		
		double speed = PIDArm(joy, location);
		if(speed > .5) speed = .5;
		if(speed < -.3) speed = -.3;
		if(speed < 0 && location < 10) speed = 0;
		if(speed > 0 && location > 110) speed = 0;
		speed = LowArm(speed);
		if(speed < .01 && speed > -.01) speed = 0;
		arm1->Set(speed,3);
		arm1_sec->Set(speed,3);
		
	}

void RobotDemo::RPS_control_code(float desired_RPS)
// THE 360 COUNT PER REVOLUTION ENCODERS ARE GOOD FOR 10K RPM, BUT THE CIM motor MAXIMUM SPEED IS APPROXIMATELY 5K.
{
#if 1
	/* If the difference between the old and new RPS (typ ~35) by greater than 1 either way,
	 * reset the ingegral terms to prevent stale values from affecting our PID calculations
	 */
	
	if (desired_RPS != prev_desired_RPS)
	{
		if (fabs(prev_desired_RPS - desired_RPS) >= 1.0)
		{
			integral_back = 0;
			integral_front = 0;
		}
		cout << desired_RPS << " " << prev_desired_RPS << endl;
		prev_desired_RPS = desired_RPS;
	}
#endif
	//motor = new Victor(motor_pwm);
	//test_encoder = new Encoder(encoder_pwm1, encoder_pwm2, true);
	if ((pid_code_timer->Get()) >= pidtime)
	{
		float actual_time = pid_code_timer ->Get();
		prev_error_back = error_back;
		prev_error_front = error_front;
		RPS_back = (back_shooter_encoder->Get() / 360.0) / actual_time;
		RPS_front = (front_shooter_encoder->Get() / 360.0) / actual_time;
		error_back = desired_RPS - RPS_back;
		error_front = desired_RPS - RPS_front;
		integral_back = integral_back + (error_back * actual_time);
		integral_front = integral_front + (error_front * actual_time);
		derivitive_back = (error_back - prev_error_back) / actual_time;
		derivitive_front = (error_front - prev_error_front) / actual_time;

		//take error and set it to motors
		{
			RPS_speed_back = (error_back * first_pterm) + (integral_back
					* iterm) + (derivitive_back * dterm);
			RPS_speed_front = (error_front * first_pterm) + (integral_front
					* iterm) + (derivitive_front * dterm);
		}

		if (RPS_speed_back > 1)
		{
			RPS_speed_back = 1;
		}
		if (RPS_speed_front > 1)
		{
			RPS_speed_front = 1;
		}
		if (RPS_speed_back < 0)
		{
			RPS_speed_back = 0;
		}
		if (RPS_speed_front < 0)
		{
			RPS_speed_front = 0;
		}
		//cout << RPS_speed << endl;
		//cout << autonomous_timer->Get() << "   RPS_back: " << RPS_back

		//<< "   RPS_front: " << RPS_front << "   Desired RPS:"
		//<< autonomous_desired_RPS << endl;
		shooter_motor_back ->Set(RPS_speed_back);
		shooter_motor_front ->Set(RPS_speed_front);
		counter++;
#if 0
		if (counter == 5)
		{
			counter = 1;
			printf(
					"                                         %i)%f   %f   %f   %f\n",
					number, RPS, RPS_speed, error,
					shooter_motor_back->Get());
			number++;
		}
		else

		{
			printf("%f   %f   %f   %f\n", RPS, RPS_speed, error,
					shooter_motor_back->Get());
		}
#endif
		pid_code_timer->Reset();
		back_shooter_encoder ->Reset();
		front_shooter_encoder ->Reset();
	}
}

	void TeleopPeriodic()
	{
		//camera->GetImage(frame);
		//imaqDrawShapeOnImage(frame, frame, { 10, 10, 100, 100 }, DrawMode::IMAQ_DRAW_VALUE, ShapeMode::IMAQ_SHAPE_OVAL, 0.0f);
		//CameraServer::GetInstance()->SetImage(frame);


		printf("Left Encoder: %i, Right Encoder: %i, Gyro: %f\n", leftEnc->Get(), rightEnc->Get(), gyro->GetAngle());

		drive->ArcadeDrive(driveStick);
		drive->SetMaxOutput((1-driveStick->GetThrottle())/2);
		//printf("%f\n", (1-stick->GetThrottle())/2);
		//leftMotor->Set(0.1);
		//rightMotor->Set(0.1);

		if (shootStick->GetRawAxis(3) > 0.5) {
			launch1->Set(1.0);
			launch2->Set(1.0);
		} else if (shootStick->GetRawAxis(2) > 0.5) {
			printf("Power Counter: %i\n", powerCounter);
			if (powerCounter < POWER_MAX) {
				powerCounter++;
				launch1->Set(-0.8);
				launch2->Set(-0.8);
			} else {
				launch1->Set(-0.6);
				launch2->Set(-0.6);
			}
		} else {
			launch1->Set(0.0);
			launch2->Set(0.0);
			powerCounter = 0.0;
		}

		//use this button to spin only one winch, to lift up.
		 if (shootStick->GetRawButton(7)) {
		 	otherWinch->Set(0.5);
		 } else if (shootStick->GetRawButton(8)) {
			 otherWinch->Set(-0.5);
		 } else {
		 	otherWinch->Set(0.0);
		 }

		if (shootStick->GetRawButton(5)) {
			winch->Set(-0.7);

			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//				otherWinch->Set(-0.5);
			}
		} else if (shootStick->GetRawButton(6)) {
			winch->Set(0.7);
			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//				otherWinch->Set(0.5);
			}
		} else {
			winch->Set(0.0);
			if (!shootStick->GetRawButton(7) && !shootStick->GetRawButton(8)) {
//,				otherWinch->Set(0.0);
			}
		}
		

		if (shootStick->GetRawButton(1)) {
			launchPiston->Set(1);
		} else {
			launchPiston->Set(0);
		}

		if (shootStick->GetRawButton(3)) {
			Autonomous::alignWithGoal(drive, launch1, launch2, winch, otherWinch, table, timer);
		}

		if (shootStick->GetRawButton(3) && debounce == false) {
			debounce = true;
			if (defenseUp) {
				defensePiston->Set(DoubleSolenoid::Value::kReverse);
				defenseUp = false;
			} else {
				defenseUp =true;
				defensePiston->Set(DoubleSolenoid::Value::kForward);
			}
		} else if (!shootStick->GetRawButton(3)){
			debounce = false;
		}
	}

	void OperatorControl(void)
	{
		AxisCamera &camera = AxisCamera::GetInstance();
		miniBotTime.Start();
		initRobot();
		
		debug("in telop");
		compressor.Start();
		GetWatchdog().SetEnabled(true);
		/*int l1, l2, l3;
		while (IsOperatorControl()) {
			GetWatchdog().Feed();
			//char val = (line1.Get() & 0x01) | (line2.Get() & 0x02) | (line3.Get() & 0x04);
			//if(l1 != line1.Get() || l2 != line2.Get() || l3 != line3.Get()) {
			//	cerr << "change " << (l1 = line1.Get()) << "\t" << (l2 = line2.Get()) << "\t" << (l3 = line3.Get()) << endl;
			//}
			cerr << "Change "<< line1.Get() <<"\t" << line2.Get() << "\t" << line3.Get() << endl;
			Wait(0.2);
		}*/
		char count=0, pneumaticCount=0;
		// was .125 when loop at .025
		lowPass lowSpeed(.04), lowStrafe(.04), lowTurn(.04), lowClaw(.04), lowArm(.04), lowArmLoc(.05);
		
		double ClawLocation=0, ArmLocation=0, OldArmLocation=0;
		
		while (IsOperatorControl() && !IsDisabled())
		{
			GetWatchdog().Feed();
			float speed = -1*stick.GetRawAxis(2);
			float strafe = stick.GetRawAxis(1);
			float turn = stick.GetRawAxis(3);

			if(!stick.GetRawButton(7)) {
				speed /= 2;
				strafe /= 2;
				turn /= 2;
			}
			if(stick.GetRawButton(8)) {
				speed /= 2;
				strafe /= 2;
				turn /= 2;
			}
			if(stick.GetRawButton(2)) {
				speed = 0;
				turn = 0;
			}
			
			Drive(lowSpeed(speed), lowTurn(turn), lowStrafe(strafe));		
			
			
			
#ifndef NDEBUG
			if(stick2.GetRawButton(10)) {
				robotInted = false;
				initRobot();
			}
#endif
			
			
			if(stick2.GetRawButton(7) && (miniBotTime.Get() >= 110 || (stick2.GetRawButton(9) && stick2.GetRawButton(10)))) { // launcher
				// the quick launcher
				MiniBot1a.Set(true);
				MiniBot1b.Set(false);
				
			} 
			if(!stick2.GetRawButton(10) && stick2.GetRawButton(9)) { // deploy in
				MiniBot2a.Set(false);
				MiniBot2b.Set(true);
				//MiniBot2a.Set(false);
				//MiniBot2b.Set(true);
			}
			if(stick2.GetRawButton(5)) { // top deploy out
				MiniBot2a.Set(true);
				MiniBot2b.Set(false);
			}
			
			
			if(stick2.GetRawButton(6)) { // open
				ClawOpen.Set(true);
				ClawClose.Set(false);
			}
			if(stick2.GetRawButton(8)) { // closed
				ClawOpen.Set(false);
				ClawClose.Set(true);
				ClawLocation += 2;
			}
			
			/*156 straight
			 * 56 90 angle
			 * 10 back
			 */
			
			if(stick2.GetRawButton(1)) { // top peg
				ClawLocation = 156;
				ArmLocation = 105;
			}
			if(stick2.GetRawButton(2)) {
				ClawLocation = 111; // the 90angle / middle peg
				ArmLocation = 50;
			}
//.........这里部分代码省略.........

	/**
	 * Runs the motors with Mecanum drive.
	 */
	void OperatorControl()//teleop code
	{
		robotDrive.SetSafetyEnabled(false);
		gyro.Reset();
		grabEncoder.Reset();
		timer.Start();
		timer.Reset();
		double liftHeight = 0; //variable for lifting thread
		int liftHeightBoxes = 0; //another variable for lifting thread
		int liftStep = 0; //height of step in inches
		int liftRamp = 0; //height of ramp in inches
		double grabPower;
		bool backOut;
		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] = 1;//set the byte to send to 1
		i2c.Transaction(toSend, 1, toReceive, 0);//send over I2C
		bool isGrabbing = false;//whether or not grabbing thread is running
		bool isLifting = false;//whether or not lifting thread is running
		bool isBraking = false;//whether or not braking thread is running
		float driveX = 0;
		float driveY = 0;
		float driveZ = 0;
		float driveGyro = 0;
		bool liftLastState = false;
		bool liftState = false; //button pressed
		double liftLastTime = 0;
		double liftTime = 0;
		bool liftRan = true;
		Timer switchTimer;
		Timer grabTimer;
		switchTimer.Start();
		grabTimer.Start();


		while (IsOperatorControl() && IsEnabled())
		{
			// Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation.
			// This sample does not use field-oriented drive, so the gyro input is set to zero.

			toSend[0] = 1;
			numToSend = 1;


			driveX = driveStick.GetRawAxis(Constants::driveXAxis);//starts driving code
			driveY = driveStick.GetRawAxis(Constants::driveYAxis);
			driveZ = driveStick.GetRawAxis(Constants::driveZAxis);
			driveGyro = gyro.GetAngle() + Constants::driveGyroTeleopOffset;


			if (driveStick.GetRawButton(Constants::driveOneAxisButton)) {//if X is greater than Y and Z, then it will only go in the direction of X
				toSend[0] = 6;
				numToSend = 1;

				if (fabs(driveX) > fabs(driveY) && fabs(driveX) > fabs(driveZ)) {
					driveY = 0;
					driveZ = 0;
				}
				else if (fabs(driveY) > fabs(driveX) && fabs(driveY) > fabs(driveZ)) {//if Y is greater than X and Z, then it will only go in the direction of Y
					driveX = 0;
					driveZ = 0;
				}
				else {//if Z is greater than X and Y, then it will only go in the direction of Z
					driveX = 0;
					driveY = 0;
				}
			}

			if (driveStick.GetRawButton(Constants::driveXYButton)) {//Z lock; only lets X an Y function
				toSend[0] = 7;
				driveZ = 0;//Stops Z while Z lock is pressed
			}

			if (!driveStick.GetRawButton(Constants::driveFieldLockButton)) {//robot moves based on the orientation of the field
				driveGyro = 0;//gyro stops while field lock is enabled
			}

			driveX = Constants::scaleJoysticks(driveX, Constants::driveXDeadZone, Constants::driveXMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveXDegree);
			driveY = Constants::scaleJoysticks(driveY, Constants::driveYDeadZone, Constants::driveYMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveYDegree);
			driveZ = Constants::scaleJoysticks(driveZ, Constants::driveZDeadZone, Constants::driveZMax * (.5 - (driveStick.GetRawAxis(Constants::driveThrottleAxis) / 2)), Constants::driveZDegree);
			robotDrive.MecanumDrive_Cartesian(driveX, driveY, driveZ, driveGyro);//makes the robot drive




			if (pdp.GetCurrent(Constants::grabPdpChannel) < Constants::grabManualCurrent) {
				pickup.setGrabber(Constants::scaleJoysticks(grabStick.GetX(), Constants::grabDeadZone, Constants::grabMax, Constants::grabDegree)); //defines the grabber
				if(grabTimer.Get() < 1) {
					toSend[0] = 6;
				}
			}
			else {
				pickup.setGrabber(0);
				grabTimer.Reset();
				toSend[0] = 6;
//.........这里部分代码省略.........