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

[commands, wpimath] Remove Mecanum/SwerveControllerCommand and HolonomicDriveController (#8119)

Browse Source
This commit is contained in:
Gold856
2025-08-01 02:05:42 -04:00
committed by GitHub
parent b251d16ef7
commit e0e774abde
51 changed files with 0 additions and 5642 deletions
Download Patch File Download Diff File Expand all files Collapse all files

85
wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java
View File

@@ -1,85 +0,0 @@
// 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.math.controller;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import java.util.ArrayList;
import java.util.List;
import org.junit.jupiter.api.Test;
class HolonomicDriveControllerTest {
private static final double kTolerance = 1 / 12.0;
private static final double kAngularTolerance = Math.toRadians(2);
@Test
void testReachesReference() {
HolonomicDriveController controller =
new HolonomicDriveController(
new PIDController(1.0, 0.0, 0.0),
new PIDController(1.0, 0.0, 0.0),
new ProfiledPIDController(
1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2.0 * Math.PI, Math.PI)));
Pose2d robotPose = new Pose2d(2.7, 23.0, Rotation2d.kZero);
List<Pose2d> waypoints = new ArrayList<>();
waypoints.add(new Pose2d(2.75, 22.521, Rotation2d.kZero));
waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.8)));
TrajectoryConfig config = new TrajectoryConfig(8.0, 4.0);
Trajectory trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
final double kDt = 0.02;
final double kTotalTime = trajectory.getTotalTime();
for (int i = 0; i < (kTotalTime / kDt); i++) {
Trajectory.State state = trajectory.sample(kDt * i);
ChassisSpeeds output = controller.calculate(robotPose, state, Rotation2d.kZero);
robotPose = robotPose.exp(new Twist2d(output.vx * kDt, output.vy * kDt, output.omega * kDt));
}
final List<Trajectory.State> states = trajectory.getStates();
final Pose2d endPose = states.get(states.size() - 1).pose;
// Java lambdas require local variables referenced from a lambda expression
// must be final or effectively final.
final Pose2d finalRobotPose = robotPose;
assertAll(
() -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
() -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
() ->
assertEquals(
0.0,
MathUtil.angleModulus(finalRobotPose.getRotation().getRadians()),
kAngularTolerance));
}
@Test
void testDoesNotRotateUnnecessarily() {
var controller =
new HolonomicDriveController(
new PIDController(1, 0, 0),
new PIDController(1, 0, 0),
new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(4, 2)));
ChassisSpeeds speeds =
controller.calculate(
new Pose2d(0, 0, new Rotation2d(1.57)), Pose2d.kZero, 0, new Rotation2d(1.57));
assertEquals(0.0, speeds.omega);
}
}

67
wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
View File

@@ -1,67 +0,0 @@
// 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 <numbers>
#include <gtest/gtest.h>
#include "frc/MathUtil.h"
#include "frc/controller/HolonomicDriveController.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "units/angular_acceleration.h"
#include "units/math.h"
#include "units/time.h"
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
EXPECT_LE(units::math::abs(val1 - val2), eps)
static constexpr units::meter_t kTolerance{1 / 12.0};
static constexpr units::radian_t kAngularTolerance{2.0 * std::numbers::pi /
180.0};
TEST(HolonomicDriveControllerTest, ReachesReference) {
frc::HolonomicDriveController controller{
frc::PIDController{1.0, 0.0, 0.0}, frc::PIDController{1.0, 0.0, 0.0},
frc::ProfiledPIDController<units::radian>{
1.0, 0.0, 0.0,
frc::TrapezoidProfile<units::radian>::Constraints{
units::radians_per_second_t{2.0 * std::numbers::pi},
units::radians_per_second_squared_t{std::numbers::pi}}}};
frc::Pose2d robotPose{2.7_m, 23_m, 0_deg};
auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
waypoints, {8.0_mps, 4.0_mps_sq});
constexpr units::second_t kDt = 20_ms;
auto totalTime = trajectory.TotalTime();
for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
auto state = trajectory.Sample(kDt * i);
auto [vx, vy, omega] = controller.Calculate(robotPose, state, 0_rad);
robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, vy * kDt, omega * kDt});
}
auto& endPose = trajectory.States().back().pose;
EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
EXPECT_NEAR_UNITS(frc::AngleModulus(robotPose.Rotation().Radians()), 0_rad,
kAngularTolerance);
}
TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
frc::HolonomicDriveController controller{
frc::PIDController{1, 0, 0}, frc::PIDController{1, 0, 0},
frc::ProfiledPIDController<units::radian>{
1, 0, 0,
frc::TrapezoidProfile<units::radian>::Constraints{
4_rad_per_s, 2_rad_per_s / 1_s}}};
frc::ChassisSpeeds speeds = controller.Calculate(
frc::Pose2d{0_m, 0_m, 1.57_rad}, frc::Pose2d{}, 0_mps, 1.57_rad);
EXPECT_EQ(0, speeds.omega.value());
}