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

    void ResetHomePosition(void){
    	
    	if(Operator_Control_Stick.GetRawButton(HOME_RESET_BUTTON) == true){
    		homePositionInOhms = armPotentiometer1.GetValue();
		}
    	
    }

/**
 * Get a sample straight from this channel on the module.
 * The sample is a 12-bit value representing the -10V to 10V range of the A/D converter in the module.
 * The units are in A/D converter codes.  Use GetVoltage() to get the analog value in calibrated units.
 *
 * @param slot The slot the analog module is plugged into
 * @param channel the channel for the value being used
 * @return A sample straight from this channel on the module.
 */
INT16 GetAnalogValue(UINT32 slot, UINT32 channel)
{
	AnalogChannel *analog = AllocateAnalogChannel(slot, channel);
	if (analog != NULL)
	{
		return analog->GetValue();
	}
	return 0;
}

	/** Function to retrieve the user inputs
	 * Inputs (For all user input modes):
	 * * Potentiometers 
	 * * Limit switches
	 * * the requested state for the robot (from the joystick buttons)
	 * For manual mode only:
	 * * The intake and ejection wheel speeds - based on the operator joystick throttle
	 * * The arm position - based on the operator joystick Y axis  
	 */
    void GetUserInputs(RobotStates_t &requestedState)
    {
    	float throttleRaw = 0.0;
        //Read potentiometer goes from -5 to 28
        potentiometerValueRight = armPotentiometer1.GetValue();
        //potentiometerValueLeft = armPotentiometer2.GetValue();
        ballCaughtLimitSwitch1 = limitSwitch1.Get(); //TODO 0 is no ball 1 is ball 
        //Read Joystick state buttons
        requestedState = ReadJoystickButtons();
        throttleRaw = DeadZone(Operator_Control_Stick.GetTwist(), EJWHEEL_MM_DEADZONE_VAL);
        manualModeThrottlePosition = (throttleRaw * -1); //invert throttle value to match the joystick
		manualModeArmPosition = DeadZone(Operator_Control_Stick.GetY(), ARM_MM_DEADZONE_VAL);
		
    }

	/**
	 * Constructor for our robot class
	 * @note This constructor uses the initializer notation to efficiently 
	 *  initialize the class.
	 *  First number in parenthesis is the sidecar the motor is on
	 *  Second number in parenthesis is the port on the sidecar
	 */
	OurRobotDemo():		
		motor1(1, 1),
		motor2(1, 2),
		motor3(1, 3),
		motor4(1, 4),
		motor5(1, 5),
		motor6(1, 6),
		leftArmIntakeMotor(2, 1),
		leftArmHoldMotor(2, 2),
		rightArmIntakeMotor(2, 4),
		rightArmHoldMotor(2, 3),
		leftArmAngleMotor(2, 5),
		rightArmAngleMotor(2, 6),
		leftDriveStick(1),
		rightDriveStick(2),
		Operator_Control_Stick(3),
		armPotentiometer1(1),
		//armPotentiometer2(2),
		limitSwitch1(2, 1),
		limitSwitch2(2, 2),
		//VALUES FROM HERE DOWN!!!! values in parenthesis are the initial value upon power up
    	potentiometerValueRight(kPotInitValue),
    	//potentiometerValueLeft(kPotInitValue),
    	manualModeThrottlePosition(0.0),
    	manualModeArmPosition(0.0),
    	inPosition(false),
    	ballCaughtLimitSwitch1(true),
    	ballCaughtLimitSwitch2(true),
    	robotState(kManualMode),
		ejectBallTimer(0)
		{
			//GetWatchdog().SetExpiration(0.1); // Must feed watchdog every 100mS
		
			//possRobotStates = {new RobotState("kBlock", K_BLOCK_ARM_ANGLE, K_BLOCK_OUT_SPD, K_BLOCK_IN_SPD, K_BLOCK_BALL_CAUGHT), 
			//		new RobotState("kFloorLoad")};
		
			//Default to homing
			returnToHome = true;
			
			//Read the current home position
			///NOTE - this requires that the robot start with the arms up in the positon to home to.
			homePositionInOhms = armPotentiometer1.GetValue();
			
			isAutonomous = false;
		}

    /**
	 * Main function to run during teleop mode
	 * Performs the following
	 *  * Initializes the motor drive to run safely
	 *  * Controls the motors with arcade steering.
	 */
    void OperatorControl()
    {
        //GetWatchdog().SetEnabled(true);
    	
    	isAutonomous = false;
    	
        //Local declarations		
        RobotStates_t requestedState = kManualMode;
        robotState = kManualMode;
        homePositionInOhms = armPotentiometer1.GetValue();
        
        //runs the code in this section over and over forever
        while(IsOperatorControl() && !IsDisabled()){
        	
            //GetWatchdog().Feed();
			
        	//Get inputs from Joystick and sensors
            GetUserInputs(requestedState);
			
			//Transition to state, control function
			robotState = StateTransitionControl(requestedState);
			
			//set arm motor outputs based on state 
			StateBasedOutput();
			
			//get drive base joysticks and drive motors on base
			ManualDriveBaseActuation();
			
			//TODO - this is basically hijacking / defining arm state
			// This should be incorporated into the default state machine ASAP
			ResetHomePosition();
			ReturnHomeControl_REMOVE_ASAP();
			
			//printing values out to the console
			//if(!IsDisabled()){
			//	cout << "State requested: " << robotState << ", Pot voltage: " << potentiometerValueRight << ", Switch: " << ballCaughtLimitSwitch1 << endl;
			//}
			//DEBUG
			cout << "TELEOP: throttle: " << manualModeThrottlePosition << ", potValue: " << potentiometerValueRight << ", Home Pos: " << homePositionInOhms << ", Return: " << returnToHome << endl;
			
			//wait delay in main loop for motor update
			Wait(0.01);
			
		}
	}

	/**
	 * This code is run in the Autonomous mode
	 */
	void Autonomous()
	{
		isAutonomous = true;
		returnToHome = true;
		
		//GetWatchdog().SetEnabled(false);
		int i = 0;
		float AutomSpeed = 0.5; //drive motors half speed
		int AutomDuration = 100; //TODO make sure 40 ms is long enough
		
		//TODO remove after next match
		//Wait(3);
		
		//resetting counter
		i = 0;
		
		//TODO - double the autonomous length
		while(i < 2*AutomDuration)
		{
			//When we get to half way thru the time, stop monving, but continue control on the arms
			if(i >= AutomDuration){
				AutomSpeed = 0.0;
			}
			
			//TODO 
			potentiometerValueRight = armPotentiometer1.GetValue();
			ReturnHomeControl_REMOVE_ASAP();
			
			//Drive left motors at half speed
			motor1.Set(AutomSpeed);
			motor3.Set(AutomSpeed);
			motor5.Set(AutomSpeed);
			
			//Drive right motors at half speed
			motor2.Set(-1 * AutomSpeed);
			motor4.Set(-1 * AutomSpeed);
			motor6.Set(-1 * AutomSpeed);
			
			Wait(.01);
			i++;
			
			cout << "AUTO: potValue: " << potentiometerValueRight << ", Home Pos: " << homePositionInOhms << ", Return: " << returnToHome << endl;
						
		}
		
		//Drive left 
		//motor1.Set(0.0);
		//motor3.Set(0.0);
		//motor5.Set(0.0);
		
		//Drive right motors at half speed
		//motor2.Set(0.0);
		//motor4.Set(0.0);
		//motor6.Set(0.0);
		
		//TODO
		//while(1){
		//	ReturnHomeControl_REMOVE_ASAP();
		//}
		
	}

	void OperatorControl(void)
	{
		float counter;
		timer.Start();
		float percent;
		deadband = .05;
		float pi = 3.141592653589793238462643;
		float bpotval, fpotval;
		float min = 600, max;
		float fchgval = .5;
		float bchgval = .5;
		
		while (IsOperatorControl())
		{
			// comp.checkCompressor();
			ShootModeSet();
			Shoot(true);
			fpotval = fpot.GetValue();
			bpotval = bpot.GetValue();
				counter = timer.Get();
				if (controller.GetRawButton(3))
				{
					frot.SetSpeed(0);
					brot.SetSpeed(0);
					flmot.SetSpeed(0);
					frmot.SetSpeed(0);
					blmot.SetSpeed(0);
					brmot.SetSpeed(0);
				}
				else if (controller.GetRawButton(BTN_L1) == false)
				{
					if (controller.GetRawButton(7))
					{
						if (fpotval < 860)
							frot.SetSpeed(-0.5);
						if (bpotval <  725)
							brot.SetSpeed(-0.5);
						if (fpotval > 860 and bpotval > 725)
						{
							frot.Set(0);
							brot.Set(0);
							flmot.Set(0.5);
							frmot.Set(0.5);
							blmot.Set(0.5);
							brmot.Set(0.5);
						}
					}
					else if (controller.GetRawButton(8))
					{
						if (fpotval < 860)
							frot.SetSpeed(-0.5);
						if (bpotval <  725)
							brot.SetSpeed(-0.5);
						if (fpotval > 860 and bpotval > 725)
						{
							frot.Set(0);
							brot.Set(0);
							flmot.Set(-0.5);
							frmot.Set(-0.5);
							blmot.Set(-0.5);
							brmot.Set(-0.5);
						}
					}
					else
					{
						if (controller.GetRawAxis(4) <= 0)
							percent = ((acos(controller.GetRawAxis(3)) / pi));
						else if (controller.GetRawAxis(4) > 0)
							percent = ((acos(-controller.GetRawAxis(3)) / pi));
						fpotval = percent * 550 + 330;
						percent = (fpotval - 330) / 550;
						bpotval = percent * 500 + 245;
						
						if (fpot.GetValue() < fpotval)
							fchgval = -.5;
						
						else if (fpot.GetValue() > fpotval)
							fchgval = .5;
						
						if (fpot.GetValue() < fpotval + 10 && fpot.GetValue() > fpotval - 10)
							fchgval = 0;
			
						if (bpot.GetValue() > bpotval)
							bchgval = -.5;
									
						else if (bpot.GetValue() < bpotval)
							bchgval = .5;
									
						if (bpot.GetValue() < bpotval + 20 && bpot.GetValue() > bpotval - 20)
							bchgval = 0;
						
						frot.Set(fchgval);
						brot.Set(bchgval);
						
						if (pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2) > deadband && controller.GetRawButton(BTN_R1))
						{
							if (controller.GetRawAxis(4) > 0)
							{
								flmot.Set(0.5 * -sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
								frmot.Set(0.5 * sqrt(pow(controller.GetRawAxis(3), 2) + pow(controller.GetRawAxis(4), 2)));
//.........这里部分代码省略.........

	void OperatorControl()
	{
		timer.Start();
		while (IsOperatorControl())
		{
			x = Stick.GetRawAxis(1);
			y = -Stick.GetRawAxis(2);
			if (fabs(x) < 0.1)
			{
				x = 0;
			}
			if (fabs(y) < 0.1)
			{
				y = 0;
			}
			x2 = x*x;
			y2 = y*y;
			magnitude = pow((x2+y2),0.5);
			angle = atan2(y,x)*180/PI;
			magnitude = max(magnitude, -1);
			magnitude = min(magnitude, 1);
			if (angle < 0)
			{
				angle += 180;
				magnitude *= -1;
			}
			/*front = 730 - (angle * 2.7888);
			if ((front + 5) < float(fpot.GetValue()))
			{
				fturn.Set(0.5);
			}
			else if ((front - 5) > float(fpot.GetValue()))
			{
				fturn.Set(-0.5);
			}
			else
			{
				fturn.Set(0);
			}*/
			back = 235 + (angle * 2.8611);
			if ((back + 10) < float(bpot.GetValue()))
						{
							bturn.Set(0.5);
						}
						else if ((back - 10) > float(bpot.GetValue()))
						{
							bturn.Set(-0.5);
						}
						else
						{
							bturn.Set(0);
						}
					if (timer.Get() > 0.1)
					{
						lcd->Clear();
						lcd->Printf(DriverStationLCD::kUser_Line1, 1, "Front = %5.4f", front);
						lcd->Printf(DriverStationLCD::kUser_Line2, 1, "Back = %5.4f", back);
						lcd->Printf(DriverStationLCD::kUser_Line3, 1, "Front Pot = %5.4f", float(fpot.GetValue()));
						lcd->Printf(DriverStationLCD::kUser_Line4, 1, "Back Pot = %5.4f", float(bpot.GetValue()));
						lcd->Printf(DriverStationLCD::kUser_Line5, 1, "Angle = %5.4f", angle);
						lcd->Printf(DriverStationLCD::kUser_Line6, 1, "Magnitude = %5.4f", magnitude);
						lcd->UpdateLCD();
						timer.Reset();
						timer.Start();
					}
			
		}
	}