mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
8f1f64ffb6
Also update copyright to include "and other WPILib contributors" and clarify license referral language to not be restricted to FIRST teams.
25 lines
848 B
C++
25 lines
848 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 <wpi/math>
|
|
|
|
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
|
#include "frc/kinematics/DifferentialDriveOdometry.h"
|
|
#include "gtest/gtest.h"
|
|
|
|
static constexpr double kEpsilon = 1E-9;
|
|
|
|
using namespace frc;
|
|
|
|
TEST(DifferentialDriveOdometry, EncoderDistances) {
|
|
DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
|
|
|
|
const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
|
|
units::meter_t(5 * wpi::math::pi));
|
|
|
|
EXPECT_NEAR(pose.X().to<double>(), 5.0, kEpsilon);
|
|
EXPECT_NEAR(pose.Y().to<double>(), 5.0, kEpsilon);
|
|
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
|
|
}
|