diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h index 91aeef1632..fd41d15ead 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h +++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h @@ -11,6 +11,7 @@ #include "DifferentialDriveKinematics.h" #include "frc/geometry/Pose2d.h" +#include "frc2/Timer.h" namespace frc { /** @@ -86,9 +87,7 @@ class DifferentialDriveOdometry { */ const Pose2d& Update(const Rotation2d& angle, const DifferentialDriveWheelSpeeds& wheelSpeeds) { - const auto now = std::chrono::system_clock::now().time_since_epoch(); - units::second_t time{now}; - return UpdateWithTime(time, angle, wheelSpeeds); + return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds); } private: diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h index 098b7ff121..e8816e13fa 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h +++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h @@ -12,6 +12,7 @@ #include "frc/geometry/Pose2d.h" #include "frc/kinematics/MecanumDriveKinematics.h" #include "frc/kinematics/MecanumDriveWheelSpeeds.h" +#include "frc2/Timer.h" namespace frc { @@ -84,9 +85,7 @@ class MecanumDriveOdometry { */ const Pose2d& Update(const Rotation2d& angle, MecanumDriveWheelSpeeds wheelSpeeds) { - const auto now = std::chrono::system_clock::now().time_since_epoch(); - units::second_t time{now}; - return UpdateWithTime(time, angle, wheelSpeeds); + return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds); } private: diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h index 2d7af7f70d..1dbe1783fa 100644 --- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h +++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h @@ -16,6 +16,7 @@ #include "SwerveDriveKinematics.h" #include "SwerveModuleState.h" #include "frc/geometry/Pose2d.h" +#include "frc2/Timer.h" namespace frc { @@ -95,9 +96,8 @@ class SwerveDriveOdometry { template const Pose2d& Update(const Rotation2d& angle, ModuleStates&&... moduleStates) { - const auto now = std::chrono::system_clock::now().time_since_epoch(); - units::second_t time{now}; - return UpdateWithTime(time, angle, moduleStates...); + return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, + moduleStates...); } private: diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java index b63f7265f0..0f411ca95d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java @@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.kinematics; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Twist2d; @@ -116,7 +117,7 @@ public class DifferentialDriveOdometry { */ public Pose2d update(Rotation2d angle, DifferentialDriveWheelSpeeds wheelSpeeds) { - return updateWithTime(System.currentTimeMillis() / 1000.0, + return updateWithTime(Timer.getFPGATimestamp(), angle, wheelSpeeds); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java index 890cb9a0af..3ea8fff0ce 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java @@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.kinematics; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Twist2d; @@ -109,7 +110,7 @@ public class MecanumDriveOdometry { */ public Pose2d update(Rotation2d angle, MecanumDriveWheelSpeeds wheelSpeeds) { - return updateWithTime(System.currentTimeMillis() / 1000.0, angle, + return updateWithTime(Timer.getFPGATimestamp(), angle, wheelSpeeds); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java index e33c7f532d..2cfea4eaca 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java @@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj.kinematics; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Twist2d; @@ -113,6 +114,6 @@ public class SwerveDriveOdometry { * @return The new pose of the robot. */ public Pose2d update(Rotation2d angle, SwerveModuleState... moduleStates) { - return updateWithTime(System.currentTimeMillis() / 1000.0, angle, moduleStates); + return updateWithTime(Timer.getFPGATimestamp(), angle, moduleStates); } }