mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-26 01:51:41 +00:00
26 lines
809 B
C++
26 lines
809 B
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#include <numbers>
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
|
|
#include "wpi/math/kinematics/DifferentialDriveOdometry.hpp"
|
|
|
|
static constexpr double kEpsilon = 1E-9;
|
|
|
|
using namespace frc;
|
|
|
|
TEST(DifferentialDriveOdometryTest, EncoderDistances) {
|
|
DifferentialDriveOdometry odometry{45_deg, 0_m, 0_m};
|
|
|
|
const auto& pose =
|
|
odometry.Update(135_deg, 0_m, units::meter_t{5 * std::numbers::pi});
|
|
|
|
EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
|
|
EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);
|
|
EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, kEpsilon);
|
|
}
|