Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Position Delta Odometry for Mecanum (#4514)

Browse Source
This commit is contained in:
Jordan McMichael
2022-10-25 15:28:59 -04:00
committed by GitHub
parent 4170ec6107
commit 901fc555f4
28 changed files with 1222 additions and 235 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
View File

@@ -15,6 +15,7 @@ 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;
@@ -43,9 +44,13 @@ class MecanumControllerCommandTest {
private Rotation2d m_angle = new Rotation2d(0);
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));
@@ -66,7 +71,10 @@ class MecanumControllerCommandTest {
private final MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(
m_kinematics, new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
m_kinematics,
new Rotation2d(0),
new MecanumDriveWheelPositions(),
new Pose2d(0, 0, new Rotation2d(0)));
public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond;
@@ -75,13 +83,13 @@ class MecanumControllerCommandTest {
this.m_rearRightSpeed = wheelSpeeds.rearRightMetersPerSecond;
}
public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
return new MecanumDriveWheelSpeeds(
m_frontLeftSpeed, m_frontRightSpeed, m_rearLeftSpeed, m_rearRightSpeed);
public MecanumDriveWheelPositions getCurrentWheelDistances() {
return new MecanumDriveWheelPositions(
m_frontLeftDistance, m_frontRightDistance, m_rearLeftDistance, m_rearRightDistance);
}
public Pose2d getRobotPose() {
m_odometry.updateWithTime(m_timer.get(), m_angle, getCurrentWheelSpeeds());
m_odometry.update(m_angle, getCurrentWheelDistances());
return m_odometry.getPoseMeters();
}
@@ -117,6 +125,10 @@ class MecanumControllerCommandTest {
while (!command.isFinished()) {
command.execute();
m_angle = trajectory.sample(m_timer.get()).poseMeters.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();

20
wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
View File

@@ -32,9 +32,13 @@ class MecanumControllerCommandTest : public ::testing::Test {
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,
@@ -55,6 +59,7 @@ class MecanumControllerCommandTest : public ::testing::Test {
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(); }
@@ -66,8 +71,17 @@ class MecanumControllerCommandTest : public ::testing::Test {
m_rearLeftSpeed, m_rearRightSpeed};
}
frc::MecanumDriveWheelPositions getCurrentWheelDistances() {
return frc::MecanumDriveWheelPositions{
m_frontLeftDistance,
m_rearLeftDistance,
m_frontRightDistance,
m_rearRightDistance,
};
}
frc::Pose2d getRobotPose() {
m_odometry.UpdateWithTime(m_timer.Get(), m_angle, getCurrentWheelSpeeds());
m_odometry.Update(m_angle, getCurrentWheelDistances());
return m_odometry.GetPose();
}
};
@@ -105,6 +119,10 @@ TEST_F(MecanumControllerCommandTest, ReachesReference) {
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();