diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 4bf418d919..81abcb845b 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -86,18 +86,18 @@ double DifferentialDrivetrainSim::GetState(int state) const { } wpi::math::Rotation2d DifferentialDrivetrainSim::GetHeading() const { - return wpi::units::radian_t{GetOutput(State::kHeading)}; + return wpi::units::radian_t{GetOutput(State::HEADING)}; } wpi::math::Pose2d DifferentialDrivetrainSim::GetPose() const { - return wpi::math::Pose2d{wpi::units::meter_t{GetOutput(State::kX)}, - wpi::units::meter_t{GetOutput(State::kY)}, + return wpi::math::Pose2d{wpi::units::meter_t{GetOutput(State::X)}, + wpi::units::meter_t{GetOutput(State::Y)}, GetHeading()}; } wpi::units::ampere_t DifferentialDrivetrainSim::GetLeftCurrentDraw() const { return m_motor.Current( - wpi::units::radians_per_second_t{m_x(State::kLeftVelocity) * + wpi::units::radians_per_second_t{m_x(State::LEFT_VELOCITY) * m_currentGearing / m_wheelRadius.value()}, wpi::units::volt_t{m_u(0)}) * @@ -106,7 +106,7 @@ wpi::units::ampere_t DifferentialDrivetrainSim::GetLeftCurrentDraw() const { wpi::units::ampere_t DifferentialDrivetrainSim::GetRightCurrentDraw() const { return m_motor.Current( - wpi::units::radians_per_second_t{m_x(State::kRightVelocity) * + wpi::units::radians_per_second_t{m_x(State::RIGHT_VELOCITY) * m_currentGearing / m_wheelRadius.value()}, wpi::units::volt_t{m_u(1)}) * @@ -122,11 +122,11 @@ void DifferentialDrivetrainSim::SetState(const wpi::math::Vectord<7>& state) { } void DifferentialDrivetrainSim::SetPose(const wpi::math::Pose2d& pose) { - m_x(State::kX) = pose.X().value(); - m_x(State::kY) = pose.Y().value(); - m_x(State::kHeading) = pose.Rotation().Radians().value(); - m_x(State::kLeftPosition) = 0; - m_x(State::kRightPosition) = 0; + m_x(State::X) = pose.X().value(); + m_x(State::Y) = pose.Y().value(); + m_x(State::HEADING) = pose.Rotation().Radians().value(); + m_x(State::LEFT_POSITION) = 0; + m_x(State::RIGHT_POSITION) = 0; } wpi::math::Vectord<7> DifferentialDrivetrainSim::Dynamics( @@ -147,13 +147,13 @@ wpi::math::Vectord<7> DifferentialDrivetrainSim::Dynamics( A.block<2, 2>(2, 0).setIdentity(); A.block<4, 2>(0, 2).setZero(); - double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0; + double v = (x(State::LEFT_VELOCITY) + x(State::RIGHT_VELOCITY)) / 2.0; wpi::math::Vectord<7> xdot; - xdot(0) = v * std::cos(x(State::kHeading)); - xdot(1) = v * std::sin(x(State::kHeading)); + xdot(0) = v * std::cos(x(State::HEADING)); + xdot(1) = v * std::sin(x(State::HEADING)); xdot(2) = - ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / (2.0 * m_rb)) + ((x(State::RIGHT_VELOCITY) - x(State::LEFT_VELOCITY)) / (2.0 * m_rb)) .value(); xdot.block<4, 1>(3, 0) = A * x.block<4, 1>(3, 0) + B * u; return xdot; diff --git a/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp b/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp index f1a7f1cb28..8fc0219a31 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp @@ -131,7 +131,7 @@ class DifferentialDrivetrainSim { * @return The encoder position. */ wpi::units::meter_t GetRightPosition() const { - return wpi::units::meter_t{GetOutput(State::kRightPosition)}; + return wpi::units::meter_t{GetOutput(State::RIGHT_POSITION)}; } /** @@ -139,7 +139,7 @@ class DifferentialDrivetrainSim { * @return The encoder velocity. */ wpi::units::meters_per_second_t GetRightVelocity() const { - return wpi::units::meters_per_second_t{GetOutput(State::kRightVelocity)}; + return wpi::units::meters_per_second_t{GetOutput(State::RIGHT_VELOCITY)}; } /** @@ -147,7 +147,7 @@ class DifferentialDrivetrainSim { * @return The encoder position. */ wpi::units::meter_t GetLeftPosition() const { - return wpi::units::meter_t{GetOutput(State::kLeftPosition)}; + return wpi::units::meter_t{GetOutput(State::LEFT_POSITION)}; } /** @@ -155,7 +155,7 @@ class DifferentialDrivetrainSim { * @return The encoder velocity. */ wpi::units::meters_per_second_t GetLeftVelocity() const { - return wpi::units::meters_per_second_t{GetOutput(State::kLeftVelocity)}; + return wpi::units::meters_per_second_t{GetOutput(State::LEFT_VELOCITY)}; } /** @@ -199,13 +199,13 @@ class DifferentialDrivetrainSim { class State { public: - static constexpr int kX = 0; - static constexpr int kY = 1; - static constexpr int kHeading = 2; - static constexpr int kLeftVelocity = 3; - static constexpr int kRightVelocity = 4; - static constexpr int kLeftPosition = 5; - static constexpr int kRightPosition = 6; + static constexpr int X = 0; + static constexpr int Y = 1; + static constexpr int HEADING = 2; + static constexpr int LEFT_VELOCITY = 3; + static constexpr int RIGHT_VELOCITY = 4; + static constexpr int LEFT_POSITION = 5; + static constexpr int RIGHT_POSITION = 6; }; /** @@ -219,15 +219,15 @@ class DifferentialDrivetrainSim { class KitbotGearing { public: /// Gear ratio of 12.75:1. - static constexpr double k12p75 = 12.75; + static constexpr double RATIO_12P75 = 12.75; /// Gear ratio of 10.71:1. - static constexpr double k10p71 = 10.71; + static constexpr double RATIO_10P71 = 10.71; /// Gear ratio of 8.45:1. - static constexpr double k8p45 = 8.45; + static constexpr double RATIO_8P45 = 8.45; /// Gear ratio of 7.31:1. - static constexpr double k7p31 = 7.31; + static constexpr double RATIO_7P31 = 7.31; /// Gear ratio of 5.95:1. - static constexpr double k5p95 = 5.95; + static constexpr double RATIO_5P95 = 5.95; }; /** @@ -236,28 +236,28 @@ class DifferentialDrivetrainSim { class KitbotMotor { public: /// One CIM motor per drive side. - static constexpr wpi::math::DCMotor SingleCIMPerSide = + static constexpr wpi::math::DCMotor SINGLE_CIM_PER_SIDE = wpi::math::DCMotor::CIM(1); /// Two CIM motors per drive side. - static constexpr wpi::math::DCMotor DualCIMPerSide = + static constexpr wpi::math::DCMotor DUAL_CIM_PER_SIDE = wpi::math::DCMotor::CIM(2); /// One Mini CIM motor per drive side. - static constexpr wpi::math::DCMotor SingleMiniCIMPerSide = + static constexpr wpi::math::DCMotor SINGLE_MINI_CIM_PER_SIDE = wpi::math::DCMotor::MiniCIM(1); /// Two Mini CIM motors per drive side. - static constexpr wpi::math::DCMotor DualMiniCIMPerSide = + static constexpr wpi::math::DCMotor DUAL_MINI_CIM_PER_SIDE = wpi::math::DCMotor::MiniCIM(2); /// One Falcon 500 motor per drive side. - static constexpr wpi::math::DCMotor SingleFalcon500PerSide = + static constexpr wpi::math::DCMotor SINGLE_FALCON_500_PER_SIDE = wpi::math::DCMotor::Falcon500(1); /// Two Falcon 500 motors per drive side. - static constexpr wpi::math::DCMotor DualFalcon500PerSide = + static constexpr wpi::math::DCMotor DUAL_FALCON_500_PER_SIDE = wpi::math::DCMotor::Falcon500(2); /// One NEO motor per drive side. - static constexpr wpi::math::DCMotor SingleNEOPerSide = + static constexpr wpi::math::DCMotor SINGLE_NEO_PER_SIDE = wpi::math::DCMotor::NEO(1); /// Two NEO motors per drive side. - static constexpr wpi::math::DCMotor DualNEOPerSide = + static constexpr wpi::math::DCMotor DUAL_NEO_PER_SIDE = wpi::math::DCMotor::NEO(2); }; @@ -267,11 +267,11 @@ class DifferentialDrivetrainSim { class KitbotWheelSize { public: /// Six inch diameter wheels. - static constexpr wpi::units::meter_t kSixInch = 6_in; + static constexpr wpi::units::meter_t SIX_INCH = 6_in; /// Eight inch diameter wheels. - static constexpr wpi::units::meter_t kEightInch = 8_in; + static constexpr wpi::units::meter_t EIGHT_INCH = 8_in; /// Ten inch diameter wheels. - static constexpr wpi::units::meter_t kTenInch = 10_in; + static constexpr wpi::units::meter_t TEN_INCH = 10_in; }; /** diff --git a/wpilibc/src/main/python/semiwrap/simulation/DifferentialDrivetrainSim.yml b/wpilibc/src/main/python/semiwrap/simulation/DifferentialDrivetrainSim.yml index 5ca862b02e..cc2bd28432 100644 --- a/wpilibc/src/main/python/semiwrap/simulation/DifferentialDrivetrainSim.yml +++ b/wpilibc/src/main/python/semiwrap/simulation/DifferentialDrivetrainSim.yml @@ -43,35 +43,35 @@ classes: default: std::array{} wpi::sim::DifferentialDrivetrainSim::State: attributes: - kX: - kY: - kHeading: - kLeftVelocity: - kRightVelocity: - kLeftPosition: - kRightPosition: + X: + Y: + HEADING: + LEFT_VELOCITY: + RIGHT_VELOCITY: + LEFT_POSITION: + RIGHT_POSITION: wpi::sim::DifferentialDrivetrainSim::KitbotGearing: attributes: - k12p75: - k10p71: - k8p45: - k7p31: - k5p95: + RATIO_12P75: + RATIO_10P71: + RATIO_8P45: + RATIO_7P31: + RATIO_5P95: wpi::sim::DifferentialDrivetrainSim::KitbotMotor: attributes: - SingleCIMPerSide: - DualCIMPerSide: - SingleMiniCIMPerSide: - DualMiniCIMPerSide: - SingleFalcon500PerSide: - DualFalcon500PerSide: - SingleNEOPerSide: - DualNEOPerSide: + SINGLE_CIM_PER_SIDE: + DUAL_CIM_PER_SIDE: + SINGLE_MINI_CIM_PER_SIDE: + DUAL_MINI_CIM_PER_SIDE: + SINGLE_FALCON_500_PER_SIDE: + DUAL_FALCON_500_PER_SIDE: + SINGLE_NEO_PER_SIDE: + DUAL_NEO_PER_SIDE: wpi::sim::DifferentialDrivetrainSim::KitbotWheelSize: attributes: - kSixInch: - kEightInch: - kTenInch: + SIX_INCH: + EIGHT_INCH: + TEN_INCH: inline_code: |- cls_DifferentialDrivetrainSim diff --git a/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java index 89bc82521e..a3d3354f3a 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java @@ -177,7 +177,7 @@ public class DifferentialDrivetrainSim { * @return The direction the robot is pointing. */ public Rotation2d getHeading() { - return new Rotation2d(getOutput(State.kHeading)); + return new Rotation2d(getOutput(State.HEADING)); } /** @@ -186,7 +186,7 @@ public class DifferentialDrivetrainSim { * @return The current pose. */ public Pose2d getPose() { - return new Pose2d(getOutput(State.kX), getOutput(State.kY), getHeading()); + return new Pose2d(getOutput(State.X), getOutput(State.Y), getHeading()); } /** @@ -195,7 +195,7 @@ public class DifferentialDrivetrainSim { * @return The encoder position in meters. */ public double getRightPosition() { - return getOutput(State.kRightPosition); + return getOutput(State.RIGHT_POSITION); } /** @@ -204,7 +204,7 @@ public class DifferentialDrivetrainSim { * @return The encoder velocity in meters per second. */ public double getRightVelocity() { - return getOutput(State.kRightVelocity); + return getOutput(State.RIGHT_VELOCITY); } /** @@ -213,7 +213,7 @@ public class DifferentialDrivetrainSim { * @return The encoder position in meters. */ public double getLeftPosition() { - return getOutput(State.kLeftPosition); + return getOutput(State.LEFT_POSITION); } /** @@ -222,7 +222,7 @@ public class DifferentialDrivetrainSim { * @return The encoder velocity in meters per second. */ public double getLeftVelocity() { - return getOutput(State.kLeftVelocity); + return getOutput(State.LEFT_VELOCITY); } /** @@ -232,7 +232,7 @@ public class DifferentialDrivetrainSim { */ public double getLeftCurrentDraw() { return m_motor.getCurrent( - getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadius, m_u.get(0, 0)) + getState(State.LEFT_VELOCITY) * m_currentGearing / m_wheelRadius, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); } @@ -243,7 +243,7 @@ public class DifferentialDrivetrainSim { */ public double getRightCurrentDraw() { return m_motor.getCurrent( - getState(State.kRightVelocity) * m_currentGearing / m_wheelRadius, m_u.get(1, 0)) + getState(State.RIGHT_VELOCITY) * m_currentGearing / m_wheelRadius, m_u.get(1, 0)) * Math.signum(m_u.get(1, 0)); } @@ -289,11 +289,11 @@ public class DifferentialDrivetrainSim { * @param pose The pose. */ public void setPose(Pose2d pose) { - m_x.set(State.kX.value, 0, pose.getX()); - m_x.set(State.kY.value, 0, pose.getY()); - m_x.set(State.kHeading.value, 0, pose.getRotation().getRadians()); - m_x.set(State.kLeftPosition.value, 0, 0); - m_x.set(State.kRightPosition.value, 0, 0); + m_x.set(State.X.value, 0, pose.getX()); + m_x.set(State.Y.value, 0, pose.getY()); + m_x.set(State.HEADING.value, 0, pose.getRotation().getRadians()); + m_x.set(State.LEFT_POSITION.value, 0, 0); + m_x.set(State.RIGHT_POSITION.value, 0, 0); } /** @@ -323,15 +323,15 @@ public class DifferentialDrivetrainSim { A.assignBlock(2, 0, Matrix.eye(Nat.N2())); - var v = (x.get(State.kLeftVelocity.value, 0) + x.get(State.kRightVelocity.value, 0)) / 2.0; + var v = (x.get(State.LEFT_VELOCITY.value, 0) + x.get(State.RIGHT_VELOCITY.value, 0)) / 2.0; var xdot = new Matrix<>(Nat.N7(), Nat.N1()); - xdot.set(0, 0, v * Math.cos(x.get(State.kHeading.value, 0))); - xdot.set(1, 0, v * Math.sin(x.get(State.kHeading.value, 0))); + xdot.set(0, 0, v * Math.cos(x.get(State.HEADING.value, 0))); + xdot.set(1, 0, v * Math.sin(x.get(State.HEADING.value, 0))); xdot.set( 2, 0, - (x.get(State.kRightVelocity.value, 0) - x.get(State.kLeftVelocity.value, 0)) + (x.get(State.RIGHT_VELOCITY.value, 0) - x.get(State.LEFT_VELOCITY.value, 0)) / (2.0 * m_rb)); xdot.assignBlock(3, 0, A.times(x.block(Nat.N4(), Nat.N1(), 3, 0)).plus(B.times(u))); @@ -351,13 +351,13 @@ public class DifferentialDrivetrainSim { /** Represents the different states of the drivetrain. */ enum State { - kX(0), - kY(1), - kHeading(2), - kLeftVelocity(3), - kRightVelocity(4), - kLeftPosition(5), - kRightPosition(6); + X(0), + Y(1), + HEADING(2), + LEFT_VELOCITY(3), + RIGHT_VELOCITY(4), + LEFT_POSITION(5), + RIGHT_POSITION(6); public final int value; @@ -372,15 +372,15 @@ public class DifferentialDrivetrainSim { */ public enum KitbotGearing { /** Gear ratio of 12.75:1. */ - k12p75(12.75), + RATIO_12P75(12.75), /** Gear ratio of 10.71:1. */ - k10p71(10.71), + RATIO_10P71(10.71), /** Gear ratio of 8.45:1. */ - k8p45(8.45), + RATIO_8P45(8.45), /** Gear ratio of 7.31:1. */ - k7p31(7.31), + RATIO_7P31(7.31), /** Gear ratio of 5.95:1. */ - k5p95(5.95); + RATIO_5P95(5.95); /** KitbotGearing value. */ public final double value; @@ -393,21 +393,21 @@ public class DifferentialDrivetrainSim { /** Represents common motor layouts of the kit drivetrain. */ public enum KitbotMotor { /** One CIM motor per drive side. */ - kSingleCIMPerSide(DCMotor.getCIM(1)), + SINGLE_CIM_PER_SIDE(DCMotor.getCIM(1)), /** Two CIM motors per drive side. */ - kDualCIMPerSide(DCMotor.getCIM(2)), + DUAL_CIM_PER_SIDE(DCMotor.getCIM(2)), /** One Mini CIM motor per drive side. */ - kSingleMiniCIMPerSide(DCMotor.getMiniCIM(1)), + SINGLE_MINI_CIM_PER_SIDE(DCMotor.getMiniCIM(1)), /** Two Mini CIM motors per drive side. */ - kDualMiniCIMPerSide(DCMotor.getMiniCIM(2)), + DUAL_MINI_CIM_PER_SIDE(DCMotor.getMiniCIM(2)), /** One Falcon 500 motor per drive side. */ - kSingleFalcon500PerSide(DCMotor.getFalcon500(1)), + SINGLE_FALCON_500_PER_SIDE(DCMotor.getFalcon500(1)), /** Two Falcon 500 motors per drive side. */ - kDoubleFalcon500PerSide(DCMotor.getFalcon500(2)), + DUAL_FALCON_500_PER_SIDE(DCMotor.getFalcon500(2)), /** One NEO motor per drive side. */ - kSingleNEOPerSide(DCMotor.getNEO(1)), + SINGLE_NEO_PER_SIDE(DCMotor.getNEO(1)), /** Two NEO motors per drive side. */ - kDoubleNEOPerSide(DCMotor.getNEO(2)); + DUAL_NEO_PER_SIDE(DCMotor.getNEO(2)); /** KitbotMotor value. */ public final DCMotor value; @@ -420,11 +420,11 @@ public class DifferentialDrivetrainSim { /** Represents common wheel sizes of the kit drivetrain. */ public enum KitbotWheelSize { /** Six inch diameter wheels. */ - kSixInch(Units.inchesToMeters(6)), + SIX_INCH(Units.inchesToMeters(6)), /** Eight inch diameter wheels. */ - kEightInch(Units.inchesToMeters(8)), + EIGHT_INCH(Units.inchesToMeters(8)), /** Ten inch diameter wheels. */ - kTenInch(Units.inchesToMeters(10)); + TEN_INCH(Units.inchesToMeters(10)); /** KitbotWheelSize value. */ public final double value; diff --git a/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java index 52e4504bbf..852712926c 100644 --- a/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java +++ b/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java @@ -81,16 +81,16 @@ class DifferentialDrivetrainSimTest { // 2 inch tolerance is OK since our ground truth is an approximation of the // ODE solution using RKDP anyway assertEquals( - groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0), - sim.getState(DifferentialDrivetrainSim.State.kX), + groundTruthX.get(DifferentialDrivetrainSim.State.X.value, 0), + sim.getState(DifferentialDrivetrainSim.State.X), 0.05); assertEquals( - groundTruthX.get(DifferentialDrivetrainSim.State.kY.value, 0), - sim.getState(DifferentialDrivetrainSim.State.kY), + groundTruthX.get(DifferentialDrivetrainSim.State.Y.value, 0), + sim.getState(DifferentialDrivetrainSim.State.Y), 0.05); assertEquals( - groundTruthX.get(DifferentialDrivetrainSim.State.kHeading.value, 0), - sim.getState(DifferentialDrivetrainSim.State.kHeading), + groundTruthX.get(DifferentialDrivetrainSim.State.HEADING.value, 0), + sim.getState(DifferentialDrivetrainSim.State.HEADING), 0.01); }