Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add ChassisSpeeds.fromRobotRelativeSpeeds() (#5744)

Browse Source
This commit is contained in:
Brayden Zee
2023-10-17 13:13:04 -04:00
committed by GitHub
parent f98c943445
commit e814595ea7
4 changed files with 126 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
View File

@@ -55,6 +55,17 @@ class ChassisSpeedsTest {
() -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
void testFromRobotRelativeSpeeds() {
final var chassisSpeeds =
ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0));
assertAll(
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
void testPlus() {
final var left = new ChassisSpeeds(1.0, 0.5, 0.75);

11
wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
View File

@@ -2,6 +2,8 @@
// 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 <cmath>
#include <gtest/gtest.h>
#include "frc/kinematics/ChassisSpeeds.h"
@@ -36,6 +38,15 @@ TEST(ChassisSpeedsTest, FromFieldRelativeSpeeds) {
EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, FromRobotRelativeSpeeds) {
const auto chassisSpeeds = frc::ChassisSpeeds::FromRobotRelativeSpeeds(
1.0_mps, 0.0_mps, 0.5_rad_per_s, 45.0_deg);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vx.value(), kEpsilon);
EXPECT_NEAR(1.0 / std::sqrt(2.0), chassisSpeeds.vy.value(), kEpsilon);
EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
}
TEST(ChassisSpeedsTest, Plus) {
const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s};
const frc::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s};