Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use FPGA Time instead of wall clock time for odometry (#1996)

Browse Source
This commit is contained in:
Prateek Machiraju
2019-10-27 10:57:35 -04:00
committed by Peter Johnson
parent d4430b765e
commit 8e333c0aad
6 changed files with 13 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
View File

@@ -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:

5
wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
View File

@@ -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:

6
wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
View File

@@ -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 <typename... ModuleStates>
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:

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
View File

@@ -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);
}
}

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
View File

@@ -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);
}
}

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
View File

@@ -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);
}
}