mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
d6f185d8e5
I started with the output of styleguide#217, then renamed a few classes to fix compilation. ntcore's StorageTest needed some manual renaming since it put the Test word in the middle instead of at the end. One limitation of wpiformat is test cases that were only named "Test" were unmodified, and an error was generated. These test cases were manually given more descriptive names: * TimedRobotTest mode test cases had "Mode" appended to the name. Java tests were renamed to match. * UvAsyncTest and UvAsyncFunctionTest cases were given alternate names
79 lines
3.2 KiB
C++
79 lines
3.2 KiB
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/numbers>
|
|
|
|
#include "frc/kinematics/ChassisSpeeds.h"
|
|
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
|
#include "gtest/gtest.h"
|
|
#include "units/angular_velocity.h"
|
|
#include "units/length.h"
|
|
#include "units/velocity.h"
|
|
|
|
using namespace frc;
|
|
|
|
static constexpr double kEpsilon = 1E-9;
|
|
|
|
TEST(DifferentialDriveKinematicsTest, InverseKinematicsFromZero) {
|
|
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
|
const ChassisSpeeds chassisSpeeds;
|
|
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
|
|
|
|
EXPECT_NEAR(wheelSpeeds.left.to<double>(), 0, kEpsilon);
|
|
EXPECT_NEAR(wheelSpeeds.right.to<double>(), 0, kEpsilon);
|
|
}
|
|
|
|
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsFromZero) {
|
|
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
|
const DifferentialDriveWheelSpeeds wheelSpeeds;
|
|
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
|
|
|
|
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
|
|
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
|
|
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
|
|
}
|
|
|
|
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForStraightLine) {
|
|
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
|
const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
|
|
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
|
|
|
|
EXPECT_NEAR(wheelSpeeds.left.to<double>(), 3, kEpsilon);
|
|
EXPECT_NEAR(wheelSpeeds.right.to<double>(), 3, kEpsilon);
|
|
}
|
|
|
|
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForStraightLine) {
|
|
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
|
const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
|
|
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
|
|
|
|
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 3, kEpsilon);
|
|
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
|
|
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
|
|
}
|
|
|
|
TEST(DifferentialDriveKinematicsTest, InverseKinematicsForRotateInPlace) {
|
|
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
|
const ChassisSpeeds chassisSpeeds{
|
|
0.0_mps, 0.0_mps, units::radians_per_second_t{wpi::numbers::pi}};
|
|
const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
|
|
|
|
EXPECT_NEAR(wheelSpeeds.left.to<double>(), -0.381 * wpi::numbers::pi,
|
|
kEpsilon);
|
|
EXPECT_NEAR(wheelSpeeds.right.to<double>(), +0.381 * wpi::numbers::pi,
|
|
kEpsilon);
|
|
}
|
|
|
|
TEST(DifferentialDriveKinematicsTest, ForwardKinematicsForRotateInPlace) {
|
|
const DifferentialDriveKinematics kinematics{0.381_m * 2};
|
|
const DifferentialDriveWheelSpeeds wheelSpeeds{
|
|
units::meters_per_second_t(+0.381 * wpi::numbers::pi),
|
|
units::meters_per_second_t(-0.381 * wpi::numbers::pi)};
|
|
const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
|
|
|
|
EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
|
|
EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
|
|
EXPECT_NEAR(chassisSpeeds.omega.to<double>(), -wpi::numbers::pi, kEpsilon);
|
|
}
|