import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public static void teleopPeriodic() {
	double currentTime = Utility.getFPGATime();

	// if not enough time has passed, no polling allowed!
	if ((currentTime - initTriggerTime) < TRIGGER_CYCLE_WAIT_US)
		return;

	if (gamepad.getRawButton(FIRE_BUTTON))  {
		fire();
		
		// reset trigger init time
		initTriggerTime = Utility.getFPGATime();
	}
	
	if (gamepad.getRawButton(HOLD_BUTTON)) {
		hold();	
		
		// reset trigger init time
		initTriggerTime = Utility.getFPGATime();
	}
	
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
private static void checkShooterControls() {
	// fire controls - using a timer to debounce
	double currentTime = Utility.getFPGATime();

	// if not enough time has passed, no polling allowed!
	if ((currentTime - initTriggerTime) < TRIGGER_CYCLE_WAIT_US)
		return;

	// shooter commands
	if (gamepad.getRawButton(HardwareIDs.FIRE_HIGH_BUTTON))
		setShooterStrength(MOTOR_HIGH);			
	
	if (gamepad.getRawButton(HardwareIDs.FIRE_MEDIUM_BUTTON))
		setShooterStrength(MOTOR_MEDIUM);			

	if (gamepad.getRawButton(HardwareIDs.FIRE_LOW_BUTTON))
		setShooterStrength(MOTOR_LOW);			
	
	if (gamepad.getRawButton(HardwareIDs.HOLD_BUTTON))
		setShooterStrength(MOTOR_OFF);
	
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public static void teleopInit() {
			
	gearTrayOn();
	//collectorOff();    // keep collector off until gamepad control pressed
	resetMotors();
	
	// spawn a wait thread to turn relays back off after a number of seconds
	/*
	new Thread() {
		public void run() {
			try {
				Thread.sleep(3000);  // wait a number of sec before starting to feed
				gearTrayOff();	 	 // turn relays off
			} catch (Exception e) {
				e.printStackTrace();
			}
		}
	}.start();
	*/
	
       initTriggerTime = Utility.getFPGATime();
       
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public boolean isTriggered()
{	
	double gyroAngle = getGyroAngle();
	
	if (Math.abs(gyroAngle - targetAngleDeg) > errorDeg) {
		
		// outside error range...
		// reset timer and return false
		startTimeUs = Utility.getFPGATime();	
		return false;
	}
	
	long currentTimeUs = Utility.getFPGATime();
	double delta = (currentTimeUs - startTimeUs)/1e6;
	//System.out.println("delta = " + delta + " duration = " + durationSec);
	
	if (delta < durationSec)
	{
		// within error range, but not for enough time
		return false;
	}
	
	System.out.println("ClosedLoopAngleEvent triggered!");
	return true;
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public void updateMotorRPM() { 
	long currentTimeMicroSeconds = Utility.getFPGATime();
	long deltaTimeMicroSeconds = currentTimeMicroSeconds - m_lastTimeMicroSeconds;
	m_lastTimeMicroSeconds = currentTimeMicroSeconds;
	
	// RPM = counts/microsec * 1000000 microsec/sec * 60 sec/min * 1/counts_per_rev
	double replacedMotorRPM = m_motorRPM[m_rpmIndex];
	double currentRPM = (m_encoder.get() / (double)deltaTimeMicroSeconds) * 60000000.0 / m_pulsesPerRotation;
    if (currentRPM > MAX_RPM) {
        currentRPM = MAX_RPM;
    }
    m_motorRPM[m_rpmIndex] = currentRPM;
    m_encoder.reset();
 
	m_averagedMotorRPM += (m_motorRPM[m_rpmIndex] - replacedMotorRPM) / m_numAveragedCycles;
	
    m_rpmIndex++;
	if (m_rpmIndex == m_numAveragedCycles) {
        m_rpmIndex = 0;
	}
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
protected boolean isFinished() 
{
    log("Gyro Angle:      "+driveTrain.getGyro().getAngle() + "                 bearing: "+bearing);
    
    //if by time
    if (driveTime > 0 && Utility.getFPGATime() >= startTime + driveTime)
    {
        driveTrain.tankDrive(0, 0);
        state = FINISHED;
        return true;
    }
    else if (distance > 0 && driveTrain.getDistanceToWall() <= distance && lockedOnIterations >= 3)
    {
        return true;
    }
    
    return false;
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
protected boolean isFinished() 
{
    
    //if by time
    if (driveTime > 0 && Utility.getFPGATime() >= startTime + driveTime)
    {
        driveTrain.tankDrive(0, 0);
        state = FINISHED;
        return true;
    }
    else if (distance > 0 && driveTrain.getDistanceToWall() <= distance && lockedOnIterations >= 3)
    {
        return true;
    }
    
    return false;
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public void start(double period, boolean periodic) {
  synchronized (m_processLock) {
    m_periodic = periodic;
    m_period = period;
    m_expirationTime = Utility.getFPGATime() * 1e-6 + m_period;
    updateAlarm();
  }
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public void initialize()
{
	//System.out.println("TimeEvent initialized!");
	startTimeUs = Utility.getFPGATime();
	
	super.initialize();
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public boolean isTriggered()
{
	long currentTimeUs = Utility.getFPGATime();
	double delta = (currentTimeUs - startTimeUs)/1e6;
	//System.out.println("delta = " + delta + " duration = " + durationSec);
	
	if (delta > durationSec)
	{
		System.out.println("TimeEvent triggered!");
		return true;
	}
	
	return false;
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public static void autoInit() {
			
	gearTrayOff();
	//collectorOff();
	resetMotors();
	
       initTriggerTime = Utility.getFPGATime();
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public static void teleopPeriodic() {

		// fire controls - using a timer to debounce
		double currentTime = Utility.getFPGATime();

		// if not enough time has passed, no polling allowed!
		if ((currentTime - initTriggerTime) < TRIGGER_CYCLE_WAIT_US)
			return;
		
		double currentPos = positionServo.get();
		
		// switch to control camera servo
		if (gamepad.getRawButton(HardwareIDs.CAMERA_CONTROL_BUTTON) == true)
		{
			if (Math.abs(currentPos - BOILER_CAM_POS) > SERVO_POS_TOLERANCE)
				moveToPos(BOILER_CAM_POS);
		}
		else
		{
			if (Math.abs(currentPos - GEAR_CAM_POS) > SERVO_POS_TOLERANCE)
				moveToPos(GEAR_CAM_POS);
		}

		// button to strobe camera LED rings
		if ((gamepad.getRawButton(HardwareIDs.CAMERA_LED_STROBE_BUTTON) == true) && (!ledState))
		{
			setCameraLed(true);
		}
		else if ((gamepad.getRawButton(HardwareIDs.CAMERA_LED_STROBE_BUTTON) == false) && (ledState))
		{
			setCameraLed(false);
		}
		
		// reset trigger init time
		initTriggerTime = Utility.getFPGATime();		

	}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public void initialize()
{
	//System.out.println("GyroAngleEvent initialized!");
	
	startTimeUs = Utility.getFPGATime();
	
	super.initialize();
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public void initialize()
{
	//System.out.println("ClosedLoopPositionEvent initialized!");
	startTimeUs = Utility.getFPGATime();
	
	super.initialize();
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public boolean isTriggered()
{		
	// measure current position error
	double actualPosInches = AutoDriveAssembly.getDistanceInches();
	double errorPosInches = Math.abs(targetPosInches - actualPosInches);
	if (errorPosInches > errorThresholdInches)
	{
		// outside error range...
		// reset timer
		startTimeUs = Utility.getFPGATime();
		return false;
	}

	long currentTimeUs = Utility.getFPGATime();
	double delta = (currentTimeUs - startTimeUs)/1e6;
	//System.out.println("delta = " + delta + " duration = " + durationSec);
	
	if (delta < durationSec)
	{
		// within error range, but not for enough time
		return false;
	}
	
	// within error range for enough time
	System.out.println("ClosedLoopPositionEvent triggered!");
	return true;
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
/**
 * Create a new time system that uses the FPGA clock. At this time, the precision of the resulting clock has not been
 * verified or tested.
 *
 * @return the FPGA-based clock; never null
 * @throws StrongbackRequirementException if the FPGA hardware is not available
 */
public static Clock fpga() {
    try {
        Utility.getFPGATime();
        // If we're here, then the method did not throw an exception and there is FPGA hardware on this platform ...
        return Utility::getFPGATime;
    } catch (UnsatisfiedLinkError | NoClassDefFoundError e) {
        throw new StrongbackRequirementException("Missing FPGA hardware or software", e);
    }
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public void resetMotorRPM() {
    for (int i = 0; i < m_numAveragedCycles; i++) {
    	m_motorRPM[i] = 0.0;
    }
    m_lastTimeMicroSeconds = Utility.getFPGATime();
    m_averagedMotorRPM = 0;
    m_encoder.start();
}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
public void setMoveMotorSpeed(double percentVbus) {
        double deltaTimeMicroSeconds = Utility.getFPGATime() - m_startTimeMicroSeconds;
        double deltaTimeSeconds = deltaTimeMicroSeconds / 1000000;

        percentVbus = limitAccel(percentVbus, m_maxSpeed, deltaTimeSeconds);
        double steeringError = m_rightEncoder.getDistance() - m_leftEncoder.getDistance();
//        double steeringError = m_yawGyro.getAngle();
        double steerOffset = STEERING_ZERO_OFFSET;
//        if (percentVbus > 0) {  // neg = forward
//            steerOffset = -steerOffset;
//        }
        m_drive.arcadeDrive(percentVbus, steerOffset + steeringError * KP_MOVE_STEERING);
//        System.out.println("Time = " + System.currentTimeMillis() + ", PercentVbus = " + percentVbus + ", Distance = " + returnPIDInput() + ", Steer error = " + steeringError + ", Left Distance = " + m_leftEncoder.getDistance() + ", Right Distance = " + m_rightEncoder.getDistance());
    }
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
protected void initialize() 
{
    setSpeed = .8;
    scaleFactor = 1;
    
    //driveTrain.getGyro().reset();
    bearing = driveTrain.getGyro().getAngle();
    startTime = Utility.getFPGATime();      //returns fpga time in MICROseconds.
    
    DRIVE_SPEED = Double.parseDouble(BadPreferences.getValue(driveSpeedKey, ".6"));
    delayTime = Double.parseDouble(BadPreferences.getValue("DRIVE_STRAIGHT_FORWARD_WITH_DISTANCE_DELAY", "2.4"));

}
 

import edu.wpi.first.wpilibj.Utility; //导入依赖的package包/类
protected void execute() 
{
    //with time
    if (distance <= 0)
    {
   
        driveTrain.getTrain().drive(-DRIVE_SPEED,
                -(driveTrain.getGyro().getAngle() - bearing) * Kp);
    }
    
    //with distance
    else
    {
        if (driveTrain.getDistanceToWall() < distance && Utility.getFPGATime() - startTime > delayTime*1000000)
        { 
            lockedOnIterations++;  
        }
        else
        { 
            if (lockedOnIterations > 0)
                lockedOnIterations = 0;

                driveTrain.getTrain().drive(DRIVE_SPEED, 
                        -(driveTrain.getGyro().getAngle() - bearing) * Kp);
            
        }
    }
}