Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +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

53
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleStateTest.java Normal file
View File

@@ -0,0 +1,53 @@
// 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.
package edu.wpi.first.wpilibj.kinematics;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import org.junit.jupiter.api.Test;
class SwerveModuleStateTest {
private static final double kEpsilon = 1E-9;
@Test
void testOptimize() {
var angleA = Rotation2d.fromDegrees(45);
var refA = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(180));
var optimizedA = SwerveModuleState.optimize(refA, angleA);
assertAll(
() -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, optimizedA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(-50);
var refB = new SwerveModuleState(4.7, Rotation2d.fromDegrees(41));
var optimizedB = SwerveModuleState.optimize(refB, angleB);
assertAll(
() -> assertEquals(-4.7, optimizedB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-139.0, optimizedB.angle.getDegrees(), kEpsilon));
}
@Test
void testNoOptimize() {
var angleA = Rotation2d.fromDegrees(0);
var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(89));
var optimizedA = SwerveModuleState.optimize(refA, angleA);
assertAll(
() -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(89.0, optimizedA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(0);
var refB = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(-2));
var optimizedB = SwerveModuleState.optimize(refB, angleB);
assertAll(
() -> assertEquals(-2.0, optimizedB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-2.0, optimizedB.angle.getDegrees(), kEpsilon));
}
}

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);
}