mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-02 02:51:42 +00:00
[commands, wpimath] Remove Mecanum/SwerveControllerCommand and HolonomicDriveController (#8119)
This commit is contained in:
Gold856
@@ -1,142 +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.wpilibj2.command;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.hal.HAL;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.simulation.SimHooks;
|
||||
import java.util.ArrayList;
|
||||
import org.junit.jupiter.api.AfterEach;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.junit.jupiter.api.parallel.ResourceLock;
|
||||
|
||||
class MecanumControllerCommandTest {
|
||||
@BeforeEach
|
||||
void setupAll() {
|
||||
HAL.initialize(500, 0);
|
||||
SimHooks.pauseTiming();
|
||||
}
|
||||
|
||||
@AfterEach
|
||||
void cleanupAll() {
|
||||
SimHooks.resumeTiming();
|
||||
}
|
||||
|
||||
private final Timer m_timer = new Timer();
|
||||
private Rotation2d m_angle = Rotation2d.kZero;
|
||||
|
||||
private double m_frontLeftSpeed;
|
||||
private double m_frontLeftDistance;
|
||||
private double m_rearLeftSpeed;
|
||||
private double m_rearLeftDistance;
|
||||
private double m_frontRightSpeed;
|
||||
private double m_frontRightDistance;
|
||||
private double m_rearRightSpeed;
|
||||
private double m_rearRightDistance;
|
||||
|
||||
private final ProfiledPIDController m_rotController =
|
||||
new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
|
||||
|
||||
private static final double kxTolerance = 1 / 12.0;
|
||||
private static final double kyTolerance = 1 / 12.0;
|
||||
private static final double kAngularTolerance = 1 / 12.0;
|
||||
|
||||
private static final double kWheelBase = 0.5;
|
||||
private static final double kTrackwidth = 0.5;
|
||||
|
||||
private final MecanumDriveKinematics m_kinematics =
|
||||
new MecanumDriveKinematics(
|
||||
new Translation2d(kWheelBase / 2, kTrackwidth / 2),
|
||||
new Translation2d(kWheelBase / 2, -kTrackwidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, kTrackwidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, -kTrackwidth / 2));
|
||||
|
||||
private final MecanumDriveOdometry m_odometry =
|
||||
new MecanumDriveOdometry(
|
||||
m_kinematics, Rotation2d.kZero, new MecanumDriveWheelPositions(), Pose2d.kZero);
|
||||
|
||||
public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
|
||||
this.m_frontLeftSpeed = wheelSpeeds.frontLeft;
|
||||
this.m_rearLeftSpeed = wheelSpeeds.rearLeft;
|
||||
this.m_frontRightSpeed = wheelSpeeds.frontRight;
|
||||
this.m_rearRightSpeed = wheelSpeeds.rearRight;
|
||||
}
|
||||
|
||||
public MecanumDriveWheelPositions getCurrentWheelDistances() {
|
||||
return new MecanumDriveWheelPositions(
|
||||
m_frontLeftDistance, m_frontRightDistance, m_rearLeftDistance, m_rearRightDistance);
|
||||
}
|
||||
|
||||
public Pose2d getRobotPose() {
|
||||
m_odometry.update(m_angle, getCurrentWheelDistances());
|
||||
return m_odometry.getPose();
|
||||
}
|
||||
|
||||
@Test
|
||||
@ResourceLock("timing")
|
||||
void testReachesReference() {
|
||||
final var subsystem = new Subsystem() {};
|
||||
|
||||
final var waypoints = new ArrayList<Pose2d>();
|
||||
waypoints.add(Pose2d.kZero);
|
||||
waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
|
||||
var config = new TrajectoryConfig(8.8, 0.1);
|
||||
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
|
||||
|
||||
final var endState = trajectory.sample(trajectory.getTotalTime());
|
||||
|
||||
final var command =
|
||||
new MecanumControllerCommand(
|
||||
trajectory,
|
||||
this::getRobotPose,
|
||||
m_kinematics,
|
||||
new PIDController(0.6, 0, 0),
|
||||
new PIDController(0.6, 0, 0),
|
||||
m_rotController,
|
||||
8.8,
|
||||
this::setWheelSpeeds,
|
||||
subsystem);
|
||||
|
||||
m_timer.restart();
|
||||
|
||||
command.initialize();
|
||||
while (!command.isFinished()) {
|
||||
command.execute();
|
||||
m_angle = trajectory.sample(m_timer.get()).pose.getRotation();
|
||||
m_frontLeftDistance += m_frontLeftSpeed * 0.005;
|
||||
m_rearLeftDistance += m_rearLeftSpeed * 0.005;
|
||||
m_frontRightDistance += m_frontRightSpeed * 0.005;
|
||||
m_rearRightDistance += m_rearRightSpeed * 0.005;
|
||||
SimHooks.stepTiming(0.005);
|
||||
}
|
||||
m_timer.stop();
|
||||
command.end(true);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(endState.pose.getX(), getRobotPose().getX(), kxTolerance),
|
||||
() -> assertEquals(endState.pose.getY(), getRobotPose().getY(), kyTolerance),
|
||||
() ->
|
||||
assertEquals(
|
||||
endState.pose.getRotation().getRadians(),
|
||||
getRobotPose().getRotation().getRadians(),
|
||||
kAngularTolerance));
|
||||
}
|
||||
}
|
||||
@@ -1,142 +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.wpilibj2.command;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertAll;
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.hal.HAL;
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.simulation.SimHooks;
|
||||
import java.util.ArrayList;
|
||||
import org.junit.jupiter.api.AfterEach;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.junit.jupiter.api.parallel.ResourceLock;
|
||||
|
||||
class SwerveControllerCommandTest {
|
||||
@BeforeEach
|
||||
void setup() {
|
||||
HAL.initialize(500, 0);
|
||||
SimHooks.pauseTiming();
|
||||
}
|
||||
|
||||
@AfterEach
|
||||
void cleanup() {
|
||||
SimHooks.resumeTiming();
|
||||
}
|
||||
|
||||
private final Timer m_timer = new Timer();
|
||||
private Rotation2d m_angle = Rotation2d.kZero;
|
||||
|
||||
private SwerveModuleState[] m_moduleStates =
|
||||
new SwerveModuleState[] {
|
||||
new SwerveModuleState(0, Rotation2d.kZero),
|
||||
new SwerveModuleState(0, Rotation2d.kZero),
|
||||
new SwerveModuleState(0, Rotation2d.kZero),
|
||||
new SwerveModuleState(0, Rotation2d.kZero)
|
||||
};
|
||||
|
||||
private final SwerveModulePosition[] m_modulePositions =
|
||||
new SwerveModulePosition[] {
|
||||
new SwerveModulePosition(0, Rotation2d.kZero),
|
||||
new SwerveModulePosition(0, Rotation2d.kZero),
|
||||
new SwerveModulePosition(0, Rotation2d.kZero),
|
||||
new SwerveModulePosition(0, Rotation2d.kZero)
|
||||
};
|
||||
|
||||
private final ProfiledPIDController m_rotController =
|
||||
new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
|
||||
|
||||
private static final double kxTolerance = 1 / 12.0;
|
||||
private static final double kyTolerance = 1 / 12.0;
|
||||
private static final double kAngularTolerance = 1 / 12.0;
|
||||
|
||||
private static final double kWheelBase = 0.5;
|
||||
private static final double kTrackwidth = 0.5;
|
||||
|
||||
private final SwerveDriveKinematics m_kinematics =
|
||||
new SwerveDriveKinematics(
|
||||
new Translation2d(kWheelBase / 2, kTrackwidth / 2),
|
||||
new Translation2d(kWheelBase / 2, -kTrackwidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, kTrackwidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, -kTrackwidth / 2));
|
||||
|
||||
private final SwerveDriveOdometry m_odometry =
|
||||
new SwerveDriveOdometry(m_kinematics, Rotation2d.kZero, m_modulePositions, Pose2d.kZero);
|
||||
|
||||
@SuppressWarnings("PMD.ArrayIsStoredDirectly")
|
||||
public void setModuleStates(SwerveModuleState[] moduleStates) {
|
||||
this.m_moduleStates = moduleStates;
|
||||
}
|
||||
|
||||
public Pose2d getRobotPose() {
|
||||
m_odometry.update(m_angle, m_modulePositions);
|
||||
return m_odometry.getPose();
|
||||
}
|
||||
|
||||
@Test
|
||||
@ResourceLock("timing")
|
||||
void testReachesReference() {
|
||||
final var subsystem = new Subsystem() {};
|
||||
|
||||
final var waypoints = new ArrayList<Pose2d>();
|
||||
waypoints.add(Pose2d.kZero);
|
||||
waypoints.add(new Pose2d(1, 5, new Rotation2d(3)));
|
||||
var config = new TrajectoryConfig(8.8, 0.1);
|
||||
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
|
||||
|
||||
final var endState = trajectory.sample(trajectory.getTotalTime());
|
||||
|
||||
final var command =
|
||||
new SwerveControllerCommand(
|
||||
trajectory,
|
||||
this::getRobotPose,
|
||||
m_kinematics,
|
||||
new PIDController(0.6, 0, 0),
|
||||
new PIDController(0.6, 0, 0),
|
||||
m_rotController,
|
||||
this::setModuleStates,
|
||||
subsystem);
|
||||
|
||||
m_timer.restart();
|
||||
|
||||
command.initialize();
|
||||
while (!command.isFinished()) {
|
||||
command.execute();
|
||||
m_angle = trajectory.sample(m_timer.get()).pose.getRotation();
|
||||
|
||||
for (int i = 0; i < m_modulePositions.length; i++) {
|
||||
m_modulePositions[i].distance += m_moduleStates[i].speed * 0.005;
|
||||
m_modulePositions[i].angle = m_moduleStates[i].angle;
|
||||
}
|
||||
|
||||
SimHooks.stepTiming(0.005);
|
||||
}
|
||||
m_timer.stop();
|
||||
command.end(true);
|
||||
|
||||
assertAll(
|
||||
() -> assertEquals(endState.pose.getX(), getRobotPose().getX(), kxTolerance),
|
||||
() -> assertEquals(endState.pose.getY(), getRobotPose().getY(), kyTolerance),
|
||||
() ->
|
||||
assertEquals(
|
||||
endState.pose.getRotation().getRadians(),
|
||||
getRobotPose().getRotation().getRadians(),
|
||||
kAngularTolerance));
|
||||
}
|
||||
}
|
||||
@@ -1,135 +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 <frc2/command/MecanumControllerCommand.h>
|
||||
#include <frc2/command/Subsystem.h>
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/Timer.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/MecanumDriveKinematics.h>
|
||||
#include <frc/kinematics/MecanumDriveOdometry.h>
|
||||
#include <frc/simulation/SimHooks.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "CommandTestBase.h"
|
||||
|
||||
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
|
||||
EXPECT_LE(units::math::abs(val1 - val2), eps)
|
||||
|
||||
class MecanumControllerCommandTest : public ::testing::Test {
|
||||
using radians_per_second_squared_t =
|
||||
units::compound_unit<units::radians,
|
||||
units::inverse<units::squared<units::second>>>;
|
||||
|
||||
protected:
|
||||
frc::Timer m_timer;
|
||||
frc::Rotation2d m_angle{0_rad};
|
||||
|
||||
units::meters_per_second_t m_frontLeftSpeed = 0.0_mps;
|
||||
units::meter_t m_frontLeftDistance = 0.0_m;
|
||||
units::meters_per_second_t m_rearLeftSpeed = 0.0_mps;
|
||||
units::meter_t m_rearLeftDistance = 0.0_m;
|
||||
units::meters_per_second_t m_frontRightSpeed = 0.0_mps;
|
||||
units::meter_t m_frontRightDistance = 0.0_m;
|
||||
units::meters_per_second_t m_rearRightSpeed = 0.0_mps;
|
||||
units::meter_t m_rearRightDistance = 0.0_m;
|
||||
|
||||
frc::ProfiledPIDController<units::radians> m_rotController{
|
||||
1, 0, 0,
|
||||
frc::TrapezoidProfile<units::radians>::Constraints{
|
||||
9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
|
||||
|
||||
static constexpr units::meter_t kxTolerance{1 / 12.0};
|
||||
static constexpr units::meter_t kyTolerance{1 / 12.0};
|
||||
static constexpr units::radian_t kAngularTolerance{1 / 12.0};
|
||||
|
||||
static constexpr units::meter_t kWheelBase{0.5};
|
||||
static constexpr units::meter_t kTrackwidth{0.5};
|
||||
|
||||
frc::MecanumDriveKinematics m_kinematics{
|
||||
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
|
||||
|
||||
frc::MecanumDriveOdometry m_odometry{m_kinematics, 0_rad,
|
||||
getCurrentWheelDistances(),
|
||||
frc::Pose2d{0_m, 0_m, 0_rad}};
|
||||
|
||||
void SetUp() override { frc::sim::PauseTiming(); }
|
||||
|
||||
void TearDown() override { frc::sim::ResumeTiming(); }
|
||||
|
||||
frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
|
||||
return frc::MecanumDriveWheelSpeeds{m_frontLeftSpeed, m_frontRightSpeed,
|
||||
m_rearLeftSpeed, m_rearRightSpeed};
|
||||
}
|
||||
|
||||
frc::MecanumDriveWheelPositions getCurrentWheelDistances() {
|
||||
return frc::MecanumDriveWheelPositions{
|
||||
m_frontLeftDistance,
|
||||
m_rearLeftDistance,
|
||||
m_frontRightDistance,
|
||||
m_rearRightDistance,
|
||||
};
|
||||
}
|
||||
|
||||
frc::Pose2d getRobotPose() {
|
||||
m_odometry.Update(m_angle, getCurrentWheelDistances());
|
||||
return m_odometry.GetPose();
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(MecanumControllerCommandTest, ReachesReference) {
|
||||
frc2::TestSubsystem subsystem;
|
||||
|
||||
auto waypoints =
|
||||
std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
|
||||
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
waypoints, {8.8_mps, 0.1_mps_sq});
|
||||
|
||||
auto endState = trajectory.Sample(trajectory.TotalTime());
|
||||
|
||||
auto command = frc2::MecanumControllerCommand(
|
||||
trajectory, [&]() { return getRobotPose(); }, m_kinematics,
|
||||
|
||||
frc::PIDController(0.6, 0, 0), frc::PIDController(0.6, 0, 0),
|
||||
m_rotController, 8.8_mps,
|
||||
[&](units::meters_per_second_t frontLeft,
|
||||
units::meters_per_second_t rearLeft,
|
||||
units::meters_per_second_t frontRight,
|
||||
units::meters_per_second_t rearRight) {
|
||||
m_frontLeftSpeed = frontLeft;
|
||||
m_rearLeftSpeed = rearLeft;
|
||||
m_frontRightSpeed = frontRight;
|
||||
m_rearRightSpeed = rearRight;
|
||||
},
|
||||
{&subsystem});
|
||||
|
||||
m_timer.Restart();
|
||||
|
||||
command.Initialize();
|
||||
while (!command.IsFinished()) {
|
||||
command.Execute();
|
||||
m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
|
||||
m_frontLeftDistance += m_frontLeftSpeed * 5_ms;
|
||||
m_rearLeftDistance += m_rearLeftSpeed * 5_ms;
|
||||
m_frontRightDistance += m_frontRightSpeed * 5_ms;
|
||||
m_rearRightDistance += m_rearRightSpeed * 5_ms;
|
||||
frc::sim::StepTiming(5_ms);
|
||||
}
|
||||
m_timer.Stop();
|
||||
command.End(false);
|
||||
|
||||
EXPECT_NEAR_UNITS(endState.pose.X(), getRobotPose().X(), kxTolerance);
|
||||
EXPECT_NEAR_UNITS(endState.pose.Y(), getRobotPose().Y(), kyTolerance);
|
||||
EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
|
||||
getRobotPose().Rotation().Radians(), kAngularTolerance);
|
||||
}
|
||||
@@ -1,113 +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 <frc2/command/Subsystem.h>
|
||||
#include <frc2/command/SwerveControllerCommand.h>
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include <frc/Timer.h>
|
||||
#include <frc/controller/PIDController.h>
|
||||
#include <frc/controller/ProfiledPIDController.h>
|
||||
#include <frc/geometry/Rotation2d.h>
|
||||
#include <frc/geometry/Translation2d.h>
|
||||
#include <frc/kinematics/SwerveDriveKinematics.h>
|
||||
#include <frc/kinematics/SwerveDriveOdometry.h>
|
||||
#include <frc/kinematics/SwerveModuleState.h>
|
||||
#include <frc/simulation/SimHooks.h>
|
||||
#include <frc/trajectory/TrajectoryGenerator.h>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "CommandTestBase.h"
|
||||
|
||||
#define EXPECT_NEAR_UNITS(val1, val2, eps) \
|
||||
EXPECT_LE(units::math::abs(val1 - val2), eps)
|
||||
|
||||
class SwerveControllerCommandTest : public ::testing::Test {
|
||||
using radians_per_second_squared_t =
|
||||
units::compound_unit<units::radians,
|
||||
units::inverse<units::squared<units::second>>>;
|
||||
|
||||
protected:
|
||||
frc::Timer m_timer;
|
||||
frc::Rotation2d m_angle{0_rad};
|
||||
|
||||
wpi::array<frc::SwerveModuleState, 4> m_moduleStates{
|
||||
frc::SwerveModuleState{}, frc::SwerveModuleState{},
|
||||
frc::SwerveModuleState{}, frc::SwerveModuleState{}};
|
||||
|
||||
wpi::array<frc::SwerveModulePosition, 4> m_modulePositions{
|
||||
frc::SwerveModulePosition{}, frc::SwerveModulePosition{},
|
||||
frc::SwerveModulePosition{}, frc::SwerveModulePosition{}};
|
||||
|
||||
frc::ProfiledPIDController<units::radians> m_rotController{
|
||||
1, 0, 0,
|
||||
frc::TrapezoidProfile<units::radians>::Constraints{
|
||||
9_rad_per_s, units::unit_t<radians_per_second_squared_t>(3)}};
|
||||
|
||||
static constexpr units::meter_t kxTolerance{1 / 12.0};
|
||||
static constexpr units::meter_t kyTolerance{1 / 12.0};
|
||||
static constexpr units::radian_t kAngularTolerance{1 / 12.0};
|
||||
|
||||
static constexpr units::meter_t kWheelBase{0.5};
|
||||
static constexpr units::meter_t kTrackwidth{0.5};
|
||||
|
||||
frc::SwerveDriveKinematics<4> m_kinematics{
|
||||
frc::Translation2d{kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{kWheelBase / 2, -kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, kTrackwidth / 2},
|
||||
frc::Translation2d{-kWheelBase / 2, -kTrackwidth / 2}};
|
||||
|
||||
frc::SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad, m_modulePositions,
|
||||
frc::Pose2d{0_m, 0_m, 0_rad}};
|
||||
|
||||
void SetUp() override { frc::sim::PauseTiming(); }
|
||||
|
||||
void TearDown() override { frc::sim::ResumeTiming(); }
|
||||
|
||||
frc::Pose2d getRobotPose() {
|
||||
m_odometry.Update(m_angle, m_modulePositions);
|
||||
return m_odometry.GetPose();
|
||||
}
|
||||
};
|
||||
|
||||
TEST_F(SwerveControllerCommandTest, ReachesReference) {
|
||||
frc2::TestSubsystem subsystem;
|
||||
|
||||
auto waypoints =
|
||||
std::vector{frc::Pose2d{0_m, 0_m, 0_rad}, frc::Pose2d{1_m, 5_m, 3_rad}};
|
||||
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
|
||||
waypoints, {8.8_mps, 0.1_mps_sq});
|
||||
|
||||
auto endState = trajectory.Sample(trajectory.TotalTime());
|
||||
|
||||
auto command = frc2::SwerveControllerCommand<4>(
|
||||
trajectory, [&]() { return getRobotPose(); }, m_kinematics,
|
||||
|
||||
frc::PIDController(0.6, 0, 0), frc::PIDController(0.6, 0, 0),
|
||||
m_rotController,
|
||||
[&](auto moduleStates) { m_moduleStates = moduleStates; }, {&subsystem});
|
||||
|
||||
m_timer.Restart();
|
||||
|
||||
command.Initialize();
|
||||
while (!command.IsFinished()) {
|
||||
command.Execute();
|
||||
m_angle = trajectory.Sample(m_timer.Get()).pose.Rotation();
|
||||
|
||||
for (size_t i = 0; i < m_modulePositions.size(); i++) {
|
||||
m_modulePositions[i].distance += m_moduleStates[i].speed * 5_ms;
|
||||
m_modulePositions[i].angle = m_moduleStates[i].angle;
|
||||
}
|
||||
|
||||
frc::sim::StepTiming(5_ms);
|
||||
}
|
||||
m_timer.Stop();
|
||||
command.End(false);
|
||||
|
||||
EXPECT_NEAR_UNITS(endState.pose.X(), getRobotPose().X(), kxTolerance);
|
||||
EXPECT_NEAR_UNITS(endState.pose.Y(), getRobotPose().Y(), kyTolerance);
|
||||
EXPECT_NEAR_UNITS(endState.pose.Rotation().Radians(),
|
||||
getRobotPose().Rotation().Radians(), kAngularTolerance);
|
||||
}
|
||||
Reference in New Issue
Block a user