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] Add tests for swerve module optimization (#3100)

Browse Source
This commit is contained in:
Prateek Machiraju
2021-01-20 23:44:37 -05:00
committed by GitHub
parent 6ba8c289c5
commit e9c86df468
2 changed files with 94 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp Normal file
View File

@@ -0,0 +1,41 @@
// 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 "frc/geometry/Rotation2d.h"
#include "frc/kinematics/SwerveModuleState.h"
#include "gtest/gtest.h"
static constexpr double kEpsilon = 1E-9;
TEST(SwerveModuleState, Optimize) {
frc::Rotation2d angleA{45_deg};
frc::SwerveModuleState refA{-2_mps, 180_deg};
auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
EXPECT_NEAR(optimizedA.speed.to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().to<double>(), 0.0, kEpsilon);
frc::Rotation2d angleB{-50_deg};
frc::SwerveModuleState refB{4.7_mps, 41_deg};
auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
EXPECT_NEAR(optimizedB.speed.to<double>(), -4.7, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().to<double>(), -139.0, kEpsilon);
}
TEST(SwerveModuleState, NoOptimize) {
frc::Rotation2d angleA{0_deg};
frc::SwerveModuleState refA{2_mps, 89_deg};
auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
EXPECT_NEAR(optimizedA.speed.to<double>(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().to<double>(), 89.0, kEpsilon);
frc::Rotation2d angleB{0_deg};
frc::SwerveModuleState refB{-2_mps, -2_deg};
auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
EXPECT_NEAR(optimizedB.speed.to<double>(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().to<double>(), -2.0, kEpsilon);
}