diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java index af50dc8589..7e914fcc7a 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -348,6 +348,6 @@ public class MecanumControllerCommand extends CommandBase { @Override public boolean isFinished() { - return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds()); + return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds()); } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java index eea06c7c6e..c6a2b4c6bf 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -201,6 +201,6 @@ public class RamseteCommand extends CommandBase { @Override public boolean isFinished() { - return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds()); + return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds()); } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java index b05dc201f0..453fcfda38 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -153,6 +153,6 @@ public class SwerveControllerCommand extends CommandBase { @Override public boolean isFinished() { - return m_timer.hasPeriodPassed(m_trajectory.getTotalTimeSeconds()); + return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds()); } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java index 85ddd03f76..0e645c6e8a 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -59,6 +59,6 @@ public class TrapezoidProfileCommand extends CommandBase { @Override public boolean isFinished() { - return m_timer.hasPeriodPassed(m_profile.totalTime()); + return m_timer.hasElapsed(m_profile.totalTime()); } } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java index 5e8ebc3ea3..c53fb42789 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -41,7 +41,7 @@ public class WaitCommand extends CommandBase { @Override public boolean isFinished() { - return m_timer.hasPeriodPassed(m_duration); + return m_timer.hasElapsed(m_duration); } @Override diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp index 88098037e7..8edc935e6a 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp @@ -247,5 +247,5 @@ void MecanumControllerCommand::Execute() { void MecanumControllerCommand::End(bool interrupted) { m_timer.Stop(); } bool MecanumControllerCommand::IsFinished() { - return m_timer.HasPeriodPassed(m_trajectory.TotalTime()); + return m_timer.HasElapsed(m_trajectory.TotalTime()); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp index b1608bfe69..e2c56aba12 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp @@ -138,5 +138,5 @@ void RamseteCommand::Execute() { void RamseteCommand::End(bool interrupted) { m_timer.Stop(); } bool RamseteCommand::IsFinished() { - return m_timer.HasPeriodPassed(m_trajectory.TotalTime()); + return m_timer.HasElapsed(m_trajectory.TotalTime()); } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp index 1279d66986..5c49bee26d 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -21,6 +21,6 @@ void WaitCommand::Initialize() { void WaitCommand::End(bool interrupted) { m_timer.Stop(); } -bool WaitCommand::IsFinished() { return m_timer.HasPeriodPassed(m_duration); } +bool WaitCommand::IsFinished() { return m_timer.HasElapsed(m_duration); } bool WaitCommand::RunsWhenDisabled() const { return true; } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc index 42d726d316..da3daf568f 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc +++ b/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc @@ -103,7 +103,7 @@ void SwerveControllerCommand::End(bool interrupted) { template bool SwerveControllerCommand::IsFinished() { - return m_timer.HasPeriodPassed(m_trajectory.TotalTime()); + return m_timer.HasElapsed(m_trajectory.TotalTime()); } } // namespace frc2 diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h index a83dc39a37..70bf7f947b 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h @@ -72,7 +72,7 @@ class TrapezoidProfileCommand void End(bool interrupted) override { m_timer.Stop(); } bool IsFinished() override { - return m_timer.HasPeriodPassed(m_profile.TotalTime()); + return m_timer.HasElapsed(m_profile.TotalTime()); } private: diff --git a/wpilibc/src/main/native/cpp/frc2/Timer.cpp b/wpilibc/src/main/native/cpp/frc2/Timer.cpp index 76721bc129..b9664237df 100644 --- a/wpilibc/src/main/native/cpp/frc2/Timer.cpp +++ b/wpilibc/src/main/native/cpp/frc2/Timer.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2008-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -116,15 +116,22 @@ void Timer::Stop() { } } +bool Timer::HasElapsed(units::second_t period) { return Get() > period; } + bool Timer::HasPeriodPassed(units::second_t period) { + return AdvanceIfElapsed(period); +} + +bool Timer::AdvanceIfElapsed(units::second_t period) { if (Get() > period) { std::scoped_lock lock(m_mutex); // Advance the start time by the period. m_startTime += period; // Don't set it to the current time... we want to avoid drift. return true; + } else { + return false; } - return false; } units::second_t Timer::GetFPGATimestamp() { diff --git a/wpilibc/src/main/native/include/frc2/Timer.h b/wpilibc/src/main/native/include/frc2/Timer.h index c1eeb16665..a6a04b1a9f 100644 --- a/wpilibc/src/main/native/include/frc2/Timer.h +++ b/wpilibc/src/main/native/include/frc2/Timer.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2008-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -88,6 +88,14 @@ class Timer { */ void Stop(); + /** + * Check if the period specified has passed. + * + * @param seconds The period to check. + * @return True if the period has passed. + */ + bool HasElapsed(units::second_t period); + /** * Check if the period specified has passed and if it has, advance the start * time by that period. This is useful to decide if it's time to do periodic @@ -98,6 +106,16 @@ class Timer { */ bool HasPeriodPassed(units::second_t period); + /** + * Check if the period specified has passed and if it has, advance the start + * time by that period. This is useful to decide if it's time to do periodic + * work without drifting later by the time it took to get around to checking. + * + * @param period The period to check for. + * @return True if the period has passed. + */ + bool AdvanceIfElapsed(units::second_t period); + /** * Return the FPGA system clock time in seconds. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java index e1e312f429..514916b0b4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2016-2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2016-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -104,21 +104,44 @@ public class Timer { m_running = false; } + /** + * Check if the period specified has passed. + * + * @param seconds The period to check. + * @return Whether the period has passed. + */ + public synchronized boolean hasElapsed(double seconds) { + return get() > seconds; + } + /** * Check if the period specified has passed and if it has, advance the start time by that period. * This is useful to decide if it's time to do periodic work without drifting later by the time it * took to get around to checking. * * @param period The period to check for (in seconds). - * @return If the period has passed. + * @return Whether the period has passed. */ public synchronized boolean hasPeriodPassed(double period) { - if (get() > period) { + return advanceIfElapsed(period); + } + + /** + * Check if the period specified has passed and if it has, advance the start time by that period. + * This is useful to decide if it's time to do periodic work without drifting later by the time it + * took to get around to checking. + * + * @param seconds The period to check. + * @return Whether the period has passed. + */ + public synchronized boolean advanceIfElapsed(double seconds) { + if (get() > seconds) { // Advance the start time by the period. // Don't set it to the current time... we want to avoid drift. - m_startTime += period * 1000; + m_startTime += seconds * 1000; return true; + } else { + return false; } - return false; } }