Java 类edu.wpi.first.wpilibj.Timer 实例源码

/**
 * {@inheritDoc}
 */
@Override
public void calibrate() {
  if (m_spi == null) return;

  Timer.delay(0.1);

  synchronized (this) {
    m_accum_count = 0;
    m_accum_gyro_x = 0.0;
    m_accum_gyro_y = 0.0;
    m_accum_gyro_z = 0.0;
  }

  Timer.delay(kCalibrationSampleTime);

  synchronized (this) {
    m_gyro_offset_x = m_accum_gyro_x / m_accum_count;
    m_gyro_offset_y = m_accum_gyro_y / m_accum_count;
    m_gyro_offset_z = m_accum_gyro_z / m_accum_count;
  }
}

protected boolean isFinished() {
    double wheelSpeedR;
    double wheelSpeedL;
    double elapsedTime;
    wheelSpeedL = RobotMap.driveTrainLeftFront.getSpeed();
    wheelSpeedR = RobotMap.driveTrainRightFront.getSpeed();
    elapsedTime = Timer.getFPGATimestamp() - startTime;
    if (elapsedTime > 1 && (wheelSpeedL <= .001 && wheelSpeedR <= .001))
    {
        //System.out.println("VISION stop by Speed");
        return true;
    }
    else if  (elapsedTime > endTime)
    {
        //System.out.println("VISION Stop by timeout");
        return true;
    }
    return false;
    //return (elapsedTime > 1 && (wheelSpeedL <= .001 && wheelSpeedR <= .001)) || (elapsedTime > endTime); //|| (NavX.ahrs.getWorldLinearAccelY() < -1); //-0.8);

}

protected void initialize() {
    RobotMap.driveTrainRightFront.setPosition(0);
    RobotMap.driveTrainLeftFront.setPosition(0);
RobotMap.driveTrainLeftFront.setEncPosition(0);
RobotMap.driveTrainRightFront.setEncPosition(0);
    startTime = Timer.getFPGATimestamp();
    maxAccel = 0;
    minAccel = 0;
    dispCount = 0;
    //initRightEnc = DriveEncoders.getRawRightValue();
    //initLeftEnc = DriveEncoders.getRawLeftValue();
    //prevTime = System.currentTimeMillis();
    //prevRightError = 0;
    //prevLeftError = 0;
    //SmartDashboard.putString("Auton Debugging", "DriveForwardInit");
    }

@Override
public void perform() {
  terminal.writer().println();
  terminal.writer().println(Messages.bold(NAME));
  terminal.writer().println();
  DigitalOutput digitalOutput = dioSet.getDigitalOutput();
  if (digitalOutput == null) {
    terminal.writer().println(Messages.boldRed("no digital output selected"));
    return;
  }
  digitalOutput.disablePWM();
  digitalOutput.enablePWM(0.25);
  Timer.delay(SLEEP_SEC);
  digitalOutput.updateDutyCycle(0.5);
  Timer.delay(SLEEP_SEC);
  digitalOutput.updateDutyCycle(0.75);
  Timer.delay(SLEEP_SEC);
  digitalOutput.updateDutyCycle(1.0);
}

public void run()
{
    double lastWriteTime = Timer.getFPGATimestamp();
    while (true)
    {
        try
        {
            String msg = logMessages.take();
            writer.write(msg);
            if (Timer.getFPGATimestamp() >= lastWriteTime + WRITE_TIME)
            {
                writer.flush();
                lastWriteTime = Timer.getFPGATimestamp();
            }
        }
        catch (InterruptedException | IOException e)
        {
            ((Throwable) e).printStackTrace();
        }
    }
}

public void run()
{
    double lastWriteTime = Timer.getFPGATimestamp();
    while (true)
    {
        try
        {
            String msg = logMessages.take();
            writer.write(msg);
            if (Timer.getFPGATimestamp() >= lastWriteTime + WRITE_TIME)
            {
                writer.flush();
                lastWriteTime = Timer.getFPGATimestamp();
            }
        }
        catch (InterruptedException | IOException e)
        {
            ((Throwable) e).printStackTrace();
        }
    }
}

@Override
public void runCrashTracked() {
    synchronized (mTaskRunningLock) {
        if (mRunning) {
            double now = Timer.getFPGATimestamp();
            Commands commands = Robot.getCommands();
            RobotState robotState = Robot.getRobotState();
            for (SubsystemLoop loop : mLoops) {
                loop.update(commands, robotState);
                Logger.getInstance().logSubsystemThread(loop.getStatus());
            }
            mDt = now - mTimeStamp;
            mTimeStamp = now;
        }
    }
}

public synchronized void start() {
    if (!mRunning) {
        System.out.println("Starting loops");
        synchronized (mTaskRunningLock) {
            mTimeStamp = Timer.getFPGATimestamp();
            for (SubsystemLoop loop : mLoops) {
                System.out.println("Starting " + loop.toString());
                loop.start();
            }
            mRunning = true;
        }
        mNotifier.startPeriodic(kPeriod);
    } else {
        System.out.println("SubsystemLooper already running");
    }
    if (!mPrinting) {
        System.out.println("Starting subsystem printer");
        mPrintTimeStamp = Timer.getFPGATimestamp();
        mPrinting = true;
        mPrintNotifier.startPeriodic(kPrintRate);
    }
}

public static void updateValues() {

    if (!initialized)
        return;

    // Default data if network table data pull fails
double defaultDoubleVal = 0.0;

// Pull data from grip
numTargets = table.getNumber("targets", defaultDoubleVal);
targetX = table.getNumber("targetX", defaultDoubleVal);
targetY = table.getNumber("targetY", defaultDoubleVal);
targetArea = table.getNumber("targetArea",defaultDoubleVal);
targetDistance = table.getNumber("targetDistance",defaultDoubleVal);

Timer.delay(0.02);
  }

@Override
public void run() {
    double current = 0;
    final double filter = 0.1666667;
    final double max = 30; // 30 Amps should not be violated in most cases

    boolean isDisabled = false;

    while (!Thread.currentThread().isInterrupted()) {
        // Rolling Avg.
        current = filter * getOutputCurrent() + current * (1 - filter);
        if (current > max * 0.9 && !isDisabled) {
            disableControl();
            isDisabled = true;
        }else if (current < max * 0.80 && isDisabled){
            enableControl();
            isDisabled = false;
        }
        Timer.delay(0.1);
    }
}

public void Init()
{
    Logger.PrintLine("Init 1",Logger.LOGGER_MESSAGE_LEVEL_DEBUG);

    m_pidController = new PIDController(10,10,10,m_encoderAverager,m_robotDrivePid);
    m_pidController.setOutputRange(-0.8,0.8);
    //m_pidController.setOutputRange(-0.4,0.4);
    Logger.PrintLine("Init 4",Logger.LOGGER_MESSAGE_LEVEL_ERROR);

    m_currentStateIndex = 0;
    SetCurrentState(m_nextStateArray[m_currentStateIndex]);
    m_disabled = false;

    m_shootingBall = false;
    m_driving = false;
    m_braking = false;
    m_detectingImage = false;
    m_currentStateIndex = 0;
    m_loadingBall = false;
    m_shooterPullingBack = false;

    m_autonomousStartTime = Timer.getFPGATimestamp();

    m_robotDrivePid.resetGyro();
}

private void HandleStateRelease()
{
    if (!m_gearReleased)
    {
        // set the gear in neutral
        m_gearControl.set(DoubleSolenoid.Value.kForward);
        m_gearReleased = true;
    }

    if(!m_latchReleased)
    {
        //release the latch
        m_latchReleaseServo.set(1);
        Timer.delay(0.5);
        m_latchReleased = true;
    }

    m_isPulledBack = false;
    m_currentState = SHOOTER_CONTROL_STATE_WAIT;
}

/**
 * Initialize the gyro. Calibration is handled by calibrate().
 */
public void initGyro() {
  result = new AccumulatorResult();

  m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
  m_analog.setAverageBits(kAverageBits);
  m_analog.setOversampleBits(kOversampleBits);
  double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
  AnalogInput.setGlobalSampleRate(sampleRate);
  Timer.delay(0.1);

  setDeadband(0.0);

  setPIDSourceType(PIDSourceType.kDisplacement);

  UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
  LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this);
}

public static void updateValues() {

    if (!initialized)
        return;

    // Default data if network table data pull fails
double defaultDoubleVal = 0.0;

// Pull data from grip
numTargets = table.getNumber("targets", defaultDoubleVal);
targetX = table.getNumber("targetX", defaultDoubleVal);
targetY = table.getNumber("targetY", defaultDoubleVal);
targetArea = table.getNumber("targetArea",defaultDoubleVal);
targetDistance = table.getNumber("targetDistance",defaultDoubleVal);

Timer.delay(0.02);
  }

/**
 * {@inheritDoc}
 */
@Override
public void calibrate() {
  if (m_spi == null) return;

  Timer.delay(0.1);

  synchronized (this) {
    m_accum_count = 0;
    m_accum_gyro_x = 0.0;
    m_accum_gyro_y = 0.0;
    m_accum_gyro_z = 0.0;
  }

  Timer.delay(kCalibrationSampleTime);

  synchronized (this) {
    m_gyro_center_x = m_accum_gyro_x / m_accum_count;
    m_gyro_center_y = m_accum_gyro_y / m_accum_count;
    m_gyro_center_z = m_accum_gyro_z / m_accum_count;
  }
}

public static void reset()
{
    System.out.println("NavXSensor::reset called!");

    if (ahrs != null) 
    {
        ahrs.reset();
        ahrs.resetDisplacement();
        ahrs.zeroYaw();

        // allow zeroing to take effect
        Timer.delay(0.1);

        // get the absolute angle after reset - Not sure why it is non-zero, but we need to record it to zero it out
        yawOffset = ahrs.getAngle();    
        System.out.println("yawOffset read = " + yawOffset);
    }
}

protected void execute() {
        //check to see limit switch 
//      if(direction.equals("in") && Robot.intakeRollerSubsystem.getOverride())
//          Robot.intakeRollerSubsystem.intake();
//      else if(direction.equals("in") && RobotMap.shooterEncoder.getRate() > 0)
//          Robot.intakeRollerSubsystem.intake();
//      else if (direction.equals("in") && buttonsNotPressed())
//          Robot.intakeRollerSubsystem.intake();
        if(direction.equals("in"))
            Robot.intakeRollerSubsystem.intake();
        else if(direction.equalsIgnoreCase("out"))
            Robot.intakeRollerSubsystem.outake();
        else if(direction.equalsIgnoreCase("auton")){
            Robot.intakeRollerSubsystem.outake();
            Timer.delay(1);
        }
        else
            Robot.intakeRollerSubsystem.neutral();
    }

/**
 * This autonomous (along with the chooser code above) shows how to select
 * between different autonomous modes using the dashboard. The sendable
 * chooser code works with the Java SmartDashboard. If you prefer the
 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
 * getString line to get the auto name from the text box below the Gyro
 *
 * You can add additional auto modes by adding additional comparisons to the
 * if-else structure below with additional strings. If using the
 * SendableChooser make sure to add them to the chooser code above as well.
 */
@Override
public void autonomous() {
    String autoSelected = chooser.getSelected();
    // String autoSelected = SmartDashboard.getString("Auto Selector",
    // defaultAuto);
    System.out.println("Auto selected: " + autoSelected);

    switch (autoSelected) {
    case customAuto:
        myRobot.setSafetyEnabled(false);
        myRobot.drive(-0.5, 1.0); // spin at half speed
        Timer.delay(2.0); // for 2 seconds
        myRobot.drive(0.0, 0.0); // stop robot
        break;
    case defaultAuto:
    default:
        myRobot.setSafetyEnabled(false);
        myRobot.drive(-0.5, 0.0); // drive forwards half speed
        Timer.delay(2.0); // for 2 seconds
        myRobot.drive(0.0, 0.0); // stop robot
        break;
    }
}

public SimulatorFrame()
{
    initComponenents();
    setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);

    RobotStateSingleton.get().addLoopListener(new RobotStateSingleton.LoopListener()
    {

        @Override
        public void looped()
        {
            mBasicPanel.update();
            mEnablePanel.setTime(Timer.getMatchTime());
        }
    });
}

public static void reset()
{
    System.out.println("NavXSensor::reset called!");

    if (ahrs != null) 
    {
        ahrs.reset();
        ahrs.resetDisplacement();
        ahrs.zeroYaw();

        // allow zeroing to take effect
        Timer.delay(0.1);

        // get the absolute angle after reset - Not sure why it is non-zero, but we need to record it to zero it out
        yawOffset = ahrs.getAngle();    
        System.out.println("yawOffset read = " + yawOffset);
    }
}

/**
 * This autonomous (along with the chooser code above) shows how to select
 * between different autonomous modes using the dashboard. The sendable
 * chooser code works with the Java SmartDashboard. If you prefer the
 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
 * getString line to get the auto name from the text box below the Gyro
 *
 * You can add additional auto modes by adding additional comparisons to the
 * if-else structure below with additional strings. If using the
 * SendableChooser make sure to add them to the chooser code above as well.
 */
@Override
public void autonomous() {
    String autoSelected = chooser.getSelected();
    // String autoSelected = SmartDashboard.getString("Auto Selector",
    // defaultAuto);
    System.out.println("Auto selected: " + autoSelected);

    switch (autoSelected) {
    case customAuto:
        myRobot.setSafetyEnabled(false);
        myRobot.drive(-0.5, 1.0); // spin at half speed
        Timer.delay(2.0); // for 2 seconds
        myRobot.drive(0.0, 0.0); // stop robot
        break;
    case defaultAuto:
    default:
        myRobot.setSafetyEnabled(false);
        myRobot.drive(-0.5, 0.0); // drive forwards half speed
        Timer.delay(2.0); // for 2 seconds
        myRobot.drive(0.0, 0.0); // stop robot
        break;
    }
}

@Override
protected void initialize() {
    //// CHANGE FOR PRACTICE TO COMPETITION DONE DONE DONE DOOOONNNEE
    //////////////////////// CHANGE DURING COMP: NEED TO FIND RIGHT ENCODERS
    if (RobotMap.armMotor.getEncPosition() < 12){
    //  latchCylinder.cylinder(true);
    //Timer.delay(2);
    //latchCylinder.cylinder(false);
    latchCylinder.cylinder(true);
    Timer.delay(.5);
    latchCylinder.cylinder(false);

    }
    //latchCylinder.cylinder(DoubleSolenoid.Value.kReverse);
    //Timer.delay(1);
    //latchCylinder.cylinder(DoubleSolenoid.Value.kForward);
    //// CHANGE FOR PRACTICE TO COMPETITION END
}

/**
 * This autonomous (along with the chooser code above) shows how to select
 * between different autonomous modes using the dashboard. The sendable
 * chooser code works with the Java SmartDashboard. If you prefer the
 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
 * getString line to get the auto name from the text box below the Gyro
 *
 * You can add additional auto modes by adding additional comparisons to the
 * if-else structure below with additional strings. If using the
 * SendableChooser make sure to add them to the chooser code above as well.
 */
@Override
public void autonomous() {
    String autoSelected = chooser.getSelected();
    // String autoSelected = SmartDashboard.getString("Auto Selector",
    // defaultAuto);
    System.out.println("Auto selected: " + autoSelected);

    switch (autoSelected) {
    case customAuto:
        myRobot.setSafetyEnabled(false);
        myRobot.drive(-0.5, 1.0); // spin at half speed
        Timer.delay(2.0); // for 2 seconds
        myRobot.drive(0.0, 0.0); // stop robot
        break;
    case defaultAuto:
    default:
        myRobot.setSafetyEnabled(false);
        myRobot.drive(-0.5, 0.0); // drive forwards half speed
        Timer.delay(2.0); // for 2 seconds
        myRobot.drive(0.0, 0.0); // stop robot
        break;
    }
}

public synchronized void run() {
  Ultrasonic u = null;
  while (m_automaticEnabled) {
    if (u == null) {
      u = m_firstSensor;
    }
    if (u == null) {
      return;
    }
    if (u.isEnabled()) {
      u.m_pingChannel.pulse(m_pingChannel.m_channel, (float) kPingTime); // do
                                                                         // the
                                                                         // ping
    }
    u = u.m_nextSensor;
    Timer.delay(.1); // wait for ping to return
  }
}

@Override
public void init()
    {

        RobotMap.sL.SystemLoggerWriteTimeline("Distance_MovePID_Init");
        brakeTimer = new Timer();
        done = false;
        P = .35;

        // System.out.println("in Init");
        //
        // I = 0; D = 0; sumL = 0; sumR = 0; prevTime = 0; prevSpeed = 0;
        //
        //
        // distanceR += (RobotMap.right1.getEncPosition() / Constants.encoderTickPerFoot);
        // P = P / distanceL;
        // timer.reset();
        // timer.start();
    }

/**
 * This autonomous (along with the chooser code above) shows how to select
 * between different autonomous modes using the dashboard. The sendable
 * chooser code works with the Java SmartDashboard. If you prefer the
 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
 * getString line to get the auto name from the text box below the Gyro
 *
 * You can add additional auto modes by adding additional comparisons to the
 * if-else structure below with additional strings. If using the
 * SendableChooser make sure to add them to the chooser code above as well.
 */
@Override
public void autonomous() {
    String autoSelected = chooser.getSelected();
    // String autoSelected = SmartDashboard.getString("Auto Selector",
    // defaultAuto);
    System.out.println("Auto selected: " + autoSelected);
    gyro.reset();
    switch (autoSelected) {
    case customAuto:
        myRobot.setSafetyEnabled(false);




        break;
    case defaultAuto:
    default:
        myRobot.setSafetyEnabled(false);
        myRobot.drive(-0.5, 0.0); // drive forwards half speed
        Timer.delay(2.0); // for 2 seconds
        myRobot.drive(0.0, 0.0); // stop robot
        break;
    }
}

@Override
protected void initialize() {
    Robot.drive.resetGyro();
    timer = new Timer();
    timer.reset();
    timer.start();
}

protected void execute() {
    double runTime = Timer.getFPGATimestamp() - startTime;


    if(runTime < 2) {
        RobotMap.driveTrainLeftFront.set(0.5);
        RobotMap.driveTrainRightFront.set(0.0);
        RobotMap.driveTrainLeftRear.set(0.0);
        RobotMap.driveTrainRightRear.set(0.0);
    } else if(runTime < 4) {
        RobotMap.driveTrainLeftFront.set(0.0);
        RobotMap.driveTrainRightFront.set(0.5);
        RobotMap.driveTrainLeftRear.set(0.0);
        RobotMap.driveTrainRightRear.set(0.0);
    } else if(runTime < 6) {
        RobotMap.driveTrainLeftFront.set(0.0);
        RobotMap.driveTrainRightFront.set(0.0);
        RobotMap.driveTrainLeftRear.set(0.5);
        RobotMap.driveTrainRightRear.set(0.0);
    } else if(runTime < 8) {
        RobotMap.driveTrainLeftFront.set(0.0);
        RobotMap.driveTrainRightFront.set(0.0);
        RobotMap.driveTrainLeftRear.set(0.0);
        RobotMap.driveTrainRightRear.set(0.5);
    } else {
        RobotMap.driveTrainLeftFront.set(0.0);
        RobotMap.driveTrainRightFront.set(0.0);
        RobotMap.driveTrainLeftRear.set(0.0);
        RobotMap.driveTrainRightRear.set(0.0);
    } 

}

protected boolean isFinished() {
    double runTime = Timer.getFPGATimestamp() - startTime;
    if(runTime < 8) {
        return false;
    }
    else {
        return true;
    }
}

protected void initialize() {

setInitEncoderVal(DriveEncoders.getAbsoluteValue());
SmartDashboard.putString("Auton Debugging", "DriveForwardInit");
System.out.println("DriveFowardInit");
startTime = Timer.getFPGATimestamp();
}

protected boolean isFinished() {


    if ( 124 < Robot.pixyValue || 130 > Robot.pixyValue)
    {
        return true;
    }
    else if ((Timer.getFPGATimestamp() - startTime) > 3)
        return true;
    return false;
    /*if (Robot.pixyInput.getAverageVoltage()  > .95 && Robot.pixyInput.getAverageVoltage() < 1.05)
        return true;
    return false;
    */
}

protected boolean isFinished() {
    if (Timer.getFPGATimestamp() - startTime > timeout) {
return true;
    }
    else {
        return false;
    }
 }

protected void initialize() {
    setInitEncoderVal(DriveEncoders.getAbsoluteValue());
    buffer = new byte[1];
    startTime = Timer.getFPGATimestamp();
Robot.pixyValue = (byte) 255;
dispCount = 0;

  }

protected void initialize() {
    RobotMap.driveTrainRightFront.setPosition(0);
    RobotMap.driveTrainLeftFront.setPosition(0);
RobotMap.driveTrainLeftFront.setEncPosition(0);
RobotMap.driveTrainRightFront.setEncPosition(0);
    startTime = Timer.getFPGATimestamp();
    maxAccel = 0;
    minAccel = 0;
    //initRightEnc = DriveEncoders.getRawRightValue();
    //initLeftEnc = DriveEncoders.getRawLeftValue();
    //prevTime = System.currentTimeMillis();
    //prevRightError = 0;
    //prevLeftError = 0;
    //SmartDashboard.putString("Auton Debugging", "DriveForwardInit");
    }

protected boolean isFinished() {
    if (Timer.getFPGATimestamp() - startTime > 5) {
        System.out.println("End for time");
    return true;
    }
else if (NavX.ahrs.getWorldLinearAccelY() < stopLevel && Math.abs(RobotMap.driveTrainRightFront.getEncPosition()) > distance) {
    System.out.println("end for Accel and Dist: Cur: " + minAccel + " " + RobotMap.driveTrainRightFront.getEncPosition() + " Dist: " + distance);
    return true;

}
    else 
        return false;
  }

protected boolean isFinished() {
    if (Timer.getFPGATimestamp() - startTime > timeout) {
        System.out.println("End for time");
    return true;
    }
    else if (NavX.ahrs.getWorldLinearAccelY() < stopLevel && Math.abs(RobotMap.driveTrainRightFront.getEncPosition()) > distance) {
    System.out.println("end for Accel and Dist: Cur:" + RobotMap.driveTrainRightFront.getEncPosition() + " Dist: " + distance + " Accel " + maxAccel);
    return true;

}
    else 
        return false;
  }

public static void intializeEncoders()
{
    RobotMap.driveTrainLeftFront.setEncPosition(0);
    RobotMap.driveTrainRightFront.setEncPosition(0);
    RobotMap.driveTrainRightFront.setPosition(0);
    RobotMap.driveTrainLeftFront.setPosition(0);
    System.out.println("OLD - RF: " + RobotMap.driveTrainRightFront.getEncPosition() + " LF:" + RobotMap.driveTrainLeftFront.getEncPosition());
    Timer.delay(0.04);
    System.out.println("NEW: RF: " + RobotMap.driveTrainRightFront.getEncPosition() + " LF:" + RobotMap.driveTrainLeftFront.getEncPosition());
}

public DriveForward(double power, double time) {

    requires(Robot.driveTrain);
    _timer = new Timer();
    _power = power;
    _time = time;

}

protected void execute() {
    double runTime = Timer.getFPGATimestamp() - startTime;

    if(runTime < 2) {
        WheelMotors.lFrontDrive.set(0.5);
        WheelMotors.rFrontDrive.set(0.0);
        WheelMotors.lRearDrive.set(0.0);
        WheelMotors.rRearDrive.set(0.0);
    } else if(runTime < 4) {
        WheelMotors.lFrontDrive.set(0.0);
        WheelMotors.rFrontDrive.set(0.5);
        WheelMotors.lRearDrive.set(0.0);
        WheelMotors.rRearDrive.set(0.0);
    } else if(runTime < 6) {
        WheelMotors.lFrontDrive.set(0.0);
        WheelMotors.rFrontDrive.set(0.0);
        WheelMotors.lRearDrive.set(0.5);
        WheelMotors.rRearDrive.set(0.0);
    } else if(runTime < 8) {
        WheelMotors.lFrontDrive.set(0.0);
        WheelMotors.rFrontDrive.set(0.0);
        WheelMotors.lRearDrive.set(0.0);
        WheelMotors.rRearDrive.set(0.5);
    } else {
        WheelMotors.lFrontDrive.set(0.0);
        WheelMotors.rFrontDrive.set(0.0);
        WheelMotors.lRearDrive.set(0.0);
        WheelMotors.rRearDrive.set(0.0);
    } 
}

protected boolean isFinished() {
    double runTime = Timer.getFPGATimestamp() - startTime;
    if(runTime < 8) {
        return false;
    }
    else {
        return true;
    }
}