mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-04 03:11:43 +00:00
936be71a7d
ChassisAccelerations and the drivetrain acceleration types are added in both Java and C++. `ChassisAccelerations` is basically just `ChassisSpeeds` but for accelerations! `DifferentialDriveWheelAccelerations`, `MecanumDriveWheelAccelerations`, and `SwerveModuleAccelerations` are the acceleration equivalent of the drivetrain speeds types. In Java, the `Kinematics` interface now has an additional generic parameter `A` which represents the accelerations, and `toChassisAccelerations` and `toWheelAccelerations` methods, which are implemented the same way as `toChassisSpeeds` and `toWheelSpeeds`. Protobuf and struct classes were also added for all four classes in Java and C++. --------- Signed-off-by: Zach Harel <zach@zharel.me> Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Co-authored-by: Tyler Veness <calcmogul@gmail.com>
95 lines
4.0 KiB
C++
95 lines
4.0 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/math/kinematics/MecanumDriveWheelAccelerations.hpp"
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include "wpi/units/acceleration.hpp"
|
|
|
|
using namespace wpi::math;
|
|
|
|
static constexpr double kEpsilon = 1E-9;
|
|
|
|
TEST(MecanumDriveWheelAccelerationsTest, DefaultConstructor) {
|
|
MecanumDriveWheelAccelerations wheelAccelerations;
|
|
|
|
EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 0.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.frontRight.value(), 0.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 0.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearRight.value(), 0.0, kEpsilon);
|
|
}
|
|
|
|
TEST(MecanumDriveWheelAccelerationsTest, ParameterizedConstructor) {
|
|
MecanumDriveWheelAccelerations wheelAccelerations{1.0_mps_sq, 2.0_mps_sq,
|
|
3.0_mps_sq, 4.0_mps_sq};
|
|
|
|
EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 1.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 3.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearRight.value(), 4.0, kEpsilon);
|
|
}
|
|
|
|
TEST(MecanumDriveWheelAccelerationsTest, Plus) {
|
|
const MecanumDriveWheelAccelerations left{1.0_mps_sq, 0.5_mps_sq, 2.0_mps_sq,
|
|
1.5_mps_sq};
|
|
const MecanumDriveWheelAccelerations right{2.0_mps_sq, 1.5_mps_sq, 0.5_mps_sq,
|
|
1.0_mps_sq};
|
|
|
|
const auto wheelAccelerations = left + right;
|
|
|
|
EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 3.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 2.5, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearRight.value(), 2.5, kEpsilon);
|
|
}
|
|
|
|
TEST(MecanumDriveWheelAccelerationsTest, Minus) {
|
|
const MecanumDriveWheelAccelerations left{5.0_mps_sq, 4.0_mps_sq, 6.0_mps_sq,
|
|
2.5_mps_sq};
|
|
const MecanumDriveWheelAccelerations right{1.0_mps_sq, 2.0_mps_sq, 3.0_mps_sq,
|
|
0.5_mps_sq};
|
|
|
|
const auto wheelAccelerations = left - right;
|
|
|
|
EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 4.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 3.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearRight.value(), 2.0, kEpsilon);
|
|
}
|
|
|
|
TEST(MecanumDriveWheelAccelerationsTest, UnaryMinus) {
|
|
const auto wheelAccelerations = -MecanumDriveWheelAccelerations{
|
|
1.0_mps_sq, -2.0_mps_sq, 3.0_mps_sq, -4.0_mps_sq};
|
|
|
|
EXPECT_NEAR(wheelAccelerations.frontLeft.value(), -1.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.frontRight.value(), 2.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearLeft.value(), -3.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearRight.value(), 4.0, kEpsilon);
|
|
}
|
|
|
|
TEST(MecanumDriveWheelAccelerationsTest, Multiplication) {
|
|
const auto wheelAccelerations =
|
|
MecanumDriveWheelAccelerations{2.0_mps_sq, 2.5_mps_sq, 3.0_mps_sq,
|
|
3.5_mps_sq} *
|
|
2.0;
|
|
|
|
EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 4.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.frontRight.value(), 5.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 6.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearRight.value(), 7.0, kEpsilon);
|
|
}
|
|
|
|
TEST(MecanumDriveWheelAccelerationsTest, Division) {
|
|
const auto wheelAccelerations =
|
|
MecanumDriveWheelAccelerations{2.0_mps_sq, 2.5_mps_sq, 1.5_mps_sq,
|
|
1.0_mps_sq} /
|
|
2.0;
|
|
|
|
EXPECT_NEAR(wheelAccelerations.frontLeft.value(), 1.0, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.frontRight.value(), 1.25, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearLeft.value(), 0.75, kEpsilon);
|
|
EXPECT_NEAR(wheelAccelerations.rearRight.value(), 0.5, kEpsilon);
|
|
}
|