diff --git a/wpilibc/src/main/native/cpp/Notifier.cpp b/wpilibc/src/main/native/cpp/Notifier.cpp index 9536acc7e10..aa78381551f 100644 --- a/wpilibc/src/main/native/cpp/Notifier.cpp +++ b/wpilibc/src/main/native/cpp/Notifier.cpp @@ -182,6 +182,10 @@ void Notifier::StartPeriodic(units::second_t period) { UpdateAlarm(); } +void Notifier::StartPeriodic(units::hertz_t frequency) { + StartPeriodic(1 / frequency); +} + void Notifier::Stop() { std::scoped_lock lock(m_processMutex); m_periodic = false; diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp index d2c39d12c5b..58b8c2cde17 100644 --- a/wpilibc/src/main/native/cpp/TimedRobot.cpp +++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp @@ -91,6 +91,8 @@ TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) { HALUsageReporting::kFramework_Timed); } +TimedRobot::TimedRobot(units::hertz_t frequency) : TimedRobot{1 / frequency} {} + TimedRobot::~TimedRobot() { if (m_notifier != HAL_kInvalidHandle) { int32_t status = 0; diff --git a/wpilibc/src/main/native/include/frc/Notifier.h b/wpilibc/src/main/native/include/frc/Notifier.h index 2841be48346..ea5b67fd58c 100644 --- a/wpilibc/src/main/native/include/frc/Notifier.h +++ b/wpilibc/src/main/native/include/frc/Notifier.h @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -107,6 +108,17 @@ class Notifier { */ void StartPeriodic(units::second_t period); + /** + * Run the callback periodically with the given frequency. + * + * The user-provided callback should be written so that it completes before + * the next time it's scheduled to run. + * + * @param frequency Frequency after which to call the callback starting one + * period after the call to this method. + */ + void StartPeriodic(units::hertz_t frequency); + /** * Stop further callback invocations. * diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h index 15afe34acb7..9d1e8ca1a16 100644 --- a/wpilibc/src/main/native/include/frc/TimedRobot.h +++ b/wpilibc/src/main/native/include/frc/TimedRobot.h @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -47,10 +48,17 @@ class TimedRobot : public IterativeRobotBase { /** * Constructor for TimedRobot. * - * @param period Period. + * @param period The period of the robot loop function. */ explicit TimedRobot(units::second_t period = kDefaultPeriod); + /** + * Constructor for TimedRobot. + * + * @param frequency The frequency of the robot loop function. + */ + explicit TimedRobot(units::hertz_t frequency); + TimedRobot(TimedRobot&&) = default; TimedRobot& operator=(TimedRobot&&) = default; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java index 54aeaddea16..f91a057a0b9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java @@ -4,9 +4,12 @@ package edu.wpi.first.wpilibj; +import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.hal.NotifierJNI; +import edu.wpi.first.units.measure.Frequency; +import edu.wpi.first.units.measure.Time; import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.locks.ReentrantLock; @@ -186,6 +189,15 @@ public void startSingle(double delaySeconds) { } } + /** + * Run the callback once after the given delay. + * + * @param delay Time to wait before the callback is called. + */ + public void startSingle(Time delay) { + startSingle(delay.in(Seconds)); + } + /** * Run the callback periodically with the given period. * @@ -207,6 +219,32 @@ public void startPeriodic(double periodSeconds) { } } + /** + * Run the callback periodically with the given period. + * + *

The user-provided callback should be written so that it completes before the next time it's + * scheduled to run. + * + * @param period Period after which to call the callback starting one period after the call to + * this method. + */ + public void startPeriodic(Time period) { + startPeriodic(period.in(Seconds)); + } + + /** + * Run the callback periodically with the given frequency. + * + *

The user-provided callback should be written so that it completes before the next time it's + * scheduled to run. + * + * @param frequency Frequency at which to call the callback, starting one period after the call to + * this method. + */ + public void startPeriodic(Frequency frequency) { + startPeriodic(frequency.asPeriod()); + } + /** * Stop further callback invocations. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index 4525d6aa659..de0fab0f26b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -11,6 +11,7 @@ import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.NotifierJNI; +import edu.wpi.first.units.measure.Frequency; import edu.wpi.first.units.measure.Time; import java.util.PriorityQueue; @@ -84,7 +85,7 @@ protected TimedRobot() { /** * Constructor for TimedRobot. * - * @param period Period in seconds. + * @param period The period of the robot loop function. */ protected TimedRobot(double period) { super(period); @@ -95,6 +96,24 @@ protected TimedRobot(double period) { HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Timed); } + /** + * Constructor for TimedRobot. + * + * @param period The period of the robot loop function. + */ + protected TimedRobot(Time period) { + this(period.in(Seconds)); + } + + /** + * Constructor for TimedRobot. + * + * @param frequency The frequency of the robot loop function. + */ + protected TimedRobot(Frequency frequency) { + this(frequency.asPeriod()); + } + @Override public void close() { NotifierJNI.stopNotifier(m_notifier); 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 fb92dc52a8d..af55a432084 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java @@ -4,6 +4,10 @@ package edu.wpi.first.wpilibj; +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.units.measure.Time; + /** * A timer class. * @@ -46,10 +50,20 @@ public static double getMatchTime() { } /** - * Pause the thread for a specified time. Pause the execution of the thread for a specified period - * of time given in seconds. Motors will continue to run at their last assigned values, and - * sensors will continue to update. Only the task containing the wait will pause until the wait - * time is expired. + * Pause the execution of the thread for a specified period of time. Motors will continue to run + * at their last assigned values, and sensors will continue to update. Only the task containing + * the wait will pause until the wait time is expired. + * + * @param period Length of time to pause + */ + public static void delay(final Time period) { + delay(period.in(Seconds)); + } + + /** + * Pause the execution of the thread for a specified period of time given in seconds. Motors will + * continue to run at their last assigned values, and sensors will continue to update. Only the + * task containing the wait will pause until the wait time is expired. * * @param seconds Length of time to pause */ @@ -137,7 +151,17 @@ public void stop() { /** * Check if the period specified has passed. * - * @param seconds The period to check. + * @param period The period to check. + * @return Whether the period has passed. + */ + public boolean hasElapsed(Time period) { + return hasElapsed(period.in(Seconds)); + } + + /** + * Check if the period specified has passed. + * + * @param seconds The period to check in seconds. * @return Whether the period has passed. */ public boolean hasElapsed(double seconds) { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java index 6d00d6402bb..e3373c844a3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java @@ -4,7 +4,10 @@ package edu.wpi.first.wpilibj; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.hal.NotifierJNI; +import edu.wpi.first.units.measure.Time; import java.io.Closeable; import java.util.PriorityQueue; import java.util.concurrent.locks.ReentrantLock; @@ -55,6 +58,16 @@ public Watchdog(double timeoutSeconds, Runnable callback) { m_tracer = new Tracer(); } + /** + * Watchdog constructor. + * + * @param timeout The watchdog's timeout with microsecond resolution. + * @param callback This function is called when the timeout expires. + */ + public Watchdog(Time timeout, Runnable callback) { + this(timeout.in(Seconds), callback); + } + @Override public void close() { disable(); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java index 9369b7811a8..01c0b8ee3b4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/event/BooleanEvent.java @@ -4,9 +4,11 @@ package edu.wpi.first.wpilibj.event; +import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.measure.Time; import java.util.concurrent.atomic.AtomicBoolean; import java.util.function.BiFunction; import java.util.function.BooleanSupplier; @@ -167,7 +169,18 @@ public boolean getAsBoolean() { * Creates a new debounced event from this event - it will become active when this event has been * active for longer than the specified period. * - * @param seconds The debounce period. + * @param period The debounce period. + * @return The debounced event (rising edges debounced only). + */ + public BooleanEvent debounce(Time period) { + return debounce(period.in(Seconds)); + } + + /** + * Creates a new debounced event from this event - it will become active when this event has been + * active for longer than the specified period. + * + * @param seconds The debounce period in seconds. * @return The debounced event (rising edges debounced only). */ public BooleanEvent debounce(double seconds) { @@ -178,7 +191,19 @@ public BooleanEvent debounce(double seconds) { * Creates a new debounced event from this event - it will become active when this event has been * active for longer than the specified period. * - * @param seconds The debounce period. + * @param period The debounce period. + * @param type The debounce type. + * @return The debounced event. + */ + public BooleanEvent debounce(Time period, Debouncer.DebounceType type) { + return debounce(period.in(Seconds), type); + } + + /** + * Creates a new debounced event from this event - it will become active when this event has been + * active for longer than the specified period. + * + * @param seconds The debounce period in seconds. * @param type The debounce type. * @return The debounced event. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java index 7ce6af8c3e8..f3e09b55262 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java @@ -4,11 +4,14 @@ package edu.wpi.first.wpilibj.smartdashboard; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.networktables.NTSendable; import edu.wpi.first.networktables.NTSendableBuilder; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.sendable.SendableRegistry; import java.util.ArrayList; import java.util.List; @@ -66,6 +69,17 @@ public synchronized void setRobotPose(double xMeters, double yMeters, Rotation2d m_objects.get(0).setPose(xMeters, yMeters, rotation); } + /** + * Set the robot pose from x, y, and rotation. + * + * @param x X location + * @param y Y location + * @param rotation rotation + */ + public synchronized void setRobotPose(Distance x, Distance y, Rotation2d rotation) { + m_objects.get(0).setPose(x.in(Meters), y.in(Meters), rotation); + } + /** * Get the robot pose. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java index f3627a6bef1..f9e38f9261f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java @@ -4,11 +4,14 @@ package edu.wpi.first.wpilibj.smartdashboard; +import static edu.wpi.first.units.Units.Meters; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.units.measure.Distance; import java.util.ArrayList; import java.util.Collections; import java.util.List; @@ -51,6 +54,17 @@ public synchronized void setPose(double xMeters, double yMeters, Rotation2d rota setPose(new Pose2d(xMeters, yMeters, rotation)); } + /** + * Set the pose from x, y, and rotation. + * + * @param x X location + * @param y Y location + * @param rotation rotation + */ + public synchronized void setPose(Distance x, Distance y, Rotation2d rotation) { + setPose(new Pose2d(x.in(Meters), y.in(Meters), rotation)); + } + /** * Get the pose. *