Timer.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
 
package org.wpilib.system;
 
import static org.wpilib.units.Units.Seconds;
 
import org.wpilib.driverstation.MatchState;
import org.wpilib.units.measure.Time;

public class Timer {
  public static double getTimestamp() {
    return RobotController.getTime() / 1_000_000_000.0;
  }

  public static double getMonotonicTimestamp() {
    return RobotController.getMonotonicTime() / 1_000_000_000.0;
  }

  public static double getMatchTime() {
    return MatchState.getMatchTime();
  }

  public static void delay(final Time period) {
    delay(period.in(Seconds));
  }

  public static void delay(final double seconds) {
    try {
      Thread.sleep((long) (seconds * 1e3));
    } catch (final InterruptedException ex) {
      Thread.currentThread().interrupt();
    }
  }
 
  private long m_startTimeNs;
  private double m_startTimeRemainderNs;
  private double m_accumulatedTimeNs;
  private boolean m_running;

  public Timer() {
    reset();
  }

  public static Timer createStarted() {
    var timer = new Timer();
    timer.start();
    return timer;
  }
 
  private long getNsClock() {
    return RobotController.getTime();
  }
 
  private double getNanoseconds() {
    if (m_running) {
      return getNsClock() - m_startTimeNs - m_startTimeRemainderNs + m_accumulatedTimeNs;
    } else {
      return m_accumulatedTimeNs;
    }
  }

  public double get() {
    return getNanoseconds() / 1_000_000_000.0;
  }

  public final void reset() {
    m_accumulatedTimeNs = 0.0;
    m_startTimeNs = getNsClock();
    m_startTimeRemainderNs = 0.0;
  }

  public void start() {
    if (!m_running) {
      m_startTimeNs = getNsClock();
      m_startTimeRemainderNs = 0.0;
      m_running = true;
    }
  }

  public void restart() {
    if (m_running) {
      stop();
    }
    reset();
    start();
  }

  public void stop() {
    if (m_running) {
      m_accumulatedTimeNs = getNanoseconds();
      m_running = false;
    }
  }

  public boolean hasElapsed(Time period) {
    return hasElapsed(period.in(Seconds));
  }

  public boolean hasElapsed(double seconds) {
    return getNanoseconds() >= seconds * 1e9;
  }

  public boolean advanceIfElapsed(double seconds) {
    double periodNs = seconds * 1e9;
 
    if (getNanoseconds() >= periodNs) {
      // Advance the start time by the period.
      // Don't set it to the current time... we want to avoid drift.
      double advanceNs = m_startTimeRemainderNs + periodNs;
      long wholeNs = (long) advanceNs;
      m_startTimeNs += wholeNs;
      m_startTimeRemainderNs = advanceNs - wholeNs;
      return true;
    } else {
      return false;
    }
  }

  public boolean isRunning() {
    return m_running;
  }
}