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

[wpimath] Position Delta Odometry for Swerve (#4493)

Browse Source
This commit is contained in:
Jordan McMichael
2022-10-25 15:28:36 -04:00
committed by GitHub
parent fe400f68c5
commit 4170ec6107
35 changed files with 1508 additions and 277 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
View File

@@ -36,6 +36,10 @@ class SwerveControllerCommandTest : public ::testing::Test {
frc::SwerveModuleState{}, frc::SwerveModuleState{},
frc::SwerveModuleState{}, frc::SwerveModuleState{}};
wpi::array<frc::SwerveModulePosition, 4> m_modulePositions{
frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}};
frc::ProfiledPIDController<units::radians> m_rotController{
1, 0, 0,
frc::TrapezoidProfile<units::radians>::Constraints{
@@ -54,19 +58,15 @@ class SwerveControllerCommandTest : public ::testing::Test {
frc::Translation2d{-kWheelBase / 2, kTrackWidth / 2},
frc::Translation2d{-kWheelBase / 2, -kTrackWidth / 2}};
frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad,
frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad, m_modulePositions,
frc::Pose2d{0_m, 0_m, 0_rad}};
void SetUp() override { frc::sim::PauseTiming(); }
void TearDown() override { frc::sim::ResumeTiming(); }
wpi::array<frc::SwerveModuleState, 4> getCurrentWheelSpeeds() {
return m_moduleStates;
}
frc::Pose2d getRobotPose() {
m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds());
m_odometry.Update(m_angle, m_modulePositions);
return m_odometry.GetPose();
}
};
@@ -95,6 +95,12 @@ TEST_F(SwerveControllerCommandTest, ReachesReference) {
while (!command.IsFinished()) {
command.Execute();
m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
for (size_t i = 0; i < m_modulePositions.size(); i++) {
m_modulePositions[i].distance += m_moduleStates[i].speed * 5_ms;
m_modulePositions[i].angle = m_moduleStates[i].angle;
}
frc::sim::StepTiming(5_ms);
}
m_timer.Stop();