diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index 51ed94a66c..bf84cb2b1f 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -16,25 +16,28 @@ using namespace frc::sim; DifferentialDrivetrainSim::DifferentialDrivetrainSim( const LinearSystem<2, 2, 2>& plant, units::meter_t trackWidth, - DCMotor driveMotor, double gearRatio, units::meter_t wheelRadius) + DCMotor driveMotor, double gearRatio, units::meter_t wheelRadius, + const std::array& measurementStdDevs) : m_plant(plant), m_rb(trackWidth / 2.0), m_wheelRadius(wheelRadius), m_motor(driveMotor), m_originalGearing(gearRatio), - m_currentGearing(gearRatio) { + m_currentGearing(gearRatio), + m_measurementStdDevs(measurementStdDevs) { m_x.setZero(); m_u.setZero(); + m_y.setZero(); } DifferentialDrivetrainSim::DifferentialDrivetrainSim( frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J, units::kilogram_t mass, units::meter_t wheelRadius, - units::meter_t trackWidth) + units::meter_t trackWidth, const std::array& measurementStdDevs) : DifferentialDrivetrainSim( frc::LinearSystemId::DrivetrainVelocitySystem( driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing), - trackWidth, driveMotor, gearing, wheelRadius) {} + trackWidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {} void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage) { @@ -48,27 +51,36 @@ void DifferentialDrivetrainSim::SetGearing(double newGearing) { void DifferentialDrivetrainSim::Update(units::second_t dt) { m_x = RungeKutta([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt); -} - -double DifferentialDrivetrainSim::GetState(int state) const { - return m_x(state); + m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs); } double DifferentialDrivetrainSim::GetGearing() const { return m_currentGearing; } +Eigen::Matrix DifferentialDrivetrainSim::GetOutput() const { + return m_y; +} + Eigen::Matrix DifferentialDrivetrainSim::GetState() const { return m_x; } +double DifferentialDrivetrainSim::GetOutput(int output) const { + return m_y(output); +} + +double DifferentialDrivetrainSim::GetState(int state) const { + return m_x(state); +} + Rotation2d DifferentialDrivetrainSim::GetHeading() const { - return Rotation2d(units::radian_t(m_x(State::kHeading))); + return Rotation2d(units::radian_t(GetOutput(State::kHeading))); } 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)))); + return Pose2d(units::meter_t(GetOutput(State::kX)), + units::meter_t(GetOutput(State::kY)), GetHeading()); } units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const { diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h index a75edb91e9..ab199840bf 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h @@ -36,10 +36,17 @@ class DifferentialDrivetrainSim { * create the drivetrainPlant. * @param wheelRadiusMeters The radius of the wheels on the drivetrain, in * meters. + * @param measurementStdDevs Standard deviations for measurements, in the form + * [x, y, heading, left velocity, right velocity, left distance, right + * distance]^T. Can be omitted if no noise is desired. Gyro standard + * deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and + * position measurement standard deviations of 0.005 meters are a reasonable + * starting point. */ - DifferentialDrivetrainSim(const LinearSystem<2, 2, 2>& plant, - units::meter_t trackWidth, DCMotor driveMotor, - double gearingRatio, units::meter_t wheelRadius); + DifferentialDrivetrainSim( + const LinearSystem<2, 2, 2>& plant, units::meter_t trackWidth, + DCMotor driveMotor, double gearingRatio, units::meter_t wheelRadius, + const std::array& measurementStdDevs = {}); /** * Create a SimDrivetrain. @@ -55,11 +62,18 @@ class DifferentialDrivetrainSim { * @param wheelRadius The radius of the wheels on the drivetrain. * @param trackWidth The robot's track width, or distance between left and * right wheels. + * @param measurementStdDevs Standard deviations for measurements, in the form + * [x, y, heading, left velocity, right velocity, left distance, right + * distance]^T. Can be omitted if no noise is desired. Gyro standard + * deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and + * position measurement standard deviations of 0.005 meters are a reasonable + * starting point. */ - DifferentialDrivetrainSim(frc::DCMotor driveMotor, double gearing, - units::kilogram_square_meter_t J, - units::kilogram_t mass, units::meter_t wheelRadius, - units::meter_t trackWidth); + DifferentialDrivetrainSim( + frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J, + units::kilogram_t mass, units::meter_t wheelRadius, + units::meter_t trackWidth, + const std::array& measurementStdDevs = {}); /** * Sets the applied voltage to the drivetrain. Note that positive voltage must @@ -86,24 +100,12 @@ class DifferentialDrivetrainSim { */ void Update(units::second_t dt); - /** - * Returns an element of the state vector. - * - * @param state The row of the state vector. - */ - double GetState(int state) const; - /** * Returns the current gearing reduction of the drivetrain, as output over * input. */ double GetGearing() const; - /** - * Returns the current state vector x. - */ - Eigen::Matrix GetState() const; - /** * Returns the direction the robot is pointing. * @@ -117,6 +119,38 @@ class DifferentialDrivetrainSim { */ Pose2d GetPose() const; + /** + * Get the right encoder position in meters. + * @return The encoder position. + */ + units::meter_t GetRightPosition() const { + return units::meter_t{GetOutput(State::kRightPosition)}; + } + + /** + * Get the right encoder velocity in meters per second. + * @return The encoder velocity. + */ + units::meters_per_second_t GetRightVelocity() const { + return units::meters_per_second_t{GetOutput(State::kRightVelocity)}; + } + + /** + * Get the left encoder position in meters. + * @return The encoder position. + */ + units::meter_t GetLeftPosition() const { + return units::meter_t{GetOutput(State::kLeftPosition)}; + } + + /** + * Get the left encoder velocity in meters per second. + * @return The encoder velocity. + */ + units::meters_per_second_t GetLeftVelocity() const { + return units::meters_per_second_t{GetOutput(State::kLeftVelocity)}; + } + /** * Returns the currently drawn current. */ @@ -189,10 +223,16 @@ class DifferentialDrivetrainSim { * @param motor The motors installed in the bot. * @param gearing The gearing reduction used. * @param wheelSize The wheel size. + * @param measurementStdDevs Standard deviations for measurements, in the form + * [x, y, heading, left velocity, right velocity, left distance, right + * distance]^T. Can be omitted if no noise is desired. Gyro standard + * deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and + * position measurement standard deviations of 0.005 meters are a reasonable + * starting point. */ - static DifferentialDrivetrainSim CreateKitbotSim(frc::DCMotor motor, - double gearing, - units::meter_t wheelSize) { + static DifferentialDrivetrainSim CreateKitbotSim( + frc::DCMotor motor, double gearing, units::meter_t wheelSize, + const std::array& measurementStdDevs = {}) { // MOI estimation -- note that I = m r^2 for point masses units::kilogram_square_meter_t batteryMoi = 12.5_lb * 10_in * 10_in; units::kilogram_square_meter_t gearboxMoi = (2.8_lb + 2.0_lb) * @@ -200,7 +240,8 @@ class DifferentialDrivetrainSim { * (26_in / 2) * (26_in / 2); return DifferentialDrivetrainSim{ - motor, gearing, batteryMoi + gearboxMoi, 25_kg, wheelSize / 2.0, 26_in}; + motor, gearing, batteryMoi + gearboxMoi, 25_kg, + wheelSize / 2.0, 26_in, measurementStdDevs}; } /** @@ -211,15 +252,47 @@ class DifferentialDrivetrainSim { * @param wheelSize The wheel size. * @param J The moment of inertia of the drivebase. This can be * calculated using frc-characterization. + * @param measurementStdDevs Standard deviations for measurements, in the form + * [x, y, heading, left velocity, right velocity, left distance, right + * distance]^T. Can be omitted if no noise is desired. Gyro standard + * deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and + * position measurement standard deviations of 0.005 meters are a reasonable + * starting point. */ static DifferentialDrivetrainSim CreateKitbotSim( frc::DCMotor motor, double gearing, units::meter_t wheelSize, - units::kilogram_square_meter_t J) { - return DifferentialDrivetrainSim{motor, gearing, J, - 25_kg, wheelSize / 2.0, 26_in}; + units::kilogram_square_meter_t J, + const std::array& measurementStdDevs = {}) { + return DifferentialDrivetrainSim{ + motor, gearing, J, 25_kg, wheelSize / 2.0, 26_in, measurementStdDevs}; } private: + /** + * Returns an element of the state vector. + * + * @param output The row of the output vector. + */ + double GetOutput(int output) const; + + /** + * Returns the current output vector y. + */ + Eigen::Matrix GetOutput() const; + + /** + * Returns an element of the state vector. Note that this will not include + * noise! + * + * @param output The row of the output vector. + */ + double GetState(int state) const; + + /** + * Returns the current state vector x. Note that this will not include noise! + */ + Eigen::Matrix GetState() const; + LinearSystem<2, 2, 2> m_plant; units::meter_t m_rb; units::meter_t m_wheelRadius; @@ -231,5 +304,7 @@ class DifferentialDrivetrainSim { Eigen::Matrix m_x; Eigen::Matrix m_u; + Eigen::Matrix m_y; + std::array m_measurementStdDevs; }; } // namespace frc::sim diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h index 4f3c65876f..543f386e45 100644 --- a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h @@ -39,8 +39,7 @@ class LinearSystemSim { * @param measurementStdDevs The standard deviations of the measurements. */ LinearSystemSim(const LinearSystem& system, - const std::array& measurementStdDevs = - std::array{}) + const std::array& measurementStdDevs = {}) : m_plant(system), m_measurementStdDevs(measurementStdDevs) { m_x = Eigen::Matrix::Zero(); m_y = Eigen::Matrix::Zero(); diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp index f09344ead3..1a474ef2eb 100644 --- a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp @@ -26,7 +26,9 @@ TEST(DifferentialDriveSim, Convergence) { motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0); frc::DifferentialDriveKinematics kinematics{24_in}; - frc::sim::DifferentialDrivetrainSim sim{plant, 24_in, motor, 1.0, 2_in}; + frc::sim::DifferentialDrivetrainSim sim{ + plant, 24_in, motor, + 1.0, 2_in, {0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}}; frc::LinearPlantInversionFeedforward feedforward{plant, 20_ms}; frc::RamseteController ramsete; @@ -64,9 +66,10 @@ TEST(DifferentialDriveSim, Convergence) { groundTruthX, voltages, 20_ms); } - EXPECT_NEAR(groundTruthX(0, 0), sim.GetState(0), 1E-6); - EXPECT_NEAR(groundTruthX(1, 0), sim.GetState(1), 1E-6); - EXPECT_NEAR(groundTruthX(2, 0), sim.GetState(2), 1E-6); + EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().to(), 0.01); + EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().to(), 0.01); + EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().to(), + 0.01); } TEST(DifferentialDriveSim, Current) { diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp index 1c44d39eff..b68b785ac6 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp @@ -42,14 +42,14 @@ void DriveSubsystem::SimulationPeriodic() { frc::RobotController::GetInputVoltage()); m_drivetrainSimulator.Update(20_ms); - m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetState( - frc::sim::DifferentialDrivetrainSim::State::kLeftPosition)); - m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetState( - frc::sim::DifferentialDrivetrainSim::State::kLeftVelocity)); - m_rightEncoderSim.SetDistance(m_drivetrainSimulator.GetState( - frc::sim::DifferentialDrivetrainSim::State::kRightPosition)); - m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetState( - frc::sim::DifferentialDrivetrainSim::State::kRightVelocity)); + m_leftEncoderSim.SetDistance( + m_drivetrainSimulator.GetLeftPosition().to()); + m_leftEncoderSim.SetRate( + m_drivetrainSimulator.GetLeftVelocity().to()); + m_rightEncoderSim.SetDistance( + m_drivetrainSimulator.GetRightPosition().to()); + m_rightEncoderSim.SetRate( + m_drivetrainSimulator.GetRightVelocity().to()); m_gyroAngleSim.SetAngle( -m_drivetrainSimulator.GetHeading().Degrees().to()); diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h index 8b92bc0c12..fec15e6996 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h @@ -161,9 +161,12 @@ class DriveSubsystem : public frc2::SubsystemBase { // These classes help simulate our drivetrain. frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ - DriveConstants::kDrivetrainPlant, DriveConstants::kTrackwidth, - DriveConstants::kDrivetrainGearbox, DriveConstants::kDrivetrainGearing, - DriveConstants::kWheelDiameter / 2}; + DriveConstants::kDrivetrainPlant, + DriveConstants::kTrackwidth, + DriveConstants::kDrivetrainGearbox, + DriveConstants::kDrivetrainGearing, + DriveConstants::kWheelDiameter / 2, + {0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}}; frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder}; frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; 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 209282b616..0ec9156d4b 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 @@ -9,6 +9,7 @@ package edu.wpi.first.wpilibj.simulation; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; import edu.wpi.first.wpilibj.system.LinearSystem; import edu.wpi.first.wpilibj.system.RungeKutta; import edu.wpi.first.wpilibj.system.plant.DCMotor; @@ -29,25 +30,26 @@ import edu.wpi.first.wpiutil.math.numbers.N7; * *

Our state-space system is: * - *

x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r, voltError_l, voltError_r, headingError]]^T + *

x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r]]^T * in the field coordinate system (dist_* are wheel distances.) * *

u = [[voltage_l, voltage_r]]^T This is typically the control input of the last timestep * from a LTVDiffDriveController. * - *

y = [[x, y, theta]]^T from a global measurement source(ex. vision), - * or y = [[dist_l, dist_r, theta]] from encoders and gyro. - * + *

y = x */ public class DifferentialDrivetrainSim { private final DCMotor m_motor; private final double m_originalGearing; + private final Matrix m_measurementStdDevs; private double m_currentGearing; private final double m_wheelRadiusMeters; @SuppressWarnings("MemberName") private Matrix m_u; @SuppressWarnings("MemberName") private Matrix m_x; + @SuppressWarnings("MemberName") + private Matrix m_y; private final double m_rb; private final LinearSystem m_plant; @@ -55,21 +57,27 @@ public class DifferentialDrivetrainSim { /** * Create a SimDrivetrain. * - * @param driveMotor A {@link DCMotor} representing the left side of the drivetrain. - * @param gearing The gearing on the drive between motor and wheel, as output over input. - * This must be the same ratio as the ratio used to identify or - * create the drivetrainPlant. - * @param jKgMetersSquared The moment of inertia of the drivetrain about its center. - * @param massKg The mass of the drivebase. - * @param wheelRadiusMeters The radius of the wheels on the drivetrain. - * @param trackWidthMeters The robot's track width, or distance between left and right wheels. + * @param driveMotor A {@link DCMotor} representing the left side of the drivetrain. + * @param gearing The gearing on the drive between motor and wheel, as output over input. + * This must be the same ratio as the ratio used to identify or + * create the drivetrainPlant. + * @param jKgMetersSquared The moment of inertia of the drivetrain about its center. + * @param massKg The mass of the drivebase. + * @param wheelRadiusMeters The radius of the wheels on the drivetrain. + * @param trackWidthMeters The robot's track width, or distance between left and right wheels. + * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading, left velocity, + * right velocity, left distance, right distance]^T. Can be null if no noise is desired. + * Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, + * and position measurement standard deviations of 0.005 meters are a reasonable starting + * point. */ public DifferentialDrivetrainSim(DCMotor driveMotor, double gearing, double jKgMetersSquared, double massKg, - double wheelRadiusMeters, double trackWidthMeters) { + double wheelRadiusMeters, double trackWidthMeters, + Matrix measurementStdDevs) { this(LinearSystemId.createDrivetrainVelocitySystem(driveMotor, massKg, wheelRadiusMeters, trackWidthMeters / 2.0, jKgMetersSquared, gearing), - driveMotor, gearing, trackWidthMeters, wheelRadiusMeters); + driveMotor, gearing, trackWidthMeters, wheelRadiusMeters, measurementStdDevs); } /** @@ -85,20 +93,27 @@ public class DifferentialDrivetrainSim { * @param trackWidthMeters The distance between the two sides of the drivetrian. Can be * found with frc-characterization. * @param wheelRadiusMeters The radius of the wheels on the drivetrain, in meters. + * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading, left velocity, + * right velocity, left distance, right distance]^T. Can be null if no noise is desired. + * Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, + * and position measurement standard deviations of 0.005 meters are a reasonable starting + * point. */ public DifferentialDrivetrainSim(LinearSystem drivetrainPlant, DCMotor driveMotor, double gearing, - double trackWidthMeters, - double wheelRadiusMeters) { + double trackWidthMeters, double wheelRadiusMeters, + Matrix measurementStdDevs) { this.m_plant = drivetrainPlant; this.m_rb = trackWidthMeters / 2.0; this.m_motor = driveMotor; this.m_originalGearing = gearing; + this.m_measurementStdDevs = measurementStdDevs; m_wheelRadiusMeters = wheelRadiusMeters; m_currentGearing = m_originalGearing; m_x = new Matrix<>(Nat.N7(), Nat.N1()); m_u = VecBuilder.fill(0, 0); + m_y = new Matrix<>(Nat.N7(), Nat.N1()); } /** @@ -117,19 +132,27 @@ public class DifferentialDrivetrainSim { // Update state estimate with RK4 m_x = RungeKutta.rungeKutta(this::getDynamics, m_x, m_u, dtSeconds); - } - - public double getState(State state) { - return m_x.get(state.value, 0); + m_y = m_x; + if (m_measurementStdDevs != null) { + m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs)); + } } /** * Returns the full simulated state of the drivetrain. */ - public Matrix getState() { + Matrix getState() { return m_x; } + double getState(State state) { + return m_x.get(state.value, 0); + } + + private double getOutput(State output) { + return m_y.get(output.value, 0); + } + /** * Returns the direction the robot is pointing. * @@ -137,16 +160,47 @@ public class DifferentialDrivetrainSim { * clockwise positive. */ public Rotation2d getHeading() { - return new Rotation2d(getState(State.kHeading)); + return new Rotation2d(getOutput(State.kHeading)); } /** * Returns the current pose. */ public Pose2d getPose() { - return new Pose2d(m_x.get(0, 0), - m_x.get(1, 0), - new Rotation2d(m_x.get(2, 0))); + return new Pose2d(getOutput(State.kX), getOutput(State.kY), + getHeading()); + } + + /** + * Get the right encoder position in meters. + * @return The encoder position. + */ + public double getRightPositionMeters() { + return getOutput(State.kRightPosition); + } + + /** + * Get the right encoder velocity in meters per second. + * @return The encoder velocity. + */ + public double getRightVelocityMetersPerSecond() { + return getOutput(State.kRightVelocity); + } + + /** + * Get the left encoder position in meters. + * @return The encoder position. + */ + public double getLeftPositionMeters() { + return getOutput(State.kLeftPosition); + } + + /** + * Get the left encoder velocity in meters per second. + * @return The encoder velocity. + */ + public double getLeftVelocityMetersPerSecond() { + return getOutput(State.kLeftVelocity); } public double getCurrentDrawAmps() { @@ -227,7 +281,7 @@ public class DifferentialDrivetrainSim { return xdot; } - public enum State { + enum State { kX(0), kY(1), kHeading(2), @@ -300,15 +354,21 @@ public class DifferentialDrivetrainSim { * @param motor The motors installed in the bot. * @param gearing The gearing reduction used. * @param wheelSize The wheel size. + * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading, left velocity, + * right velocity, left distance, right distance]^T. Can be null if no noise is desired. + * Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, + * and position measurement standard deviations of 0.005 meters are a reasonable starting + * point. */ public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing, - KitbotWheelSize wheelSize) { + KitbotWheelSize wheelSize, + Matrix measurementStdDevs) { // MOI estimation -- note that I = m r^2 for point masses var batteryMoi = 12.5 / 2.2 * Math.pow(Units.inchesToMeters(10), 2); var gearboxMoi = (2.8 /* CIM motor */ * 2 / 2.2 + 2.0 /* Toughbox Mini- ish */) * Math.pow(Units.inchesToMeters(26.0 / 2.0), 2); - return createKitbotSim(motor, gearing, wheelSize, batteryMoi + gearboxMoi); + return createKitbotSim(motor, gearing, wheelSize, batteryMoi + gearboxMoi, measurementStdDevs); } /** @@ -319,10 +379,16 @@ public class DifferentialDrivetrainSim { * @param wheelSize The wheel size. * @param jKgMetersSquared The moment of inertia of the drivebase. This can be calculated using * frc-characterization. + * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading, left velocity, + * right velocity, left distance, right distance]^T. Can be null if no noise is desired. + * Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, + * and position measurement standard deviations of 0.005 meters are a reasonable starting + * point. */ public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing, - KitbotWheelSize wheelSize, double jKgMetersSquared) { + KitbotWheelSize wheelSize, double jKgMetersSquared, + Matrix measurementStdDevs) { return new DifferentialDrivetrainSim(motor.value, gearing.value, jKgMetersSquared, 25 / 2.2, - wheelSize.value / 2.0, Units.inchesToMeters(26)); + wheelSize.value / 2.0, Units.inchesToMeters(26), measurementStdDevs); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java index dc69ffa33f..18ca6c0ee1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java @@ -60,7 +60,7 @@ public class LinearSystemSim system, Matrix measurementStdDevs) { 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 4a0eab3dec..ff832a0baf 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 @@ -47,7 +47,7 @@ class DifferentialDrivetrainSimTest { var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); var sim = new DifferentialDrivetrainSim(plant, - motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2)); + motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2), VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005)); var feedforward = new LinearPlantInversionFeedforward<>(plant, 0.020); var ramsete = new RamseteController(); @@ -99,7 +99,7 @@ class DifferentialDrivetrainSimTest { 0.5, 1.0); var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); - var sim = new DifferentialDrivetrainSim(plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2)); + var sim = new DifferentialDrivetrainSim(plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2), null); sim.setInputs(-12, -12); for (int i = 0; i < 10; i++) { @@ -132,7 +132,7 @@ class DifferentialDrivetrainSimTest { 5.0); var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); - var sim = new DifferentialDrivetrainSim(plant, motor, 5, kinematics.trackWidthMeters, Units.inchesToMeters(2)); + var sim = new DifferentialDrivetrainSim(plant, motor, 5, kinematics.trackWidthMeters, Units.inchesToMeters(2), VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005)); sim.setInputs(2, 4); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java index 3a0d651c73..d1f4d22dbc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpiutil.math.VecBuilder; public class DriveSubsystem extends SubsystemBase { // The motors on the left side of the drive. @@ -87,7 +88,8 @@ public class DriveSubsystem extends SubsystemBase { Constants.DriveConstants.kDriveGearbox, Constants.DriveConstants.kDriveGearing, Constants.DriveConstants.kTrackwidthMeters, - Constants.DriveConstants.kWheelDiameterMeters / 2.0); + Constants.DriveConstants.kWheelDiameterMeters / 2.0, + VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005)); // The encoder and gyro angle sims let us set simulated sensor readings m_leftEncoderSim = new EncoderSim(m_leftEncoder); @@ -118,10 +120,10 @@ public class DriveSubsystem extends SubsystemBase { -m_rightMotors.get() * RobotController.getBatteryVoltage()); m_drivetrainSimulator.update(0.020); - m_leftEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftPosition)); - m_leftEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftVelocity)); - m_rightEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightPosition)); - m_rightEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightVelocity)); + m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters()); + m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond()); + m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters()); + m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond()); m_gyroAngleSim.set(-m_drivetrainSimulator.getHeading().getDegrees()); m_fieldSim.setRobotPose(getPose());