C++ DigitalInput类代码示例

/*
 * Get the value from a digital input channel.
 * Retrieve the value of a single digital input channel from the FPGA.
 *
 * @param slot The slot the digital input module is plugged into
 * @param channel The particular channel this digital input is using
 */
UINT32 GetDigitalInput(UINT32 slot, UINT32 channel)
{
    DigitalInput *digIn = AllocateDigitalInput(slot, channel);
    if (digIn)
        return digIn->Get();
    else
        return 0;
}

/*
 * Get the value from a digital input channel.
 * Retrieve the value of a single digital input channel from the FPGA.
 *
 * @param slot The slot the digital input module is plugged into
 * @param channel The particular channel this digital input is using
 */
UINT32 GetDigitalInput(UINT8 moduleNumber, UINT32 channel)
{
	DigitalInput *digIn = AllocateDigitalInput(moduleNumber, channel);
	if (digIn)
		return digIn->Get();
	else
		return 0;
}

	void AutonomousType4() {
		SmartDashboard::PutString("STATUS:", "STARTING AUTO 4");

		//Lift, turn, drive
		chainLift.SetSpeed(0.5);
		while (midPoint.Get() && maxUp.Get()) {
		}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Polar(0, 0, 0.3);

		if (WaitF(2.5))
			return;
		robotDrive.MecanumDrive_Polar(0.25, 0, 0);
		if (WaitF(5.6))
			return;
		robotDrive.MecanumDrive_Polar(0, 0, 0.3);
		if (WaitF(2))
			return;
		robotDrive.MecanumDrive_Polar(0, 0, 0);
		//chainLift.SetSpeed(-0.4);
		//while (maxDown.Get() && IsAutonomous()) {
		//}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Polar(0, 0, 0);
		SmartDashboard::PutString("STATUS:", "AUTO 4 COMPLETE");
	}

	//Choose which auto to use
	void AutonomousInit() {

		chainLift.SetSpeed(0);
		canGrabber.SetSpeed(0);
		robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		SmartDashboard::PutString("STATUS:", "STARTING AUTO");
		robotDrive.SetSafetyEnabled(false);
		chainLift.SetSafetyEnabled(false);

		SmartDashboard::PutBoolean("Auto switch A: ", autoSwitch1.Get());
		SmartDashboard::PutBoolean("Auto switch B: ", autoSwitch2.Get());

		//Select auto type
		if (autoSwitch1.Get()) {
			if (autoSwitch2.Get())
				AutonomousType4();
			else
				//1 on 2 grab n back
				AutonomousType8();
		} else {
			if (autoSwitch2.Get())
				//1 off, 2 on: grab n turn
				AutonomousType10();
			else {
				SmartAutoPicker();
			}
			//Do Nothing
		}
	}

void RobotDemo::dumb_climber_code()
{
	//printf("Top:%i   Bottom:%i  " , top_claw_limit_switch->Get() , bottom_claw_limit_switch->Get());
	if (drive_stick_prim ->GetRawButton(climber_hold_up))
	{
		if (top_claw_limit_switch->Get() == 1)
		{
			climbing_motor->Set(0.0);
			//printf("STOPPED\n");
		}
		else
		{
			climbing_motor->Set(1);
			//printf("GOING UP\n");
		}
	} // not climber hold up

	else if (drive_stick_prim->GetRawButton(climber_hold_down))
	{
		if (bottom_claw_limit_switch->Get() == 0)
		{
			climbing_motor->Set(0.0);
			//printf("STOPED\n");
		}
		else
		{
			climbing_motor->Set(-1);
			//printf("GOING DOWN\n");
		}
	} // hold down button
	else
	{ // no js buttons pushed
		climbing_motor->Set(0.0);
	}
}

	/**
	 * Runs the motors under driver control with either tank or arcade steering selected
	 * by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis. 
	 */
	void OperatorControl(void)
	{
		Victor armMotor(5);						// create arm motor instance
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();

			// determine if tank or arcade mode; default with no jumper is for tank drive
			if (ds->GetDigitalIn(ARCADE_MODE) == 0) {	
				myRobot->TankDrive(leftStick, rightStick);	 // drive with tank style
			} else{
				myRobot->ArcadeDrive(rightStick);	         // drive with arcade style (use right stick)
			}

			// Control the movement of the arm using the joystick
			// Use the "Y" value of the arm joystick to control the movement of the arm
			float armStickDirection = armStick->GetY();

			// if at a limit and telling the arm to move past the limit, don't drive the motor
			if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) {
				armStickDirection = 0;
			} else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) {
				armStickDirection = 0;
			}

			// Set the motor value 
			armMotor.Set(armStickDirection);
		}
	}

/******************************************************************************
 * Function:     DisabledPeriodic
 *
 * Description:  Run the functions that are expected during the period when the
 *               robot is disabled.
 ******************************************************************************/
void DisabledPeriodic()
{
  Scheduler::GetInstance()->Run();

  Light1.Set(1);
  Light2.Set(1);


  while(IsDisabled())
  {
    UpdateActuatorCmnds(0,0,false,false,false,false,false,false,false,0,0,0,0,0);

    ReadAutonSwitch();

    UpdateSmartDashboad(Sw1.Get(),
                        Sw2.Get(),
                        Sw3.Get(),
                        Sw4.Get(),
                        BlSw.Get(),
                        (double)AutonState,
                        (double)0,
                        ballarm.GetDistance(),
                        gyroOne.GetAngle(),
                        accel.GetX(),
                        accel.GetY(),
                        accel.GetZ(),
                        (double)0,
                        (double)0);
  wait(kUpdatePeriod);
  }
}

	void Autonomous ()
	{
		DriveTrain.StartDriveTrain();
		Shooter.StartShooterAuto();
		AutonomousState = 1;
		while(IsAutonomous())
		{
			if (AutonomousSwitch.Get() == 0)
			{
				if (AutonomousState == 1)
				{
					DriveTrain.RunDriveTrain(1, 1, 0, 0);
					if ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal >= 2400))
					{
						AutonomousState = 2;
					}
				}
				else if ((AutonomousState == 2) && (HotGoal == true))
				{
					Shooter.RunShooter(0, 1, 0);
				}
			}
			else if (AutonomousSwitch.Get() == 1)
			{
				if (AutonomousState == 1)
				{
					DriveTrain.RunDriveTrain(1, 1, 0, 0);
					if ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal >= 2400))
					{
						AutonomousState = 2;
					}
				}
				else if ((AutonomousState == 2) && (WhichHotGoal != 0))
				{
					DriveTrain.RunDriveTrain(WhichHotGoal, -WhichHotGoal, 0, 0);
					if (((WhichHotGoal == 1) && ((DriveTrain.LeftEncTotal >= 2800) || (DriveTrain.RightEncTotal <= 2000))) || ((WhichHotGoal == -1) && ((DriveTrain.LeftEncTotal <= 2000) || (DriveTrain.RightEncTotal >= 2800))))
					{
						AutonomousState = 3;
					}
				}
				else if (AutonomousState == 3)
				{
					Shooter.RunShooter(0, 1, 0);
					AutonomousState = 4;
				}
				else if (AutonomousState == 4)
				{
					DriveTrain.RunDriveTrain(-WhichHotGoal, WhichHotGoal, 0, 0);
					if (((WhichHotGoal == 1) && ((DriveTrain.LeftEncTotal <= 2400) || (DriveTrain.RightEncTotal >= 2400))) || ((WhichHotGoal == -1) && ((DriveTrain.LeftEncTotal >= 2400) || (DriveTrain.RightEncTotal <= 2400))))
					{
						AutonomousState = 5;
					}
				}
			}
		}
	}

	void shootCatapult()
	{
		if (logitech.GetRawButton(2) && buttonOne.Get()==0 && buttonTwo.Get()==0)
		{
			dogSolenoid.Set(DoubleSolenoid::kReverse);
			Wait(0.5);
			ratchetSolenoid.Set(DoubleSolenoid::kReverse);
			Wait(1);
		}
	}

void RobotDemo::DriverLCD()
{
	if (cycle_counter >= 50)
	{
		dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "RPS Back:%f  ",
				RPS_back);
		dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, "RPS Front:%f  ",
				RPS_front);
		dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, "RPS DRPS:%f  ",
				desired_RPS_control);
#if 0
		if (shooter_fire_piston_A->Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"Fire   ");
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"Retracting...   ");
		}
#endif
		//dsLCD->Printf(DriverStationLCD::kUser_Line4, 1,"TopLS:%i   BotLS:%i   ", top_claw_limit_switch->Get(),
		//	bottom_claw_limit_switch ->Get());
		if (top_claw_limit_switch->Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "!TOP");
		}
		else if (!bottom_claw_limit_switch->Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "!BOTTOM");
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Neither");
		}
		if (shooter_angle_1->Get())
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Up   ");
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Down   ");
		}
		if (arcadedrive)
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Arcade   ");
		}
		else
		{
			dsLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Tank   ");
		}
		dsLCD->UpdateLCD();
		//cycle_counter = 0;
	}
	//cycle_counter++;
}

	void loadCatapult()
	{
		if (buttonOne.Get()==1 && buttonTwo.Get()==1 && dogSolenoid.Get()==DoubleSolenoid::kReverse)
		{
			dogSolenoid.Set(DoubleSolenoid::kForward);
			Wait(0.5);
			ratchetSolenoid.Set(DoubleSolenoid::kForward);
			Wait(0.5);
			catapultMotor.Set(1);
		}
	}

	void autonomousCatapultRelease()
	{
		if ((leftLimitSwitch.Get()== 0 || rightLimitSwitch.Get()== 0) && winchMotor.Get() == 0)
		{
			stopDriving(); // Stops the drive so the robot doesn't flip on itself or something
			winchMotor.Set(0); // Redundant line for extra safety that can be removed after testing (The winch should already be off)
			dogSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the dog gear
			Wait(0.2); // Giving the pistons time to disengage properly
			ratchetSolenoid.Set(DoubleSolenoid::kReverse); // Brings the pneumatic piston backward to disengage the ratchet
			Wait(5); // Waits 5 seconds after shooting before starting to load the catapult
		}
	}

	//Grab first yellow, back up to auto zone, DON'T DROP
	void AutonomousType12() {
		SmartDashboard::PutString("STATUS:", "STARTING AUTO 12");
		chainLift.SetSpeed(0.5);
		while (IsAutonomous() && IsEnabled() && maxUp.Get() && midPoint.Get()) {
		}
		chainLift.SetSpeed(0);
		robotDrive.MecanumDrive_Cartesian(0, 0.4, 0);
		if (WaitF(3))
			return;
		robotDrive.MecanumDrive_Cartesian(0, 0, 0);
		SmartDashboard::PutString("STATUS:", "AUTO 12 COMPLETE");
	}

		bool UpdateState(
				float powerLevel,    //typically 1.0 for forward -1.0 for backward
				double targetPosition,
				HookSwitch hookSwitch=NoHookSwitch)
		{
		    
			if (powerLevel < 0.0 && !lowerLimitSwitch->Get()){
				if (motorController) {
					motorController->Disable();
				}
				motor->Set(0.0);
            	encoder->Reset();
				encoder->Start();
				return true;
			}
			double position = encoder->GetDistance();
			if (motorController) {
				if (!motorController->IsEnabled())
					motorController->Enable();
				motorController->SetSetpoint(targetPosition);
			} else {
            	if (position < targetPosition-ClimberCloseTolerance) {
            		motor->Set(1.0);
            	} else if (position > targetPosition+ClimberCloseTolerance){
            		motor->Set(-1.0);            		
            	}
			}

			if ( (hookSwitch == UpperHookSwitch && !upperHookSwitch->Get())||
				 (hookSwitch == LowerHookSwitch && !lowerHookSwitch->Get())||
				 (hookSwitch == NoHookSwitch &&
							targetPosition -ClimberCloseTolerance <= position && position <= targetPosition + ClimberCloseTolerance ) ) {
				if (motorController) {
					motorController->Disable();
				}
				motor->Set(0.0);
				encoder->Start();
				return true;
			} else if (hookSwitch != NoHookSwitch &&
					( (powerLevel > 0 && position > targetPosition) ||
						(powerLevel < 0 && position < targetPosition) ) ) {
				robot->AbortClimb("Grab did not occur when expected");
				return false;
			} // else if (hookSwitch != NoHookSwitch && Check motor power draw)
			// abort here if looking for switch and started drawing a lot of power 
			return false;
		}

	void Test() 
	{
		DriverStationLCD *screen = DriverStationLCD::GetInstance();
		while (IsTest())
		{
			if ((buttonOne.Get()) == 1)
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Not Pressed!");
			}
			else
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line1,"Button1_Pressed!");
			}
			if ((buttonTwo.Get()) == 1)
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Not Pressed!");
			}
			else
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line2,"Button2_Pressed!");
			}
			if ((togglebuttonOne.Get()) == 0)
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Is_On!");
			}
			else
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Left Is_Off!");
			}
			if ((togglebuttonTwo.Get()) == 0)
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Is_On!");
			}
			else
			{
				screen -> PrintfLine(DriverStationLCD::kUser_Line4,"Right Is_Off!");
			}
			if (buttonOne.Get() == 0)
			{
				emergencyCompressor.Set(Relay::kOff);
			}
			
#if 0
		screen -> PrintfLine(DriverStationLCD::kUser_Line1,"ButtonOneIs_%d", buttonOne.Get());
		screen -> PrintfLine(DriverStationLCD::kUser_Line2,"toggle_%d", buttonTwo.Get());
		screen -> PrintfLine(DriverStationLCD::kUser_Line3,"Button_%d", togglebuttonOne.Get());
		screen -> PrintfLine(DriverStationLCD::kUser_Line4,"toggle_%d", togglebuttonTwo.Get());
#endif
		Wait(0.005);
		screen -> UpdateLCD();
		}
	}