C++ AnalogChannel::GetVoltage方法代码示例

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void) {

		while (IsOperatorControl()) {
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Voltage: %f",
					signal.GetVoltage());
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "CVoltage: %f",
					signalControlVoltage.GetVoltage());
			dsLCD->UpdateLCD();
			Wait(0.005); // wait for a motor update time
		}
	}

	void TeleopPeriodic() {
		if(m_driver->GetRawButton(BUTTON_LB)) {
			// PYRAMID
			m_PIDController->SetSetpoint(PLATE_PYRAMID_THREE_POINT);
			m_PIDController->Enable();
		} else if(m_driver->GetRawButton(BUTTON_RB)) {
			// FEEDER
			m_PIDController->SetSetpoint(PLATE_FEEDER_THREE_POINT);
			m_PIDController->Enable();
		} else if(m_driver->GetRawAxis(TRIGGERS) > 0.5) {
			m_PIDController->SetSetpoint(PLATE_TEN_POINT_CLIMB);
			m_PIDController->Enable();
		} else {
			// MANUAL CONTROL
			m_PIDController->Disable();
			
			m_plate1->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
			m_plate2->Set(-deadband(m_driver->GetRawAxis(LEFT_Y)));
		}
		
		// ----- PRINT -----
		SmartDashboard::PutNumber("Plate Position: ", m_plateSensor->GetVoltage());
		SmartDashboard::PutNumber("PID GET: ", m_plateSensor->PIDGet());
		
	} // TeleopPeriodic()

	void HandleArmInputs(void)
	{
		if (gamepad.GetLeftY() < -0.1)
		{
			if (potentiometer.GetVoltage() < 4.5)
			{
				armMotor.Set(1.0);
			}
			else
			{
				armMotor.Set(0.0);
			}
		}
		else if (gamepad.GetLeftY() > 0.1)
		{
			if (potentiometer.GetVoltage() > .5)
			{
				armMotor.Set(-1.0);
			}
			else
			{
				armMotor.Set(0.0);
			}	
		}
		else
		{
			armMotor.Set(0.0);
		}
		
		if (gamepad.GetEvent(BUTTON_CLAW_1_LOCKED) == kEventClosed)
		{
			greenClaw.Set(DoubleSolenoid::kForward);
		}
		else if (gamepad.GetEvent(BUTTON_CLAW_1_UNLOCKED) == kEventClosed)
		{
			greenClaw.Set(DoubleSolenoid::kReverse);
		}
		else if (gamepad.GetEvent(BUTTON_CLAW_2_LOCKED) == kEventClosed)
		{
			yellowClaw.Set(DoubleSolenoid::kForward);
		}
		else if (gamepad.GetEvent(BUTTON_CLAW_2_UNLOCKED) == kEventClosed)
		{
			yellowClaw.Set(DoubleSolenoid::kReverse);
		}
	}

/**
 * Get a scaled sample straight from this channel on the module.
 * The value is scaled to units of Volts using the calibrated scaling data from GetLSBWeight() and GetOffset().
 * @param slot The slot the analog module is plugged into
 * @param channel The channel in the module assicated with this analog channel
 * @return A scaled sample straight from this channel on the module.
 */
float GetAnalogVoltage(UINT32 slot, UINT32 channel)
{
	AnalogChannel *analog = AllocateAnalogChannel(slot, channel);
	if (analog != NULL)
	{
		return analog->GetVoltage();
	}
	return 0.0;
}

	/**
	 * Runs the motors with arcade steering.
	 */
	void OperatorControl(void)
	{
       encoder.Start();
		while (IsEnabled())
		{
			float controlv = control.GetVoltage();
			motor.Set(controlv/5); //get's voltage from control and divides it by 5
			double rate = encoder.GetRate();
			double distance = encoder.GetDistance();
			printf ("%f %f \n", rate, distance);

		}
	}

	void Autonomous(void)
	{
		myRobot.SetSafetyEnabled(false);
		
		// Move the arm to a level position
		armMotor.Set(ARM_FWD);
		while (!((potentiometer.GetVoltage() > 2.4) && (potentiometer.GetVoltage() < 2.6))) 
		{
			// Check to make sure we don't overrotate the arm in either direction
			if (potentiometer.GetVoltage() > 4.5 || potentiometer.GetVoltage() < 0.5)
			{
				armMotor.Set(0.0);
				return;
			}
			Wait(0.2);
			UpdateStatusDisplays();
		}
		armMotor.Set(0.0);
		
		// Drive robot 
		
		DoAutonomousMoveStep(&m_autoForward[0], "Moving...");
		DoAutonomousMoveStep(&m_autoForward[1], "Moving...");
		
		//Shoot for five seconds
		Timer* t = new Timer();
		t->Start();
		
		shooterMotor.Set(SHOOTER_FWD);
		indexerMotor.Set(INDEXER_FWD);

		while(!t->HasPeriodPassed(5.0))
		{
			Wait(0.02);
		}
		shooterMotor.Set(0.0);
		indexerMotor.Set(0.0);
	}

	void UpdateStatusDisplays(void)
	{		
		// Joystick values
		SmartDashboard::PutNumber("stickX", stick.GetX());
		SmartDashboard::PutNumber("stickY", stick.GetY());
		SmartDashboard::PutBoolean("shift", stick2.GetState(BUTTON_SHIFT) ? kStateClosed : kStateOpen);
		
		// Shooter/Indexer values
		SmartDashboard::PutBoolean("indexSwitch", indexSwitch.GetState() ? kStateClosed : kStateOpen);
		SmartDashboard::PutNumber("shooterMotor", shooterMotor.Get());
		SmartDashboard::PutNumber("indexerMotor", indexerMotor.Get());

		// Misc Motor Values
		SmartDashboard::PutNumber("collectorMotor", collectorMotor.Get());
		SmartDashboard::PutNumber("armMotor", armMotor.Get());

		// Arm position via potentiometer voltage (2.5 volts is center position)
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line3, "Voltage: %3.1f", potentiometer.GetVoltage());
		SmartDashboard::PutNumber("Potentiometer", potentiometer.GetVoltage());
		
		// Claw lock states
		SmartDashboard::PutBoolean("Green Claw State", greenClawLockSwitch.GetState());
		SmartDashboard::PutBoolean("Yellow Claw State", yellowClawLockSwitch.GetState());
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line4, "Green : %s", 
			greenClawLockSwitch.GetState() ? "Locked" : "Unlocked");
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line5, "Yellow: %s", 
			yellowClawLockSwitch.GetState() ? "Locked" : "Unlocked");
		
		// Pneumatic shifter count
		SmartDashboard::PutNumber("Shift Count", m_shiftCount);
		
		// State viariables
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line6, "CMR: %s SMR: %s JTR: %s",
			m_collectorMotorRunning ? "T" : "F",
			m_shooterMotorRunning ? "T" : "F",   
			m_jogTimerRunning ? "T" : "F");
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		log->Info("TELEOP START");
		
//		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			double distance = dist.GetVoltage() / (5.0 / 512.0);
			SmartDashboard::Log(distance, "Rangefinder distance");
			SmartDashboard::Log(dist.GetVoltage(), "Rangefinder voltage");
			
			Sol1.Set(stick1.GetRawButton(1));
			Sol2.Set(stick1.GetRawButton(2));
			Sol3.Set(stick1.GetRawButton(3));
			Sol4.Set(stick1.GetRawButton(4));
			Sol5.Set(stick1.GetRawButton(5));
			
			if (stick1.GetRawButton(6)) {
				if (!lastButton) log->Shot(stick1.GetZ(), stick2.GetZ());
				lastButton = true;
			} else {
				lastButton = false;
			}
			
			SmartDashboard::Log(((stick1.GetZ() + 1) / 2) * 2500, "Distance");
			SmartDashboard::Log(((stick2.GetZ() + 1) / 2) * 5.0, "Compression");
			SmartDashboard::Log(LookUp(stick1.GetZ() * 2500, /* stick2.GetZ() * 5.0 */ dist.GetVoltage()), "Shooter Speed");
			
//			myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
//			lJagA.Set(stick1.GetY());
//			lJagB.Set(stick1.GetY());
//			rJagA.Set(stick2.GetY());
//			rJagB.Set(stick2.GetY());
			Wait(0.010);				// wait for a motor update time
		}
	}

	void OperatorControl(void)
	{
		myRobot.SetSafetyEnabled(true);
		
		gamepad.EnableButton(BUTTON_COLLECTOR_FWD);
		gamepad.EnableButton(BUTTON_COLLECTOR_REV);
		gamepad.EnableButton(BUTTON_SHOOTER);
		gamepad.EnableButton(BUTTON_CLAW_1_LOCKED);
		gamepad.EnableButton(BUTTON_CLAW_2_LOCKED);
		gamepad.EnableButton(BUTTON_CLAW_1_UNLOCKED);
		gamepad.EnableButton(BUTTON_CLAW_2_UNLOCKED);
		stick2.EnableButton(BUTTON_SHIFT);

		// Set inital states for all switches and buttons
		gamepad.Update();
		indexSwitch.Update();
		stick2.Update();
		
		// Set initial states for all pneumatic actuators
		shifter.Set(DoubleSolenoid::kReverse);
		greenClaw.Set(DoubleSolenoid::kReverse);
		yellowClaw.Set(DoubleSolenoid::kReverse);

		compressor.Start ();
		
		while (IsOperatorControl())
		{
			gamepad.Update();
			stick2.Update();
			indexSwitch.Update();
			
			HandleCollectorInputs();
			HandleDriverInputsManual();
			HandleArmInputs();
			HandleShooterInputs();
			dsLCD->PrintfLine(DriverStationLCD::kUser_Line2, "Voltage: %f", potentiometer.GetVoltage());			dsLCD->UpdateLCD();
			Wait(0.005);				// wait for a motor update time
		}
	}

	// The class constructor, called once upon bootup to initialize variables
	RobotDemo(void)

	{
		/*
		 * these must be initialized in the same order
		 * as they are declared above.
		 */
		myRobot = new RobotDrive(1, 2); // drive using motors connected to PWM 1 and 2
		leftstick = new Joystick(1); // leftstick = joystick on usb port # 1
		rightstick = new Joystick(2);
		shoulderPotentiometerChannel = new AnalogChannel(1);

		left = new DigitalInput(1); // these DigitalInput instances are used to get 0 or 1 from light sensors
		middle = new DigitalInput(2); // connected to digital sidecar's channel 1, 2, and 3
		right = new DigitalInput(3);

		shoulderMotor = new RobotDrive (3, 10); //drive using motor connected to PWM 3 and the unused PWM 10
		armMotor = new RobotDrive (5, 9); //drive using motor connected to PWM 5 and the unused PWM 9 
		gripperMotor = new Servo (4); //drive using motor connected to servo channel 1
										
		minibotDeployMotor = new Servo (6); //drive using motor connected to servo channel 2
		minibotCloseMotor1 = new Servo (7); //drive using motor connected to servo channel 3
		minibotCloseMotor2 = new Servo (8); //drive using motor connected to servo channel 3


		ds = DriverStation::GetInstance();
		dsLCD = DriverStationLCD::GetInstance();

		//set the variables declared earlier below
		shoulderDestinationVoltage = 5-shoulderPotentiometerChannel->GetVoltage();

		GetWatchdog().SetExpiration(0.1);
		/*a watchdog is an "inbuilt" system that monitors your code to 
		 * ensure that everything is running the way it is supposed to.
		 * To do this, the watchdog must be "fed" every once in a while.
		 * If not, it will die, and disable the code*/
	}

	void HandleArmInputs(void)
	{
		if (!m_jogTimerRunning)
		{
			if (gamepad.GetLeftY() < -0.1)
			{
				if (potentiometer.GetVoltage() < 4.5)
				{
					armMotor.Set(ARM_FWD);
				}
				else
				{
					armMotor.Set(0.0);
				}
			}
			else if (gamepad.GetLeftY() > 0.1)
			{
				if (potentiometer.GetVoltage() > .5)
				{
					armMotor.Set(ARM_REV);
				}
				else
				{
					armMotor.Set(0.0);
				}	
			}
			else if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kUp))
			{
				armMotor.Set(ARM_FWD);
				jogTimer.Start();
				jogTimer.Reset();
				m_jogTimerRunning = true;
			}
			else if (kEventClosed == gamepad.GetDPadEvent(Gamepad::kDown))
			{
				armMotor.Set(ARM_REV);
				jogTimer.Start();
				jogTimer.Reset();
				m_jogTimerRunning = true;
			}
			else
			{
				armMotor.Set(0.0);
			}
		}
		else if (jogTimer.HasPeriodPassed(JOG_TIME))
		{
			armMotor.Set(0);
			jogTimer.Stop();
			jogTimer.Reset();
			m_jogTimerRunning = false;
		}

		if (gamepad.GetEvent(BUTTON_CLAW_1_LOCKED) == kEventClosed)
		{
			greenClaw.Set(DoubleSolenoid::kForward);
		}
		else if (gamepad.GetEvent(BUTTON_CLAW_1_UNLOCKED) == kEventClosed)
		{
			greenClaw.Set(DoubleSolenoid::kReverse);
		}
		else if (gamepad.GetEvent(BUTTON_CLAW_2_LOCKED) == kEventClosed)
		{
			yellowClaw.Set(DoubleSolenoid::kForward);
		}
		else if (gamepad.GetEvent(BUTTON_CLAW_2_UNLOCKED) == kEventClosed)
		{
			yellowClaw.Set(DoubleSolenoid::kReverse);
		}
		
	}

	/*
	 * OPERATOR CONTROL using a arcade style drive
	 */
	void OperatorControl(void) {

		GetWatchdog().SetEnabled(true);
		while (IsOperatorControl()) //This code will loop continuously as long it is operator control mode
		{
			GetWatchdog().Feed(); // Feed the watchdog	

			shoulderPotentiometerReading = (5-(shoulderPotentiometerChannel->GetVoltage())); // reads the potentiometer at channel 1

			/* COMMENT GRIPPER CODE BELOW
			if (shoulderPotentiometerReading == 0) {
				gripperMotor->SetAngle(0); //opens gripper
			}
			else if (shoulderPotentiometerReading > 3) {
				gripperMotor->SetAngle(55); //closes gripper
			}
			*/

			if (leftstick->GetRawButton(4) == true) {
				shoulderMotor->Drive(-kMaxShoulderMotorSpeed, 0); //moves shoulderMotor down
				shoulderDestinationVoltage = shoulderPotentiometerReading;
			} else if (leftstick->GetRawButton(5) == true) {
				shoulderMotor->Drive(kMaxShoulderMotorSpeed, 0); //moves shoulderMotor up
				shoulderDestinationVoltage = shoulderPotentiometerReading;
			} else {
				
				//LEFTSTICK PRESETS
				if (leftstick->GetRawButton(2) == true) {
					//shoulderDestinationVoltage = kPickUpPosition;
				} else if (leftstick->GetRawButton(6) == true) {
					shoulderDestinationVoltage = kTopRow;
				} else if (leftstick->GetRawButton(7) == true) {
					shoulderDestinationVoltage = kMiddleRow;
				} else if (leftstick->GetRawButton(8) == true) {
					shoulderDestinationVoltage = kBottomRow;
				} else if (leftstick->GetRawButton(9) == true) {
					shoulderDestinationVoltage = kBottomRowSecondColumn;
				} else if (leftstick->GetRawButton(10) == true) {
					shoulderDestinationVoltage = kMiddleRowSecondColumn;
				} else if (leftstick->GetRawButton(11) == true) {
					shoulderDestinationVoltage = kTopRowSecondColumn;
				}

				MoveShoulderTo(shoulderDestinationVoltage);
			}
			
			
			
			//----------------------------------------------------------------
			

			//GRIPPER OPEN AND CLOSE
			if (leftstick->GetRawButton(1) == true) {
				gripperMotor->SetAngle(0); //opens gripper
			} else {
				gripperMotor->SetAngle(55); //closes gripper
			}
			
			//MINIBOT DEPLOYMENT 
			if (leftstick->GetRawButton(3) == true) {
				minibotDeployMotor->SetAngle(60); //releases four-bar
				myRobot->Drive(0.0, 0); // stop robot since controls are going to be inverted momentarily - don't want to go from full forward to full reverse instantly
			}

			//RIGHTSTICK PRESETS

			if (rightstick->GetRawButton(1) == true) {
				//back of the robot is moved forward by pushing forward on the joystick
				myRobot->ArcadeDrive((rightstick->GetY()),
						(rightstick->GetX()), false); // inverted drive control	
			} else {
				//front of the robot is moved forward by pushing forward on the joystick
				myRobot->ArcadeDrive(-(rightstick->GetY()),
						(rightstick->GetX()), false); // normal drive control		
			}
			
			//MINIBOT CLOSING/CLIMBING POLE
			if (rightstick->GetRawButton(3) == true) {
				//minibot closes only after its four-bar is dropped
				minibotCloseMotor1->SetAngle(45); //closes minibot so it starts climbing pole
				minibotCloseMotor2->SetAngle(45); //closes minibot so it starts climbing pole
			} else if (rightstick->GetRawButton(4) == true) {
				//MANUAL ARM CONTROL
				armMotor->Drive(-kMaxArmMotorSpeed/2.0, 0); //turn armMotor counter clockwise
			} else if (rightstick->GetRawButton(5) == true) {
				//MANUAL ARM CONTROL
				armMotor->Drive(kMaxArmMotorSpeed/1.5, 0); //turn armMotor cw
			} 
		}
	}

	float setSpeed(float newMagnitude)
	{
		tarTheta = atan2(yVector, xVector);
		curTheta = -(posEncoder->GetVoltage() - FLOFFSET ) / 5 * 2 * PI;
		
		
		//	Code Snippet
		diffTheta = tarTheta - curTheta;
			
		if (diffTheta > PI) 
		{
			diffTheta -= 2*PI;
		} 
		else if (diffTheta < -PI) 
		{
			diffTheta += 2*PI;
		}

		if (diffTheta > PI/2) 
		{
			diffTheta -= PI;
			mag = mag * -1;
		} 
		else if (diffTheta < -PI/2) 
		{
			diffTheta += PI;
			mag = mag * -1;
		}

		turnVel = diffTheta / (PI/2);
		
		if (0 < turnVel && turnVel < .25)
		{
			turnVel = .25;
		} 
		if (0 > turnVel && turnVel > -.25)
		{
			turnVel = -.25;
		}
		if (fabs(diffTheta) < PI/45 )
		{
			turnVel = 0;
		}
		if (((turnVel > 0 && prevTurnVel < 0)
				|| (turnVel < 0&& prevTurnVel> 0)) 
				&& !changeSign)
		{
			changeSign = true;
//			moveTime = baneTimer.Get() + .1; 				**FIX BANETIMER
		}
		if (changeSign) 
		{
			turnVel = 0;
//			if (moveTime < baneTimer.Get()) 
			{
				changeSign = false;
			}
		}
		
		//	/Code Snippet
		
		
		if (!(xVector == 0 && yVector == 0))
		{
			turnWheel->Set(turnVel);							
			moveWheel->Set(mag);
		}
		else
		{
			turnWheel->Set(0);
			moveWheel->Set(0);
		}
		
		
	}

	//Returns distance in inches
	float getDistance()
	{
		return ultrasonic->GetVoltage()*(503.0/5.0);
	}

	double GripPIDSource::PIDGet()
	{
		return (m_potentiometer->GetVoltage() - 0.33)/1.7;
	}