From 9725aff83bbf5afc8f0ba3ad1894f6c10d3c2ca4 Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Sun, 27 Sep 2020 16:25:17 -0400 Subject: [PATCH] [wpilib] Clean up DifferentialDrivetrainSim API (#2747) Co-authored-by: Tyler Veness --- .../simulation/DifferentialDrivetrainSim.cpp | 2 +- .../simulation/DifferentialDrivetrainSim.h | 21 +++++++++++-------- .../simulation/DifferentialDriveSimTest.cpp | 5 ++--- .../simulation/DifferentialDrivetrainSim.java | 16 +++++++------- .../DifferentialDrivetrainSimTest.java | 4 ++-- 5 files changed, 26 insertions(+), 22 deletions(-) diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 35e2e7f58c..fff76f22fc 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -66,7 +66,7 @@ Rotation2d DifferentialDrivetrainSim::GetHeading() const { return Rotation2d(units::radian_t(m_x(State::kHeading))); } -Pose2d DifferentialDrivetrainSim::GetEstimatedPosition() const { +Pose2d DifferentialDrivetrainSim::GetPose() const { return Pose2d(units::meter_t(m_x(State::kX)), units::meter_t(m_x(State::kY)), Rotation2d(units::radian_t(m_x(State::kHeading)))); } diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h index a9dc8a33bf..c6d19c6c15 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h @@ -62,7 +62,7 @@ class DifferentialDrivetrainSim { units::meter_t trackWidth); /** - * Set the applied voltage to the drivetrain. Note that positive voltage must + * Sets the applied voltage to the drivetrain. Note that positive voltage must * make that side of the drivetrain travel forward (+X). * * @param leftVoltage The left voltage. @@ -71,7 +71,7 @@ class DifferentialDrivetrainSim { void SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage); /** - * Set the gearing reduction on the drivetrain. This is commonly used for + * Sets the gearing reduction on the drivetrain. This is commonly used for * shifting drivetrains. * * @param newGearing The new gear ratio, as output over input. @@ -94,7 +94,8 @@ class DifferentialDrivetrainSim { double GetState(int state) const; /** - * Get the current gearing reduction of the drivetrain, as output over input. + * Returns the current gearing reduction of the drivetrain, as output over + * input. */ double GetGearing() const; @@ -104,15 +105,17 @@ class DifferentialDrivetrainSim { Eigen::Matrix GetState() const; /** - * Get the estimated direction the robot is pointing. Note that this angle is - * counterclockwise-positive, while most gyros are clockwise positive. + * Returns the direction the robot is pointing. + * + * Note that this angle is counterclockwise-positive, while most gyros are + * clockwise positive. */ Rotation2d GetHeading() const; /** - * Returns the current estimated position. + * Returns the current pose. */ - Pose2d GetEstimatedPosition() const; + Pose2d GetPose() const; /** * Returns the currently drawn current. @@ -173,7 +176,7 @@ class DifferentialDrivetrainSim { * @param gearing The gearing reduction used. * @param wheelSize The wheel size. */ - static DifferentialDrivetrainSim createKitbotSim(frc::DCMotor motor, + static DifferentialDrivetrainSim CreateKitbotSim(frc::DCMotor motor, double gearing, units::meter_t wheelSize) { // MOI estimation -- note that I = m r^2 for point masses @@ -195,7 +198,7 @@ class DifferentialDrivetrainSim { * @param J The moment of inertia of the drivebase. This can be * calculated using frc-characterization. */ - static DifferentialDrivetrainSim createKitbotSim( + static DifferentialDrivetrainSim CreateKitbotSim( frc::DCMotor motor, double gearing, units::meter_t wheelSize, units::kilogram_square_meter_t J) { return DifferentialDrivetrainSim{motor, gearing, J, diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp index b4aff7aa0e..f09344ead3 100644 --- a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp @@ -46,7 +46,7 @@ TEST(DifferentialDriveSim, Convergence) { for (double t = 0; t < trajectory.TotalTime().to(); t += 0.02) { auto state = trajectory.Sample(20_ms); - auto ramseteOut = ramsete.Calculate(sim.GetEstimatedPosition(), state); + auto ramseteOut = ramsete.Calculate(sim.GetPose(), state); auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut); auto voltages = feedforward.Calculate( @@ -111,6 +111,5 @@ TEST(DifferentialDriveSim, ModelStability) { sim.Update(20_ms); } - EXPECT_LT(units::math::abs(sim.GetEstimatedPosition().Translation().Norm()), - 100_m); + EXPECT_LT(units::math::abs(sim.GetPose().Translation().Norm()), 100_m); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java index 6791fd2e73..bf09e67ac5 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java @@ -101,7 +101,7 @@ public class DifferentialDrivetrainSim { } /** - * Set the applied voltage to the drivetrain. Note that positive voltage must make that + * Sets the applied voltage to the drivetrain. Note that positive voltage must make that * side of the drivetrain travel forward (+X). * * @param leftVoltageVolts The left voltage. @@ -123,24 +123,26 @@ public class DifferentialDrivetrainSim { } /** - * Get the full simulated state of the drivetrain. + * Returns the full simulated state of the drivetrain. */ public Matrix getState() { return m_x; } /** - * Get the estimated direction the robot is pointing. Note that this angle is - * counterclockwise-positive, while most gyros are clockwise positive. + * Returns the direction the robot is pointing. + * + *

Note that this angle is counterclockwise-positive, while most gyros are + * clockwise positive. */ public Rotation2d getHeading() { return new Rotation2d(getState(State.kHeading)); } /** - * Get the estimated position of the drivetrain. + * Returns the current pose. */ - public Pose2d getEstimatedPosition() { + public Pose2d getPose() { return new Pose2d(m_x.get(0, 0), m_x.get(1, 0), new Rotation2d(m_x.get(2, 0))); @@ -163,7 +165,7 @@ public class DifferentialDrivetrainSim { } /** - * Set the gearing reduction on the drivetrain. This is commonly used for + * Sets the gearing reduction on the drivetrain. This is commonly used for * shifting drivetrains. * * @param newGearRatio The new gear ratio, as output over input. diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java index 7ac5521101..4a0eab3dec 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java @@ -65,7 +65,7 @@ class DifferentialDrivetrainSimTest { for (double t = 0; t < traj.getTotalTimeSeconds(); t += 0.020) { var state = traj.sample(0.020); - var ramseteOut = ramsete.calculate(sim.getEstimatedPosition(), state); + var ramseteOut = ramsete.calculate(sim.getPose(), state); var wheelSpeeds = kinematics.toWheelSpeeds(ramseteOut); @@ -141,6 +141,6 @@ class DifferentialDrivetrainSimTest { sim.update(0.020); } - assertTrue(Math.abs(sim.getEstimatedPosition().getTranslation().getNorm()) < 100); + assertTrue(Math.abs(sim.getPose().getTranslation().getNorm()) < 100); } }