C++ DriverStation::GetDigitalIn方法代码示例

    // Test Autonomous
    void TestAutonomous()
    {
        robotDrive.SetSafetyEnabled(false);

        // STEP 1: Set all of the states.
        // SAFETY AND SANITY - SET ALL TO ZERO
        loaded = winchSwitch.Get();
        loading = false;
        intake.Set(0.0);
        winch.Set(0.0);

        // STEP 2: Move forward to optimum shooting position
        Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);

        // STEP 3: Drop the arm for a clean shot
        arm.Set(DoubleSolenoid::kForward);
        Wait(1.0); // Ken

        // STEP 4: Launch the catapult
        LaunchCatapult();

        Wait (1.0); // Ken

        if (ds->GetDigitalIn(0))
        {
            // STEP 5: Start the intake motor and backup to our origin position to pick up another ball
            InitiateLoad();
            intake.Set(-INTAKE_COLLECT);
            while (CheckLoad());
            Drive(AUTO_DRIVE_SPEED, SHOT_POSN_DIST);
            Wait(1.0);

            // STEP 6: Shut off the intake, bring up the arm and move to shooting position
            intake.Set(0.0);
            arm.Set(DoubleSolenoid::kReverse);
            Wait (1.0);
            Drive(-AUTO_DRIVE_SPEED, SHOT_POSN_DIST);

            // Step 7: drop the arm for a clean shot and shoot
            arm.Set(DoubleSolenoid::kForward);

            // UNTESTED KICKED OFF FIELD
            Wait(1.0); // Ken
            LaunchCatapult();
        }

        // Get us fully into the zone for 5 points
        Drive(-AUTO_DRIVE_SPEED, INTO_ZONE_DIST - SHOT_POSN_DIST);

        // SAFETY AND SANITY - SET ALL TO ZERO
        intake.Set(0.0);
        winch.Set(0.0);
    }

	/**
	 * unchanged from SimpleDemo:
	 * Runs the motors under driver control with either tank or arcade steering selected
	 * by a jumper in DS Digin 0. 
	 */
	void OperatorControl(void)
	{
		DPRINTF(LOG_DEBUG, "OperatorControl");
		GetWatchdog().Feed();

		while (1)
		{
			// 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)
			}
		}  
	} // end operator control	

    /**
     * unchanged from SimpleDemo:
     *
     * Runs the motors under driver control with either tank or arcade
     * steering selected by a jumper in DS Digin 0.
     *
     * added for vision:
     *
     * Adjusts the servo gimbal based on the color tracked.  Driving the
     * robot or operating an arm based on color input from gimbal-mounted
     * camera is currently left as an exercise for the teams.
     */
    void OperatorControl(void)
    {
        char funcName[] = "OperatorControl";
        DPRINTF(LOG_DEBUG, "OperatorControl");
        //GetWatchdog().Feed();
        TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT);

        /* for controlling loop execution time */
        float loopTime = 0.05;
        double currentTime = GetTime();
        double lastTime = currentTime;

        double savedImageTimestamp = 0.0;
        bool foundColor = false;
        bool staleImage = false;

        while (IsOperatorControl())
        {
            setServoPositions(rightStick->GetX(), rightStick->GetY());

            /* calculate gimbal position based on color found */
            if (FindColor
                (IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, &par,
                 &cReport))
            {
                foundColor = true;
                if (par.imageTimestamp == savedImageTimestamp)
                {
                    // This image has been processed already,
                    // so don't do anything for this loop
                    staleImage = true;
                }
                else
                {
                    staleImage = false;
                    savedImageTimestamp = par.imageTimestamp;
                    // compute final H & V destination
                    horizontalDestination = par.center_mass_x_normalized;
                    verticalDestination = par.center_mass_y_normalized;
                }
            }
            else
            {
                foundColor = false;
            }

            PrintReport(&cReport);

            if (!staleImage)
            {
                if (foundColor)
                {
                    /* Move the servo a bit each loop toward the
                     * destination.  Alternative ways to task servos are
                     * to move immediately vs.  incrementally toward the
                     * final destination. Incremental method reduces the
                     * need for calibration of the servo movement while
                     * moving toward the target.
                     */
                    ShowActivity
                        ("** %s found: Servo: x: %f  y: %f",
                         td.name, horizontalDestination, verticalDestination);
                }
                else
                {
                    ShowActivity("** %s not found", td.name);
                }
            }

            dashboardData.UpdateAndSend();

            // sleep to keep loop at constant rate
            // elapsed time can vary significantly due to debug printout
            currentTime = GetTime();
            lastTime = currentTime;
            if (loopTime > ElapsedTime(lastTime))
            {
                Wait(loopTime - ElapsedTime(lastTime));
            }
        }

        while (IsOperatorControl())
        {
            // determine if tank or arcade mode; default with no jumper is
            // for tank drive
            if (ds->GetDigitalIn(ARCADE_MODE) == 0)
            {
                // drive with tank style
//.........这里部分代码省略.........

void Arm::DriveArm() {
	float claw_target= CLAW_OPEN;

	DriverStation *ds = DriverStation::GetInstance();

	if(ds->GetDigitalIn(3) && !ds->GetDigitalIn(4))
	{
		float tower_target = (ds->GetEnhancedIO().GetAnalogInRatio(1) * (TOWER_MAX - TOWER_MIN)) + TOWER_MIN;
		float wrist_target = (ds->GetEnhancedIO().GetAnalogInRatio(3) * (WRIST_MAX-WRIST_MIN)) + WRIST_MIN;
	
		if (tower_target > TOWER_MAX)
			tower_target = TOWER_MAX;
		else if (tower_target < TOWER_MIN)
			tower_target = TOWER_MIN;
	
		if (wrist_target < WRIST_MIN)
			wrist_target = WRIST_MIN;
		if (wrist_target > WRIST_MAX)
			wrist_target = WRIST_MAX;
	
		if (ds->GetDigitalIn(1)) {
			claw_target = CLAW_CLOSED;
		}
	
		float tower_voltage = towerPID->GetOutput(GetTowerPot(), tower_target, 0,
				.03) * -1;
		float claw_voltage = clawPID->GetOutput(GetClawPot(), claw_target, 0, .1);
		float wrist_voltage = wristPID->GetOutput(GetWristPot(), wrist_target, 0,
				.2) * -1;
		
		if(!towerLimit->Get() && tower_voltage < 0)
		{
			tower_voltage = 0;
			towerMotor->Set(0);
		}
		
		if(fabs(claw_voltage) > .5)
			wrist_voltage = 0;
		
		towerMotor->Set(tower_voltage);
		wristMotor->Set(wrist_voltage);
		clawMotor->Set(claw_voltage);
	}
	else if(ds->GetDigitalIn(3) && ds->GetDigitalIn(4))
	{
		float tower_voltage = towerPID->GetOutput(GetTowerPot(), TOWER_AUTO_DOWN, 0,
						.03) * -1;
		float claw_voltage = clawPID->GetOutput(GetClawPot(), CLAW_OPEN, 0, .1);
		float wrist_voltage = wristPID->GetOutput(GetWristPot(), WRIST_AUTO_DOWN, 0,
				.2) * -1;
		
		towerMotor->Set(tower_voltage);
		wristMotor->Set(wrist_voltage);
		clawMotor->Set(claw_voltage);
	}
	else if(!ds->GetDigitalIn(3) && ds->GetDigitalIn(4))
	{
		float tower_voltage = towerPID->GetOutput(GetTowerPot(), TOWER_AUTO_UP, 0,
						.03) * -1;
		float claw_voltage = clawPID->GetOutput(GetClawPot(), CLAW_CLOSED, 0, .1);
		float wrist_voltage = wristPID->GetOutput(GetWristPot(), WRIST_AUTO_UP, 0,
				.2) * -1;
		
		towerMotor->Set(tower_voltage);
		wristMotor->Set(wrist_voltage);
		clawMotor->Set(claw_voltage);
	}
}