From b61f08d3fa1559e312fa9bf5bccbe892233d0935 Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 20 Sep 2020 09:39:52 -0700 Subject: [PATCH] [wpilib] Add physics simulation support with state-space (#2615) This includes physics simulation support for arms/elevator models, as well as differential drivetrains. Swerve might be added at a later date. Co-authored-by: Claudius Tewari Co-authored-by: Prateek Machiraju Co-authored-by: Tyler Veness --- .../simulation/DifferentialDrivetrainSim.cpp | 121 +++++++ .../native/cpp/simulation/ElevatorSim.cpp | 79 +++++ .../native/cpp/simulation/FlywheelSim.cpp | 41 +++ .../cpp/simulation/SingleJointedArmSim.cpp | 109 +++++++ .../include/frc/simulation/BatterySim.h | 55 ++++ .../simulation/DifferentialDrivetrainSim.h | 218 +++++++++++++ .../include/frc/simulation/ElevatorSim.h | 103 ++++++ .../include/frc/simulation/FlywheelSim.h | 73 +++++ .../include/frc/simulation/LinearSystemSim.h | 136 ++++++++ .../frc/simulation/SingleJointedArmSim.h | 139 ++++++++ .../native/cpp/simulation/ElevatorSimTest.cpp | 69 ++++ .../simulation/SingleJointedArmSimTest.cpp | 25 ++ .../cpp/simulation/StateSpaceSimTest.cpp | 57 ++++ .../cpp/examples/ArmSimulation/cpp/Robot.cpp | 102 ++++++ .../cpp/Drivetrain.cpp | 58 ++++ .../DifferentialDriveSimulation/cpp/Robot.cpp | 54 ++++ .../include/Drivetrain.h | 97 ++++++ .../examples/ElevatorSimulation/cpp/Robot.cpp | 109 +++++++ .../src/main/cpp/examples/examples.json | 50 +++ .../first/wpilibj/simulation/BatterySim.java | 52 +++ .../simulation/DifferentialDrivetrainSim.java | 303 ++++++++++++++++++ .../first/wpilibj/simulation/ElevatorSim.java | 121 +++++++ .../first/wpilibj/simulation/FlywheelSim.java | 79 +++++ .../wpilibj/simulation/LinearSystemSim.java | 137 ++++++++ .../simulation/SingleJointedArmSim.java | 213 ++++++++++++ .../first/wpilibj/simulation/ArmSimTest.java | 38 +++ .../DifferentialDrivetrainSimTest.java | 146 +++++++++ .../wpilibj/simulation/ElevatorSimTest.java | 82 +++++ .../wpilibj/examples/armsimulation/Main.java | 29 ++ .../wpilibj/examples/armsimulation/Robot.java | 101 ++++++ .../examples/elevatorsimulation/Main.java | 29 ++ .../examples/elevatorsimulation/Robot.java | 100 ++++++ .../wpi/first/wpilibj/examples/examples.json | 49 +++ .../Constants.java | 88 +++++ .../Main.java | 29 ++ .../Robot.java | 59 ++++ .../RobotContainer.java | 136 ++++++++ .../subsystems/DriveSubsystem.java | 257 +++++++++++++++ .../examples/statespaceelevator/Robot.java | 13 +- .../first/wpilibj/system/plant/DCMotor.java | 41 ++- .../wpilibj/system/plant/LinearSystemId.java | 4 +- .../include/frc/system/plant/LinearSystemId.h | 4 +- ...olAffinePlantInversionFeedforwardTest.java | 13 +- 43 files changed, 3787 insertions(+), 31 deletions(-) create mode 100644 wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp create mode 100644 wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp create mode 100644 wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp create mode 100644 wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp create mode 100644 wpilibc/src/main/native/include/frc/simulation/BatterySim.h create mode 100644 wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h create mode 100644 wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h create mode 100644 wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h create mode 100644 wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h create mode 100644 wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h create mode 100644 wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp create mode 100644 wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp create mode 100644 wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Drivetrain.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Robot.cpp create mode 100644 wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h create mode 100644 wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ArmSimTest.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp new file mode 100644 index 0000000000..35e2e7f58c --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -0,0 +1,121 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/simulation/DifferentialDrivetrainSim.h" + +#include + +#include "frc/system/RungeKutta.h" + +using namespace frc; +using namespace frc::sim; + +DifferentialDrivetrainSim::DifferentialDrivetrainSim( + const LinearSystem<2, 2, 2>& plant, units::meter_t trackWidth, + DCMotor driveMotor, double gearRatio, units::meter_t wheelRadius) + : m_plant(plant), + m_rb(trackWidth / 2.0), + m_wheelRadius(wheelRadius), + m_motor(driveMotor), + m_originalGearing(gearRatio), + m_currentGearing(gearRatio) { + m_x.setZero(); + m_u.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) + : DifferentialDrivetrainSim( + frc::LinearSystemId::DrivetrainVelocitySystem( + driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing), + trackWidth, driveMotor, gearing, wheelRadius) {} + +void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage, + units::volt_t rightVoltage) { + m_u << leftVoltage.to(), rightVoltage.to(); +} + +void DifferentialDrivetrainSim::SetGearing(double newGearing) { + m_currentGearing = 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); +} + +double DifferentialDrivetrainSim::GetGearing() const { + return m_currentGearing; +} + +Eigen::Matrix DifferentialDrivetrainSim::GetState() const { + return m_x; +} + +Rotation2d DifferentialDrivetrainSim::GetHeading() const { + return Rotation2d(units::radian_t(m_x(State::kHeading))); +} + +Pose2d DifferentialDrivetrainSim::GetEstimatedPosition() 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)))); +} + +units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const { + auto loadIleft = + m_motor.Current(units::radians_per_second_t(m_x(State::kLeftVelocity) * + m_currentGearing / + m_wheelRadius.to()), + units::volt_t(m_u(0))) * + wpi::sgn(m_u(0)); + + auto loadIRight = + m_motor.Current(units::radians_per_second_t(m_x(State::kRightVelocity) * + m_currentGearing / + m_wheelRadius.to()), + units::volt_t(m_u(1))) * + wpi::sgn(m_u(1)); + + return loadIleft + loadIRight; +} + +Eigen::Matrix DifferentialDrivetrainSim::Dynamics( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { + // Because G^2 can be factored out of A, we can divide by the old ratio + // squared and multiply by the new ratio squared to get a new drivetrain + // model. + Eigen::Matrix B; + B.block<2, 2>(0, 0) = m_plant.B() * m_currentGearing * m_currentGearing / + m_originalGearing / m_originalGearing; + B.block<2, 2>(2, 0).setZero(); + + // Because G can be factored out of B, we can divide by the old ratio and + // multiply by the new ratio to get a new drivetrain model. + Eigen::Matrix A; + A.block<2, 2>(0, 0) = m_plant.A() * m_currentGearing / m_originalGearing; + + A.block<2, 2>(2, 0).setIdentity(); + A.block<4, 2>(0, 2).setZero(); + + double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0; + + Eigen::Matrix xdot; + xdot(0) = v * std::cos(x(State::kHeading)); + xdot(1) = v * std::sin(x(State::kHeading)); + xdot(2) = + ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / (2.0 * m_rb)) + .to(); + xdot.block<4, 1>(3, 0) = A * x.block<4, 1>(3, 0) + B * u; + return xdot; +} diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp new file mode 100644 index 0000000000..500f5de695 --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/simulation/ElevatorSim.h" + +#include + +#include "frc/StateSpaceUtil.h" +#include "frc/system/RungeKutta.h" +#include "frc/system/plant/LinearSystemId.h" + +using namespace frc; +using namespace frc::sim; + +ElevatorSim::ElevatorSim(const DCMotor& gearbox, units::kilogram_t carriageMass, + double gearing, units::meter_t drumRadius, + units::meter_t minHeight, units::meter_t maxHeight, + bool addNoise, + const std::array& m_measurementStdDevs) + : LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass, + drumRadius, gearing), + addNoise, m_measurementStdDevs), + m_motor(gearbox), + m_drumRadius(drumRadius), + m_minHeight(minHeight), + m_maxHeight(maxHeight), + m_gearing(gearing) {} + +bool ElevatorSim::HasHitLowerLimit(const Eigen::Matrix& x) const { + return x(0) < m_minHeight.to(); +} + +bool ElevatorSim::HasHitUpperLimit(const Eigen::Matrix& x) const { + return x(0) > m_maxHeight.to(); +} + +units::meter_t ElevatorSim::GetPosition() const { return units::meter_t{Y(0)}; } + +units::meters_per_second_t ElevatorSim::GetVelocity() const { + return units::meters_per_second_t{m_x(1)}; +} + +units::ampere_t ElevatorSim::GetCurrentDraw() const { + // I = V / R - omega / (Kv * R) + // Reductions are greater than 1, so a reduction of 10:1 would mean the motor + // is spinning 10x faster than the output. + + // v = r w, so w = v / r + units::meters_per_second_t velocity{m_x(1)}; + units::radians_per_second_t motorVelocity = + velocity / m_drumRadius * m_gearing * 1_rad; + + // Perform calculation and return. + return m_motor.Current(motorVelocity, units::volt_t{m_u(0)}) * + wpi::sgn(m_u(0)); +} + +Eigen::Matrix ElevatorSim::UpdateX( + const Eigen::Matrix& currentXhat, + const Eigen::Matrix& u, units::second_t dt) { + auto updatedXhat = RungeKutta( + [&](const Eigen::Matrix& x, + const Eigen::Matrix& u_) + -> Eigen::Matrix { + return m_plant.A() * x + m_plant.B() * u_ + MakeMatrix<2, 1>(0.0, -9.8); + }, + currentXhat, u, dt); + // Check for collision after updating x-hat. + if (HasHitLowerLimit(updatedXhat)) { + return MakeMatrix<2, 1>(m_minHeight.to(), 0.0); + } else if (HasHitUpperLimit(updatedXhat)) { + return MakeMatrix<2, 1>(m_maxHeight.to(), 0.0); + } + return updatedXhat; +} diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp new file mode 100644 index 0000000000..8748667d9b --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/simulation/FlywheelSim.h" + +#include + +#include "frc/system/plant/LinearSystemId.h" + +using namespace frc; +using namespace frc::sim; + +FlywheelSim::FlywheelSim(const LinearSystem<1, 1, 1>& plant, + const DCMotor& gearbox, double gearing, bool addNoise, + const std::array& measurementStdDevs) + : LinearSystemSim<1, 1, 1>(plant, addNoise, measurementStdDevs), + m_motor(gearbox), + m_gearing(gearing) {} + +FlywheelSim::FlywheelSim(const DCMotor& gearbox, double gearing, + units::kilogram_square_meter_t moi, bool addNoise, + const std::array& measurementStdDevs) + : FlywheelSim(LinearSystemId::FlywheelSystem(gearbox, moi, gearing), + gearbox, gearing, addNoise, measurementStdDevs) {} + +units::radians_per_second_t FlywheelSim::GetAngularVelocity() const { + return units::radians_per_second_t{Y(0)}; +} + +units::ampere_t FlywheelSim::GetCurrentDraw() const { + // I = V / R - omega / (Kv * R) + // Reductions are greater than 1, so a reduction of 10:1 would mean the motor + // is spinning 10x faster than the output. + return m_motor.Current(GetAngularVelocity() * m_gearing, + units::volt_t{m_u(0)}) * + wpi::sgn(m_u(0)); +} diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp new file mode 100644 index 0000000000..156889c4ec --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -0,0 +1,109 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/simulation/SingleJointedArmSim.h" + +#include + +#include +#include + +#include "frc/system/RungeKutta.h" +#include "frc/system/plant/LinearSystemId.h" + +using namespace frc; +using namespace frc::sim; + +SingleJointedArmSim::SingleJointedArmSim( + const LinearSystem<2, 1, 1>& system, const DCMotor motor, double G, + units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle, + units::radian_t maxAngle, bool addNoise, + const std::array& measurementStdDevs) + : LinearSystemSim<2, 1, 1>(system, addNoise, measurementStdDevs), + m_r(armLength), + m_minAngle(minAngle), + m_maxAngle(maxAngle), + m_mass(mass), + m_motor(motor), + m_gearing(G) {} + +SingleJointedArmSim::SingleJointedArmSim( + const DCMotor& motor, units::kilogram_square_meter_t J, double G, + units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle, + units::radian_t maxAngle, bool addNoise, + const std::array& measurementStdDevs) + : SingleJointedArmSim(LinearSystemId::SingleJointedArmSystem(motor, J, G), + motor, G, mass, armLength, minAngle, maxAngle, + addNoise, measurementStdDevs) {} + +SingleJointedArmSim::SingleJointedArmSim( + const DCMotor& motor, double G, units::kilogram_t mass, + units::meter_t armLength, units::radian_t minAngle, + units::radian_t maxAngle, bool addNoise, + const std::array& measurementStdDevs) + : SingleJointedArmSim( + LinearSystemId::SingleJointedArmSystem( + motor, 1.0 / 3.0 * mass * armLength * armLength, G), + motor, G, mass, armLength, minAngle, maxAngle, addNoise, + measurementStdDevs) {} + +bool SingleJointedArmSim::HasHitLowerLimit( + const Eigen::Matrix& x) const { + return x(0) < m_minAngle.to(); +} + +bool SingleJointedArmSim::HasHitUpperLimit( + const Eigen::Matrix& x) const { + return x(0) > m_maxAngle.to(); +} + +units::radian_t SingleJointedArmSim::GetAngle() const { + return units::radian_t{m_x(0)}; +} + +units::radians_per_second_t SingleJointedArmSim::GetVelocity() const { + return units::radians_per_second_t{m_x(1)}; +} + +Eigen::Matrix SingleJointedArmSim::UpdateX( + const Eigen::Matrix& currentXhat, + const Eigen::Matrix& u, units::second_t dt) { + // Horizontal case: + // Torque = F * r = I * alpha + // alpha = F * r / I + // Since F = mg, + // alpha = m * g * r / I + // Finally, multiply RHS by cos(theta) to account for the arm angle + // This acceleration is added to the linear system dynamics x-dot = Ax + Bu + // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I * + // std::cos(theta)]] + + auto updatedXhat = RungeKutta( + [&](const auto& x, const auto& u) -> Eigen::Matrix { + return m_plant.A() * x + m_plant.B() * u + + MakeMatrix<2, 1>(0.0, (m_mass * m_r * -9.8 * 3.0 / + (m_mass * m_r * m_r) * std::cos(x(0))) + .template to()); + }, + currentXhat, u, dt); + + // Check for collisions. + if (HasHitLowerLimit(updatedXhat)) { + return MakeMatrix<2, 1>(m_minAngle.to(), 0.0); + } else if (HasHitUpperLimit(updatedXhat)) { + return MakeMatrix<2, 1>(m_maxAngle.to(), 0.0); + } + return updatedXhat; +} + +units::ampere_t SingleJointedArmSim::GetCurrentDraw() const { + // Reductions are greater than 1, so a reduction of 10:1 would mean the motor + // is spinning 10x faster than the output + units::radians_per_second_t motorVelocity{m_x(1) * m_gearing}; + return m_motor.Current(motorVelocity, units::volt_t{m_u(0)}) * + wpi::sgn(m_u(0)); +} diff --git a/wpilibc/src/main/native/include/frc/simulation/BatterySim.h b/wpilibc/src/main/native/include/frc/simulation/BatterySim.h new file mode 100644 index 0000000000..ef8fb5d32b --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/BatterySim.h @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include + +namespace frc::sim { + +class BatterySim { + public: + /** + * Calculate the loaded battery voltage. Use this with + * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery + * voltage, which can then be retrieved with the {@link + * RobotController#getBatteryVoltage()} method. + * + * @param nominalVoltage The nominal battery voltage. Usually 12v. + * @param resistance The forward resistance of the battery. Most batteries + * are at or below 20 milliohms. + * @param currents The currents drawn from the battery. + * @return The battery's voltage under load. + */ + static units::volt_t Calculate( + units::volt_t nominalVoltage, units::ohm_t resistance, + std::initializer_list currents) { + return nominalVoltage - + std::accumulate(currents.begin(), currents.end(), 0_A) * resistance; + } + + /** + * Calculate the loaded battery voltage. Use this with + * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery + * voltage, which can then be retrieved with the {@link + * RobotController#getBatteryVoltage()} method. This function assumes a + * nominal voltage of 12v and a resistance of 20 milliohms (0.020 ohms) + * + * @param currents The currents drawn from the battery. + * @return The battery's voltage under load. + */ + static units::volt_t Calculate( + std::initializer_list currents) { + return Calculate(12_V, 0.02_Ohm, currents); + } +}; + +} // namespace frc::sim diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h new file mode 100644 index 0000000000..8737cfa930 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h @@ -0,0 +1,218 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace frc::sim { + +class DifferentialDrivetrainSim { + public: + /** + * Create a SimDrivetrain. + * + * @param drivetrainPlant The LinearSystem representing the robot's + * drivetrain. This system can be created with + * LinearSystemId#createDrivetrainVelocitySystem or + * LinearSystemId#identifyDrivetrainSystem. + * @param trackWidth The robot's track width. + * @param driveMotor A {@link DCMotor} representing the left side of + * the drivetrain. + * @param gearingRatio The gearingRatio ratio of the left side, as output + * over input. This must be the same ratio as the ratio used to identify or + * create the drivetrainPlant. + * @param wheelRadiusMeters The radius of the wheels on the drivetrain, in + * meters. + */ + DifferentialDrivetrainSim(const LinearSystem<2, 2, 2>& plant, + units::meter_t trackWidth, DCMotor driveMotor, + double gearingRatio, units::meter_t wheelRadius); + + /** + * 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 J The moment of inertia of the drivetrain about its + * center. + * @param mass The mass of the drivebase. + * @param wheelRadius The radius of the wheels on the drivetrain. + * @param trackWidth The robot's track width, or distance between left and + * right wheels. + */ + DifferentialDrivetrainSim(frc::DCMotor driveMotor, double gearing, + units::kilogram_square_meter_t J, + units::kilogram_t mass, units::meter_t wheelRadius, + units::meter_t trackWidth); + + /** + * Set 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. + * @param rightVoltage The right voltage. + */ + void SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage); + + /** + * Set the gearing reduction on the drivetrain. This is commonly used for + * shifting drivetrains. + * + * @param newGearing The new gear ratio, as output over input. + */ + void SetGearing(double newGearing); + + /** + * Updates the simulation. + * + * @param dt The time that's passed since the last {@link #update(double)} + * call. + */ + 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; + + /** + * Get the current gearing reduction of the drivetrain, as output over input. + */ + double GetGearing() const; + + /** + * Returns the current state vector x. + */ + 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. + */ + Rotation2d GetHeading() const; + + /** + * Returns the current estimated position. + */ + Pose2d GetEstimatedPosition() const; + + /** + * Returns the currently drawn current. + */ + units::ampere_t GetCurrentDraw() const; + + Eigen::Matrix Dynamics(const Eigen::Matrix& x, + const Eigen::Matrix& u); + + 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; + }; + + /** + * Represents a gearing option of the Toughbox mini. + * 12.75:1 -- 14:50 and 14:50 + * 10.71:1 -- 14:50 and 16:48 + * 8.45:1 -- 14:50 and 19:45 + * 7.31:1 -- 14:50 and 21:43 + * 5.95:1 -- 14:50 and 24:40 + */ + class KitbotGearing { + public: + static constexpr double k12p75 = 12.75; + static constexpr double k10p71 = 10.71; + static constexpr double k8p45 = 8.45; + static constexpr double k7p31 = 7.31; + static constexpr double k5p95 = 5.95; + }; + + class KitbotMotor { + public: + static constexpr frc::DCMotor SingleCIMPerSide = frc::DCMotor::CIM(1); + static constexpr frc::DCMotor DualCIMPerSide = frc::DCMotor::CIM(2); + static constexpr frc::DCMotor SingleMiniCIMPerSide = + frc::DCMotor::MiniCIM(1); + static constexpr frc::DCMotor DualMiniCIMPerSide = frc::DCMotor::MiniCIM(2); + }; + + class KitbotWheelSize { + public: + static constexpr units::meter_t kSixInch = 6_in; + static constexpr units::meter_t kEightInch = 8_in; + static constexpr units::meter_t kTenInch = 10_in; + }; + + /** + * Create a sim for the standard FRC kitbot. + * + * @param motor The motors installed in the bot. + * @param gearing The gearing reduction used. + * @param wheelSize The wheel size. + */ + static DifferentialDrivetrainSim createKitbotSim(frc::DCMotor motor, + double gearing, + units::meter_t wheelSize) { + // 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) * + 2 // CIM plus toughbox per side + * (26_in / 2) * (26_in / 2); + + return DifferentialDrivetrainSim{ + motor, gearing, batteryMoi + gearboxMoi, 25_kg, wheelSize / 2.0, 26_in}; + } + + /** + * Create a sim for the standard FRC kitbot. + * + * @param motor The motors installed in the bot. + * @param gearing The gearing reduction used. + * @param wheelSize The wheel size. + * @param J The moment of inertia of the drivebase. This can be + * calculated using frc-characterization. + */ + 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}; + } + + private: + const LinearSystem<2, 2, 2>& m_plant; + units::meter_t m_rb; + units::meter_t m_wheelRadius; + + DCMotor m_motor; + + double m_originalGearing; + double m_currentGearing; + + Eigen::Matrix m_x; + Eigen::Matrix m_u; +}; +} // namespace frc::sim diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h new file mode 100644 index 0000000000..5c32392665 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -0,0 +1,103 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include + +#include "frc/simulation/LinearSystemSim.h" +#include "frc/system/plant/DCMotor.h" + +namespace frc::sim { +/** + * Represents a simulated elevator mechanism. + */ +class ElevatorSim : public LinearSystemSim<2, 1, 1> { + public: + /** + * Constructs a simulated elevator mechanism. + * + * @param gearbox The type of and number of motors in your elevator + * gearbox. + * @param carriageMass The mass of the elevator carriage. + * @param gearing The gearing of the elevator (numbers greater than + * 1 represent reductions). + * @param drumRadius The radius of the drum that your cable is wrapped + * around. + * @param minHeight The minimum allowed height of the elevator. + * @param maxHeight The maximum allowed height of the elevator. + * @param addNoise Whether the sim should automatically add some + * encoder noise. + * @param measurementStdDevs The standard deviation of the measurement noise. + */ + ElevatorSim(const DCMotor& gearbox, units::kilogram_t carriageMass, + double gearing, units::meter_t drumRadius, + units::meter_t minHeight, units::meter_t maxHeight, + bool addNoise = false, + const std::array& m_measurementStdDevs = {0.0}); + + /** + * Returns whether the elevator has hit the lower limit. + * + * @param x The current elevator state. + * @return Whether the elevator has hit the lower limit. + */ + bool HasHitLowerLimit(const Eigen::Matrix& x) const; + + /** + * Returns whether the elevator has hit the upper limit. + * + * @param x The current elevator state. + * @return Whether the elevator has hit the uppwer limit. + */ + bool HasHitUpperLimit(const Eigen::Matrix& x) const; + + /** + * Returns the position of the elevator. + * + * @return The position of the elevator. + */ + units::meter_t GetPosition() const; + + /** + * Returns the velocity of the elevator. + * + * @return The velocity of the elevator. + */ + units::meters_per_second_t GetVelocity() const; + + /** + * Returns the elevator current draw. + * + * @return The elevator current draw. + */ + units::ampere_t GetCurrentDraw() const override; + + protected: + /** + * Updates the state estimate of the elevator. + * + * @param currentXhat The current state estimate. + * @param u The system inputs (voltage). + * @param dt The time difference between controller updates. + */ + Eigen::Matrix UpdateX( + const Eigen::Matrix& currentXhat, + const Eigen::Matrix& u, units::second_t dt) override; + + private: + DCMotor m_motor; + units::meter_t m_drumRadius; + units::meter_t m_minHeight; + units::meter_t m_maxHeight; + double m_gearing; +}; +} // namespace frc::sim diff --git a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h new file mode 100644 index 0000000000..bc1748a38d --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "frc/simulation/LinearSystemSim.h" +#include "frc/system/LinearSystem.h" +#include "frc/system/plant/DCMotor.h" + +namespace frc::sim { +/** + * Represents a simulated flywheel mechanism. + */ +class FlywheelSim : public LinearSystemSim<1, 1, 1> { + public: + /** + * Creates a simulated flywhel mechanism. + * + * @param plant The linear system representing the flywheel. + * @param gearbox The type of and number of motors in the flywheel + * gearbox. + * @param gearing The gearing of the flywheel (numbers greater than + * 1 represent reductions). + * @param addNoise Whether the sim should automatically add some + * encoder noise. + * @param measurementStdDevs The standard deviation of the measurement noise. + */ + FlywheelSim(const LinearSystem<1, 1, 1>& plant, const DCMotor& gearbox, + double gearing, bool addNoise = false, + const std::array& measurementStdDevs = {0.0}); + + /** + * Creates a simulated flywhel mechanism. + * + * @param gearbox The type of and number of motors in the flywheel + * gearbox. + * @param gearing The gearing of the flywheel (numbers greater than + * 1 represent reductions). + * @param moi The moment of inertia of the flywheel. + * @param addNoise Whether the sim should automatically add some + * encoder noise. + * @param measurementStdDevs The standard deviation of the measurement noise. + */ + FlywheelSim(const DCMotor& gearbox, double gearing, + units::kilogram_square_meter_t moi, bool addNoise = false, + const std::array& measurementStdDevs = {0.0}); + + /** + * Returns the flywheel velocity. + * + * @return The flywheel velocity. + */ + units::radians_per_second_t GetAngularVelocity() const; + + /** + * Returns the flywheel current draw. + * + * @return The flywheel current draw. + */ + units::ampere_t GetCurrentDraw() const override; + + private: + DCMotor m_motor; + double m_gearing; +}; +} // 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 new file mode 100644 index 0000000000..9023f44f14 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h @@ -0,0 +1,136 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include + +#include "frc/StateSpaceUtil.h" +#include "frc/system/LinearSystem.h" + +namespace frc::sim { +/** + * Represents a simulated generic linear system. + */ +template +class LinearSystemSim { + public: + /** + * Creates a simulated generic linear system. + * + * @param system The system to simulate. + * @param addNoise Whether the sim should automatically add some + * measurement noise. + * @param measurementStdDevs The standard deviations of the measurement noise. + */ + LinearSystemSim(const LinearSystem& system, + bool addNoise = false, + const std::array& measurementStdDevs = {0.0}) + : m_plant(system), + m_shouldAddNoise(addNoise), + m_measurementStdDevs(measurementStdDevs) { + m_x = Eigen::Matrix::Zero(); + m_y = Eigen::Matrix::Zero(); + m_u = Eigen::Matrix::Zero(); + } + + /** + * Returns whether the sim should add noise to the measurements. + * + * @return Whether the sim should add noise to the measurements. + */ + bool ShouldAddNoise() const { return m_shouldAddNoise; } + + /** + * Returns the current output of the plant. + * + * @return The current output of the plant. + */ + const Eigen::Matrix& Y() const { return m_y; } + + /** + * Returns an element of the current output of the plant. + * + * @param row The row to return. + */ + double Y(int i) const { return m_y(i); } + + /** + * Sets whether the sim should add noise to measurements. + * + * @param shouldAddNoise Whether the sim should add noise to measurements. + */ + void SetShouldAddNoise(bool shouldAddNoise) { + m_shouldAddNoise = shouldAddNoise; + } + + /** + * Updates the linear system sim. + * + * @param dt The time between updates. + */ + void Update(units::second_t dt) { + // Update x. By default, this is the linear system dynamics x_k+1 = Ax_k + + // Bu_k + m_x = UpdateX(m_x, m_u, dt); + + // y = Cx + Du + m_y = m_plant.CalculateY(m_x, m_u); + + // Add noise if needed. + if (m_shouldAddNoise) { + m_y += frc::MakeWhiteNoiseVector(m_measurementStdDevs); + } + } + + /** + * Sets the system inputs (usually voltages). + * + * @param u The system inputs. + */ + void SetInput(const Eigen::Matrix& u) { m_u = u; } + + /** + * Resets the system state. + * + * @param state The state to reset to. + */ + void ResetState(const Eigen::Matrix& state) { + m_x = state; + } + + protected: + /** + * Updates the state estimate of the system. + * + * @param currentXhat The current state estimate. + * @param u The system inputs (usually voltage). + * @param dt The time difference between controller updates. + */ + virtual Eigen::Matrix UpdateX( + const Eigen::Matrix& currentXhat, + const Eigen::Matrix& u, units::second_t dt) { + return m_plant.CalculateX(currentXhat, u, dt); + } + + virtual units::ampere_t GetCurrentDraw() const { + return units::ampere_t(0.0); + } + + LinearSystem m_plant; + bool m_shouldAddNoise; + + Eigen::Matrix m_x; + Eigen::Matrix m_y; + Eigen::Matrix m_u; + std::array m_measurementStdDevs; +}; +} // namespace frc::sim diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h new file mode 100644 index 0000000000..51b519895f --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h @@ -0,0 +1,139 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include +#include + +#include "frc/simulation/LinearSystemSim.h" +#include "frc/system/plant/DCMotor.h" + +namespace frc::sim { +/** + * Represents a simulated arm mechanism. + */ +class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { + public: + /** + * Creates a simulated arm mechanism. + * + * @param system The system representing this arm. + * @param motor The type and number of motors on the arm gearbox. + * @param G The gear ratio of the arm (numbers greater than 1 + * represent reductions). + * @param mass The mass of the arm. + * @param armLength The length of the arm. + * @param minAngle The minimum allowed angle for the arm. + * @param maxAngle The maximum allowed angle for the arm. + * @param addNoise Whether the sim should automatically add some + * encoder noise. + * @param measurementStdDevs The standard deviation of the measurement noise. + */ + SingleJointedArmSim(const LinearSystem<2, 1, 1>& system, const DCMotor motor, + double G, units::kilogram_t mass, + units::meter_t armLength, units::radian_t minAngle, + units::radian_t maxAngle, bool addNoise, + const std::array& measurementStdDevs); + /** + * Creates a simulated arm mechanism. + * + * @param motor The type and number of motors on the arm gearbox. + * @param j The moment of inertia of the arm. + * @param G The gear ratio of the arm (numbers greater than 1 + * represent reductions). + * @param mass The mass of the arm. + * @param armLength The length of the arm. + * @param minAngle The minimum allowed angle for the arm. + * @param maxAngle The maximum allowed angle for the arm. + * @param addNoise Whether the sim should automatically add some + * encoder noise. + * @param measurementStdDevs The standard deviation of the measurement noise. + */ + SingleJointedArmSim(const DCMotor& motor, units::kilogram_square_meter_t J, + double G, units::kilogram_t mass, + units::meter_t armLength, units::radian_t minAngle, + units::radian_t maxAngle, bool addNoise, + const std::array& measurementStdDevs); + + /** + * Creates a simulated arm mechanism. + * + * @param motor The type and number of motors on the arm gearbox. + * @param G The gear ratio of the arm (numbers greater than 1 + * represent reductions). + * @param mass The mass of the arm. + * @param armLength The length of the arm. + * @param minAngle The minimum allowed angle for the arm. This is + * measured from horizontal, with straight out being 0. + * @param maxAngle The maximum allowed angle for the arm. This is + * measured from horizontal, with straight out being 0. + * @param addNoise Whether the sim should automatically add some + * encoder noise. + * @param measurementStdDevs The standard deviation of the measurement noise. + */ + SingleJointedArmSim(const DCMotor& motor, double G, units::kilogram_t mass, + units::meter_t armLength, units::radian_t minAngle, + units::radian_t maxAngle, bool addNoise, + const std::array& measurementStdDevs); + + /** + * Returns whether the arm has hit the lower limit. + * + * @param x The current arm state. + * @return Whether the arm has hit the lower limit. + */ + bool HasHitLowerLimit(const Eigen::Matrix& x) const; + + /** + * Returns whether the arm has hit the upper limit. + * + * @param x The current arm state. + * @return Whether the arm has hit the upper limit. + */ + bool HasHitUpperLimit(const Eigen::Matrix& x) const; + + /** + * Returns the current arm angle. + * + * @return The current arm angle. + */ + units::radian_t GetAngle() const; + + /** + * Returns the current arm velocity. + * + * @return The current arm velocity. + */ + units::radians_per_second_t GetVelocity() const; + + /** + * Updates the state estimate of the arm. + * + * @param currentXhat The current state estimate. + * @param u The system inputs (voltage). + * @param dt The time difference between controller updates. + */ + Eigen::Matrix UpdateX( + const Eigen::Matrix& currentXhat, + const Eigen::Matrix& u, units::second_t dt) override; + + units::ampere_t GetCurrentDraw() const override; + + private: + units::meter_t m_r; + units::radian_t m_minAngle; + units::radian_t m_maxAngle; + units::kilogram_t m_mass; + const DCMotor m_motor; + double m_gearing; +}; +} // namespace frc::sim diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp new file mode 100644 index 0000000000..9532a04b08 --- /dev/null +++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include + +#include "frc/Encoder.h" +#include "frc/PWMVictorSPX.h" +#include "frc/RobotController.h" +#include "frc/StateSpaceUtil.h" +#include "frc/controller/PIDController.h" +#include "frc/simulation/ElevatorSim.h" +#include "frc/simulation/EncoderSim.h" +#include "frc/system/plant/DCMotor.h" +#include "frc/system/plant/LinearSystemId.h" +#include "gtest/gtest.h" + +TEST(ElevatorSim, StateSpaceSim) { + frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 8_kg, 13.67, + units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 3_m, + true, {0.01}); + frc2::PIDController controller(10, 0.0, 0.0); + + frc::PWMVictorSPX motor(0); + frc::Encoder encoder(0, 1); + frc::sim::EncoderSim encoderSim(encoder); + + for (size_t i = 0; i < 100; ++i) { + controller.SetSetpoint(2.0); + auto nextVoltage = controller.Calculate(encoderSim.GetDistance()); + motor.Set(nextVoltage / frc::RobotController::GetInputVoltage()); + + auto u = frc::MakeMatrix<1, 1>(motor.Get() * + frc::RobotController::GetInputVoltage()); + sim.SetInput(u); + sim.Update(20_ms); + + const auto& y = sim.Y(); + encoderSim.SetDistance(y(0)); + } + + EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().to(), 0.2); +} + +TEST(ElevatorSim, MinMax) { + frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 8_kg, 13.67, + units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 1_m, + true, {0.01}); + for (size_t i = 0; i < 100; ++i) { + sim.SetInput(frc::MakeMatrix<1, 1>(0.0)); + sim.Update(20_ms); + + auto height = sim.GetPosition(); + EXPECT_TRUE(height > -0.05_m); + } + + for (size_t i = 0; i < 100; ++i) { + sim.SetInput(frc::MakeMatrix<1, 1>(12.0)); + sim.Update(20_ms); + + auto height = sim.GetPosition(); + EXPECT_TRUE(height < 1.05_m); + } +} diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp new file mode 100644 index 0000000000..0434f45e39 --- /dev/null +++ b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "frc/simulation/SingleJointedArmSim.h" +#include "gtest/gtest.h" + +TEST(SingleJointedArmTest, Disabled) { + frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 10_kg, + 9.5_in, -180_deg, 0_deg, false, {0.0}); + sim.ResetState(frc::MakeMatrix<2, 1>(0.0, 0.0)); + + for (size_t i = 0; i < 12 / 0.02; ++i) { + sim.SetInput(frc::MakeMatrix<1, 1>(0.0)); + sim.Update(20_ms); + } + + // The arm should swing down. + EXPECT_NEAR(sim.GetAngle().to(), -wpi::math::pi / 2, 0.01); +} diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp new file mode 100644 index 0000000000..a9061b953c --- /dev/null +++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include +#include + +#include "frc/Encoder.h" +#include "frc/PWMVictorSPX.h" +#include "frc/RobotController.h" +#include "frc/controller/PIDController.h" +#include "frc/controller/SimpleMotorFeedforward.h" +#include "frc/simulation/BatterySim.h" +#include "frc/simulation/DifferentialDrivetrainSim.h" +#include "frc/simulation/ElevatorSim.h" +#include "frc/simulation/EncoderSim.h" +#include "frc/simulation/FlywheelSim.h" +#include "frc/simulation/LinearSystemSim.h" +#include "frc/simulation/PWMSim.h" +#include "frc/simulation/RoboRioSim.h" +#include "frc/simulation/SingleJointedArmSim.h" +#include "frc/system/plant/LinearSystemId.h" +#include "gtest/gtest.h" + +TEST(StateSpaceSimTest, TestFlywheelSim) { + const frc::LinearSystem<1, 1, 1> plant = + frc::LinearSystemId::IdentifyVelocitySystem(0.02, 0.01); + frc::sim::FlywheelSim sim{plant, frc::DCMotor::NEO(2), 1.0}; + frc2::PIDController controller{0.2, 0.0, 0.0}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; + frc::Encoder encoder{0, 1}; + frc::sim::EncoderSim encoderSim{encoder}; + frc::PWMVictorSPX motor{0}; + + for (int i = 0; i < 100; i++) { + // RobotPeriodic runs first + auto voltageOut = controller.Calculate(encoder.GetRate(), 200.0); + motor.SetVoltage(units::volt_t(voltageOut) + + feedforward.Calculate(200_rad_per_s)); + + // Then, SimulationPeriodic runs + frc::sim::RoboRioSim::SetVInVoltage( + frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}).to()); + sim.SetInput(frc::MakeMatrix<1, 1>( + motor.Get() * frc::RobotController::GetInputVoltage())); + sim.Update(20_ms); + encoderSim.SetRate(sim.GetAngularVelocity().to()); + } + + ASSERT_TRUE(std::abs(200 - encoder.GetRate()) < 0.1); +} diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp new file mode 100644 index 0000000000..cba1260b2e --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp @@ -0,0 +1,102 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * This is a sample program to demonstrate how to use a state-space controller + * to control an arm. + */ +class Robot : public frc::TimedRobot { + static constexpr int kMotorPort = 0; + static constexpr int kEncoderAChannel = 0; + static constexpr int kEncoderBChannel = 1; + static constexpr int kJoystickPort = 0; + + // The P gain for the PID controller that drives this arm. + static constexpr double kArmKp = 5.0; + + // distance per pulse = (angle per revolution) / (pulses per revolution) + // = (2 * PI rads) / (4096 pulses) + static constexpr double kArmEncoderDistPerPulse = + 2.0 * wpi::math::pi / 4096.0; + + // The arm gearbox represents a gerbox containing two Vex 775pro motors. + frc::DCMotor m_armGearbox = frc::DCMotor::Vex775Pro(2); + + // Standard classes for controlling our arm + frc2::PIDController m_controller{kArmKp, 0, 0}; + frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; + frc::PWMVictorSPX m_motor{kMotorPort}; + frc::Joystick m_joystick{kJoystickPort}; + + // Simulation classes help us simulate what's going on, including gravity. + // This sim represents an arm with 2 775s, a 100:1 reduction, a mass of 5kg, + // 30in overall arm length, range of motion nin [-180, 0] degrees, and noise + // with a standard deviation of 0.5 degrees. + frc::sim::SingleJointedArmSim m_armSim{ + m_armGearbox, 100.0, 5_kg, 30_in, + -180_deg, 0_deg, true, {(0.5_deg).to()}}; + frc::sim::EncoderSim m_encoderSim{m_encoder}; + + public: + void RobotInit() { m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse); } + + void SimulationPeriodic() { + // In this method, we update our simulation of what our arm is doing + // First, we set our "inputs" (voltages) + m_armSim.SetInput(frc::MakeMatrix<1, 1>( + m_motor.Get() * frc::RobotController::GetInputVoltage())); + + // Next, we update it. The standard loop time is 20ms. + m_armSim.Update(20_ms); + + // Finally, we set our simulated encoder's readings and simulated battery + // voltage + m_encoderSim.SetDistance(m_armSim.GetAngle().to()); + // SimBattery estimates loaded battery voltages + frc::sim::RoboRioSim::SetVInVoltage( + frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()}) + .to()); + } + + void TeleopPeriodic() { + if (m_joystick.GetTrigger()) { + // Here, we run PID control like normal, with a constant setpoint of 30in. + double pidOutput = + m_controller.Calculate(m_encoder.GetDistance(), (30_in).to()); + m_motor.SetVoltage(units::volt_t(pidOutput)); + } else { + // Otherwise, we disable the motor. + m_motor.Set(0.0); + } + } + + void DisabledInit() { + // This just makes sure that our simulation code knows that the motor's off. + m_motor.Set(0.0); + } +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Drivetrain.cpp new file mode 100644 index 0000000000..712bc16683 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Drivetrain.cpp @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Drivetrain.h" + +#include + +void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) { + auto leftFeedforward = m_feedforward.Calculate(speeds.left); + auto rightFeedforward = m_feedforward.Calculate(speeds.right); + double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(), + speeds.left.to()); + double rightOutput = m_rightPIDController.Calculate( + m_rightEncoder.GetRate(), speeds.right.to()); + + m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward); + m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward); +} + +void Drivetrain::Drive(units::meters_per_second_t xSpeed, + units::radians_per_second_t rot) { + SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot})); +} + +void Drivetrain::UpdateOdometry() { + m_odometry.Update(m_gyro.GetRotation2d(), + units::meter_t(m_leftEncoder.GetDistance()), + units::meter_t(m_rightEncoder.GetDistance())); +} + +void Drivetrain::SimulationPeriodic() { + // To update our simulation, we set motor voltage inputs, update the + // simulation, and write the simulated positions and velocities to our + // simulated encoder and gyro. We negate the right side so that positive + // voltages make the right side move forward. + m_drivetrainSimulator.SetInputs(units::volt_t{m_leftLeader.Get()} * + frc::RobotController::GetInputVoltage(), + units::volt_t{-m_rightLeader.Get()} * + 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_gyroSim.SetAngle( + -m_drivetrainSimulator.GetHeading().Degrees().to()); + + m_fieldSim.SetRobotPose(m_odometry.GetPose()); +} diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Robot.cpp new file mode 100644 index 0000000000..195e1cdde9 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Robot.cpp @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include + +#include "Drivetrain.h" + +class Robot : public frc::TimedRobot { + public: + void AutonomousPeriodic() override { + TeleopPeriodic(); + m_drive.UpdateOdometry(); + } + + void TeleopPeriodic() override { + // Get the x speed. We are inverting this because Xbox controllers return + // negative values when we push forward. + const auto xSpeed = -m_speedLimiter.Calculate( + m_controller.GetY(frc::GenericHID::kLeftHand)) * + Drivetrain::kMaxSpeed; + + // Get the rate of angular rotation. We are inverting this because we want a + // positive value when we pull to the left (remember, CCW is positive in + // mathematics). Xbox controllers return positive values when you pull to + // the right by default. + auto rot = -m_rotLimiter.Calculate( + m_controller.GetX(frc::GenericHID::kRightHand)) * + Drivetrain::kMaxAngularSpeed; + + m_drive.Drive(xSpeed, rot); + } + + void SimulationPeriodic() override { m_drive.SimulationPeriodic(); } + + private: + frc::XboxController m_controller{0}; + + // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 + // to 1. + frc::SlewRateLimiter m_speedLimiter{3 / 1_s}; + frc::SlewRateLimiter m_rotLimiter{3 / 1_s}; + + Drivetrain m_drive; +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h new file mode 100644 index 0000000000..6d29a27ea7 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h @@ -0,0 +1,97 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Represents a differential drive style drivetrain. + */ +class Drivetrain { + public: + Drivetrain() { + m_gyro.Reset(); + // Set the distance per pulse for the drive encoders. We can simply use the + // distance traveled for one rotation of the wheel divided by the encoder + // resolution. + m_leftEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + kEncoderResolution); + m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + kEncoderResolution); + + m_leftEncoder.Reset(); + m_rightEncoder.Reset(); + } + + static constexpr units::meters_per_second_t kMaxSpeed = + 3.0_mps; // 3 meters per second + static constexpr units::radians_per_second_t kMaxAngularSpeed{ + wpi::math::pi}; // 1/2 rotation per second + + void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); + void Drive(units::meters_per_second_t xSpeed, + units::radians_per_second_t rot); + void UpdateOdometry(); + + void SimulationPeriodic(); + + private: + static constexpr units::meter_t kTrackWidth = 0.381_m * 2; + static constexpr double kWheelRadius = 0.0508; // meters + static constexpr int kEncoderResolution = 4096; + + frc::PWMVictorSPX m_leftLeader{1}; + frc::PWMVictorSPX m_leftFollower{2}; + frc::PWMVictorSPX m_rightLeader{3}; + frc::PWMVictorSPX m_rightFollower{4}; + + frc::SpeedControllerGroup m_leftGroup{m_leftLeader, m_leftFollower}; + frc::SpeedControllerGroup m_rightGroup{m_rightLeader, m_rightFollower}; + + frc::Encoder m_leftEncoder{0, 1}; + frc::Encoder m_rightEncoder{2, 3}; + + frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0}; + frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0}; + + frc::AnalogGyro m_gyro{0}; + + frc::DifferentialDriveKinematics m_kinematics{kTrackWidth}; + frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()}; + + // Gains are for example purposes only - must be determined for your own + // robot! + frc::SimpleMotorFeedforward m_feedforward{1_V, 3_V / 1_mps}; + + // Simulation classes help us simulate our robot + frc::sim::AnalogGyroSim m_gyroSim{m_gyro}; + frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder}; + frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; + frc::Field2d m_fieldSim{}; + frc::LinearSystem<2, 2, 2> m_drivetrainSystem = + frc::LinearSystemId::IdentifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ + m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in}; +}; diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp new file mode 100644 index 0000000000..9c36ee8817 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp @@ -0,0 +1,109 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * This is a sample program to demonstrate how to use a state-space controller + * to control an arm. + */ +class Robot : public frc::TimedRobot { + static constexpr int kMotorPort = 0; + static constexpr int kEncoderAChannel = 0; + static constexpr int kEncoderBChannel = 1; + static constexpr int kJoystickPort = 0; + + static constexpr double kElevatorKp = 5.0; + static constexpr double kElevatorGearing = 10.0; + static constexpr units::meter_t kElevatorDrumRadius = 2_in; + static constexpr units::kilogram_t kCarriageMass = 4.0_kg; + + static constexpr units::meter_t kMinElevatorHeight = 0_in; + static constexpr units::meter_t kMaxElevatorHeight = 50_in; + + // distance per pulse = (distance per revolution) / (pulses per revolution) + // = (Pi * D) / ppr + static constexpr double kArmEncoderDistPerPulse = + 2.0 * wpi::math::pi * kElevatorDrumRadius.to() / 4096.0; + + // This gearbox represents a gearbox containing 4 Vex 775pro motors. + frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4); + + // Standard classes for controlling our elevator + frc2::PIDController m_controller{kElevatorKp, 0, 0}; + frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; + frc::PWMVictorSPX m_motor{kMotorPort}; + frc::Joystick m_joystick{kJoystickPort}; + + // Simulation classes help us simulate what's going on, including gravity. + frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox, + kCarriageMass, + kElevatorGearing, + kElevatorDrumRadius, + kMinElevatorHeight, + kMaxElevatorHeight, + true, + {0.01}}; + frc::sim::EncoderSim m_encoderSim{m_encoder}; + + public: + void RobotInit() { m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse); } + + void SimulationPeriodic() { + // In this method, we update our simulation of what our elevator is doing + // First, we set our "inputs" (voltages) + m_elevatorSim.SetInput(frc::MakeMatrix<1, 1>( + m_motor.Get() * frc::RobotController::GetInputVoltage())); + + // Next, we update it. The standard loop time is 20ms. + m_elevatorSim.Update(20_ms); + + // Finally, we set our simulated encoder's readings and simulated battery + // voltage + m_encoderSim.SetDistance(m_elevatorSim.GetPosition().to()); + // SimBattery estimates loaded battery voltages + frc::sim::RoboRioSim::SetVInVoltage( + frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}) + .to()); + } + + void TeleopPeriodic() { + if (m_joystick.GetTrigger()) { + // Here, we run PID control like normal, with a constant setpoint of 30in. + double pidOutput = + m_controller.Calculate(m_encoder.GetDistance(), (30_in).to()); + m_motor.SetVoltage(units::volt_t(pidOutput)); + } else { + // Otherwise, we disable the motor. + m_motor.Set(0.0); + } + } + + void DisabledInit() { + // This just makes sure that our simulation code knows that the motor's off. + m_motor.Set(0.0); + } +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index 49eb6f0e7a..101bed0972 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -611,5 +611,55 @@ "gradlebase": "cpp", "mainclass": "Main", "commandversion": 2 + }, + { + "name": "ElevatorSimulation", + "description": "Demonstrates the use of physics simulation with a simple elevator.", + "tags": [ + "Elevator", + "State space", + "Digital", + "Sensors", + "Simulation", + "Physics" + ], + "foldername": "ElevatorSimulation", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "ArmSimulation", + "description": "Demonstrates the use of physics simulation with a simple single-jointed arm.", + "tags": [ + "Arm", + "State space", + "Digital", + "Sensors", + "Simulation", + "Physics" + ], + "foldername": "ArmSimulation", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "DifferentialDriveSimulation", + "description": "Demonstrates the use of physics simulation with a differential drivetrain and the Field2d class.", + "tags": [ + "Differential Drive", + "State space", + "Digital", + "Sensors", + "Simulation", + "Physics", + "Drivetrain", + "Field2d" + ], + "foldername": "DifferentialDriveSimulation", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 } ] diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java new file mode 100644 index 0000000000..0150ed4fa9 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.wpilibj.RobotController; + +public final class BatterySim { + private BatterySim() { + // Utility class + } + + /** + * Calculate the loaded battery voltage. Use this with + * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery + * voltage, which can then be retrieved with the {@link RobotController#getBatteryVoltage()} + * method. + * + * @param nominalVoltage The nominal battery voltage. Usually 12v. + * @param resistanceOhms The forward resistance of the battery. Most batteries are at or + * below 20 milliohms. + * @param currents The currents drawn from the battery. + * @return The battery's voltage under load. + */ + public static double calculateLoadedBatteryVoltage(double nominalVoltage, + double resistanceOhms, + double... currents) { + double retval = nominalVoltage; + for (var current : currents) { + retval -= current * resistanceOhms; + } + return retval; + } + + /** + * Calculate the loaded battery voltage. Use this with + * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery + * voltage, which can then be retrieved with the {@link RobotController#getBatteryVoltage()} + * method. This function assumes a nominal voltage of 12v and a resistance of 20 milliohms + * (0.020 ohms) + * + * @param currents The currents drawn from the battery. + * @return The battery's voltage under load. + */ + public static double calculateDefaultBatteryLoadedVoltage(double... currents) { + return calculateLoadedBatteryVoltage(12, 0.02, currents); + } +} 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 new file mode 100644 index 0000000000..6791fd2e73 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java @@ -0,0 +1,303 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +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.system.LinearSystem; +import edu.wpi.first.wpilibj.system.RungeKutta; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; +import edu.wpi.first.wpiutil.math.numbers.N7; + +/** + * This class simulates the state of the drivetrain. In simulationPeriodic, users should first set inputs from motors with + * {@link #setInputs(double, double)}, call {@link #update(double)} to update the simulation, + * and set estimated encoder and gyro positions, as well as estimated odometry pose. Teams can use {@link edu.wpi.first.wpilibj.simulation.Field2d} to + * visualize their robot on the Sim GUI's field. + * + *

Our state-space system is: + * + *

x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r, voltError_l, voltError_r, headingError]]^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. + * + */ +public class DifferentialDrivetrainSim { + private final DCMotor m_motor; + private final double m_originalGearing; + private double m_currentGearing; + private final double m_wheelRadiusMeters; + @SuppressWarnings("MemberName") + private Matrix m_u; + @SuppressWarnings("MemberName") + private Matrix m_x; + + private final double m_rb; + private final LinearSystem m_plant; + + /** + * 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. + */ + public DifferentialDrivetrainSim(DCMotor driveMotor, double gearing, + double jKgMetersSquared, double massKg, + double wheelRadiusMeters, double trackWidthMeters) { + this(LinearSystemId.createDrivetrainVelocitySystem(driveMotor, massKg, wheelRadiusMeters, + trackWidthMeters / 2.0, jKgMetersSquared, gearing), + driveMotor, gearing, trackWidthMeters, wheelRadiusMeters); + } + + /** + * Create a SimDrivetrain + * . + * @param drivetrainPlant The {@link LinearSystem} representing the robot's drivetrain. This + * system can be created with {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor, double, double, double, double, double)} + * or {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double, double, double)}. + * @param driveMotor A {@link DCMotor} representing the drivetrain. + * @param gearing The gearingRatio ratio of the robot, as output over input. + * This must be the same ratio as the ratio used to identify or + * create the drivetrainPlant. + * @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. + */ + public DifferentialDrivetrainSim(LinearSystem drivetrainPlant, + DCMotor driveMotor, double gearing, + double trackWidthMeters, + double wheelRadiusMeters) { + this.m_plant = drivetrainPlant; + this.m_rb = trackWidthMeters / 2.0; + this.m_motor = driveMotor; + this.m_originalGearing = gearing; + m_wheelRadiusMeters = wheelRadiusMeters; + m_currentGearing = m_originalGearing; + + m_x = new Matrix<>(Nat.N7(), Nat.N1()); + } + + /** + * Set 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. + * @param rightVoltageVolts The right voltage. + */ + public void setInputs(double leftVoltageVolts, double rightVoltageVolts) { + m_u = VecBuilder.fill(leftVoltageVolts, rightVoltageVolts); + } + + @SuppressWarnings("LocalVariableName") + public void update(double dtSeconds) { + + // 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); + } + + /** + * Get 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. + */ + public Rotation2d getHeading() { + return new Rotation2d(getState(State.kHeading)); + } + + /** + * Get the estimated position of the drivetrain. + */ + public Pose2d getEstimatedPosition() { + return new Pose2d(m_x.get(0, 0), + m_x.get(1, 0), + new Rotation2d(m_x.get(2, 0))); + } + + public double getCurrentDrawAmps() { + var loadIleft = m_motor.getCurrent( + getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadiusMeters, + m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); + + var loadIright = m_motor.getCurrent( + getState(State.kRightVelocity) * m_currentGearing / m_wheelRadiusMeters, + m_u.get(1, 0)) * Math.signum(m_u.get(1, 0)); + + return loadIleft + loadIright; + } + + public double getCurrentGearing() { + return m_currentGearing; + } + + /** + * Set the gearing reduction on the drivetrain. This is commonly used for + * shifting drivetrains. + * + * @param newGearRatio The new gear ratio, as output over input. + */ + public void setCurrentGearing(double newGearRatio) { + this.m_currentGearing = newGearRatio; + } + + @SuppressWarnings({"DuplicatedCode", "LocalVariableName"}) + protected Matrix getDynamics(Matrix x, Matrix u) { + + // Because G can be factored out of B, we can divide by the old ratio and multiply + // by the new ratio to get a new drivetrain model. + var B = new Matrix<>(Nat.N4(), Nat.N2()); + B.assignBlock(0, 0, m_plant.getB().times(this.m_currentGearing / this.m_originalGearing)); + + // Because G^2 can be factored out of A, we can divide by the old ratio squared and multiply + // by the new ratio squared to get a new drivetrain model. + var A = new Matrix<>(Nat.N4(), Nat.N4()); + A.assignBlock(0, 0, m_plant.getA().times((this.m_currentGearing * this.m_currentGearing) + / (this.m_originalGearing * this.m_originalGearing))); + + 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 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(2, 0, (x.get(State.kRightVelocity.value, 0) + - x.get(State.kLeftVelocity.value, 0)) / (2.0 * m_rb)); + xdot.assignBlock(3, 0, + A.times(x.block(Nat.N4(), Nat.N1(), 3, 0)) + .plus(B.times(u))); + + return xdot; + } + + public enum State { + kX(0), + kY(1), + kHeading(2), + kLeftVelocity(3), + kRightVelocity(4), + kLeftPosition(5), + kRightPosition(6); + + @SuppressWarnings("MemberName") + public final int value; + + State(int i) { + this.value = i; + } + } + + /** + * Represents a gearing option of the Toughbox mini. + * 12.75:1 -- 14:50 and 14:50 + * 10.71:1 -- 14:50 and 16:48 + * 8.45:1 -- 14:50 and 19:45 + * 7.31:1 -- 14:50 and 21:43 + * 5.95:1 -- 14:50 and 24:40 + */ + public enum KitbotGearing { + k12p75(12.75), + k10p71(10.71), + k8p45(8.45), + k7p31(7.31), + k5p95(5.95); + + @SuppressWarnings("MemberName") + public final double value; + + KitbotGearing(double i) { + this.value = i; + } + } + + public enum KitbotMotor { + kSingleCIMPerSide(DCMotor.getCIM(1)), + kDualCIMPerSide(DCMotor.getCIM(2)), + kSingleMiniCIMPerSide(DCMotor.getMiniCIM(1)), + kDualMiniCIMPerSide(DCMotor.getMiniCIM(2)); + + @SuppressWarnings("MemberName") + public final DCMotor value; + + KitbotMotor(DCMotor i) { + this.value = i; + } + } + + public enum KitbotWheelSize { + SixInch(Units.inchesToMeters(6)), + EightInch(Units.inchesToMeters(8)), + TenInch(Units.inchesToMeters(10)); + + @SuppressWarnings("MemberName") + public final double value; + + KitbotWheelSize(double i) { + this.value = i; + } + } + + /** + * Create a sim for the standard FRC kitbot. + * + * @param motor The motors installed in the bot. + * @param gearing The gearing reduction used. + * @param wheelSize The wheel size. + */ + public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing, + KitbotWheelSize wheelSize) { + // 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); + } + + /** + * Create a sim for the standard FRC kitbot. + * + * @param motor The motors installed in the bot. + * @param gearing The gearing reduction used. + * @param wheelSize The wheel size. + * @param jKgMetersSquared The moment of inertia of the drivebase. This can be calculated using + * frc-characterization. + */ + public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing, + KitbotWheelSize wheelSize, double jKgMetersSquared) { + return new DifferentialDrivetrainSim(motor.value, gearing.value, jKgMetersSquared, 25 / 2.2, + wheelSize.value / 2.0, Units.inchesToMeters(26)); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java new file mode 100644 index 0000000000..fa16a302d9 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -0,0 +1,121 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.RungeKutta; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; + +public class ElevatorSim extends LinearSystemSim { + + private final DCMotor m_motor; + private final double m_drumRadiusMeters; + private final double m_minHeightMeters; + private final double m_maxHeightMeters; + private final double m_gearing; + + /** + * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate + * * the state of the system. In simulationPeriodic, users should first set inputs from motors, update + * * the simulation and write simulated outputs to sensors. + * + * @param elevatorGearbox The {@link DCMotor} representing the elevator motor. + * @param carriageMassKg The mass of the carriage. + * @param drumRadiusMeters The radius of the drum driving the elevator. + * @param gearingReduction The reduction between motor and drum, as output over input. + * In most cases this is greater than one. + * @param m_measurementStdDevs Standard deviations for position noise. Can be null + * if no added noise is desired. + */ + public ElevatorSim(DCMotor elevatorGearbox, double carriageMassKg, + double drumRadiusMeters, double gearingReduction, + double minHeightMeters, double maxHeightMeters, + Matrix m_measurementStdDevs) { + super(LinearSystemId.createElevatorSystem(elevatorGearbox, carriageMassKg, drumRadiusMeters, + gearingReduction), m_measurementStdDevs); + this.m_motor = elevatorGearbox; + this.m_gearing = gearingReduction; + this.m_drumRadiusMeters = drumRadiusMeters; + this.m_minHeightMeters = minHeightMeters; + this.m_maxHeightMeters = maxHeightMeters; + } + + /** + * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate + * the state of the system. In simulationPeriodic, users should first set inputs from motors, update + * the simulation and write simulated outputs to sensors. + * + * @param elevatorPlant The elevator system being controlled. This system can be created + * with {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, double, double)} + * or {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#identifyPositionSystem(double, double)}. + * @param m_measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false. + */ + public ElevatorSim(LinearSystem elevatorPlant, + Matrix m_measurementStdDevs, DCMotor elevatorGearbox, + double gearingReduction, double drumRadiusMeters, + double minHeightMeters, double maxHeightMeters) { + super(elevatorPlant, m_measurementStdDevs); + this.m_motor = elevatorGearbox; + this.m_gearing = gearingReduction; + this.m_drumRadiusMeters = drumRadiusMeters; + this.m_minHeightMeters = minHeightMeters; + this.m_maxHeightMeters = maxHeightMeters; + } + + public boolean hasHitLowerLimit(Matrix x) { + return x.get(0, 0) < this.m_minHeightMeters; + } + + public boolean hasHitUpperLimit(Matrix x) { + return x.get(0, 0) > this.m_maxHeightMeters; + } + + @Override + protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { + var updatedXhat = RungeKutta.rungeKutta( + (x, u_) -> (m_plant.getA().times(x)).plus(m_plant.getB().times(u_)).plus(VecBuilder.fill(0, -9.8)), + currentXhat, u, dtSeconds); + + // We check for collision after updating xhat + if (hasHitLowerLimit(updatedXhat)) { + return VecBuilder.fill(m_minHeightMeters, 0); + } else if (hasHitUpperLimit(updatedXhat)) { + return VecBuilder.fill(m_maxHeightMeters, 0); + } + + return updatedXhat; + } + + public void setInputVoltage(double volts) { + setInput(volts); + } + + public double getPositionMeters() { + return getOutput(0); + } + + public double getVelocityMetersPerSecond() { + return m_x.get(0, 1); + } + + @Override + public double getCurrentDrawAmps() { + // I = V / R - omega / (Kv * R) + // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is + // spinning 10x faster than the output + // v = r w, so w = v/r + double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadiusMeters * m_gearing; + var appliedVoltage = getInput(0); + return m_motor.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java new file mode 100644 index 0000000000..f3ffeff801 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.numbers.N1; + +public class FlywheelSim extends LinearSystemSim { + + private final DCMotor m_motor; + private final double m_gearing; + + /** + * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate + * the state of the system. In simulationPeriodic, users should first set inputs from motors, update + * the simulation and write simulated outputs to sensors. + * + * @param flywheelPlant The flywheel system being controlled. This system can be created + * using {@link LinearSystemId#createFlywheelSystem(DCMotor, double, double)} + * or {@link LinearSystemId#identifyVelocitySystem(double, double)} + * @param flywheelGearbox The DCMotor representing the motor driving the flywheel. + * @param gearing The reduction between motor and drum, as output over input. + * In most cases this is greater than one. + * @param measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false. + */ + public FlywheelSim(LinearSystem flywheelPlant, DCMotor flywheelGearbox, + double gearing, Matrix measurementStdDevs) { + super(flywheelPlant, measurementStdDevs); + this.m_motor = flywheelGearbox; + this.m_gearing = gearing; + } + + /** + * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate + * the state of the system. In simulationPeriodic, users should first set inputs from motors, update + * the simulation and write simulated outputs to sensors. + * + * @param flywheelGearbox The DCMotor representing the motor driving the flywheel. + * @param gearing The reduction between motor and drum, as output over input. + * In most cases this is greater than one. + * @param jKgMetersSquared The moment of inertia J of the flywheel. This can be + * calculated with CAD. If J is unknown, {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)} + * using FRC-Characterization's kV and kA may be better. + * @param measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false. + */ + public FlywheelSim(DCMotor flywheelGearbox, double gearing, + double jKgMetersSquared, Matrix measurementStdDevs) { + super(LinearSystemId.createFlywheelSystem(flywheelGearbox, jKgMetersSquared, gearing), + measurementStdDevs); + this.m_motor = flywheelGearbox; + this.m_gearing = gearing; + } + + public double getAngularVelocityRadPerSec() { + return getOutput(0); + } + + public double getAngularVelocityRPM() { + return Units.radiansPerSecondToRotationsPerMinute(getOutput(0)); + } + + @Override + public double getCurrentDrawAmps() { + // I = V / R - omega / (Kv * R) + // Reductions are output over input, so a reduction of 2:1 means the motor is spinning + // 2x faster than the flywheel + return m_motor.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0)) + * Math.signum(m_u.get(0, 0)); + } +} 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 new file mode 100644 index 0000000000..315c530483 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java @@ -0,0 +1,137 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; +import org.ejml.MatrixDimensionException; +import org.ejml.simple.SimpleMatrix; + +/** + * This class helps simulate linear systems. To use this class, do the following in the + * {@link edu.wpi.first.wpilibj.IterativeRobotBase#simulationPeriodic} method. + * + *

Call {@link #setInput(double...)} with the inputs to the system (usually voltage). + * + *

Call {@link #update} to update the simulation. + * + *

Set simulated sensor readings with the simulated positions in {@link #getOutput(int)} + * + * @param The number of states of the system. + * @param The number of inputs to the system. + * @param The number of outputs of the system. + */ +@SuppressWarnings("ClassTypeParameterName") +public class LinearSystemSim { + + protected final LinearSystem m_plant; + + @SuppressWarnings("MemberName") + protected Matrix m_x; + @SuppressWarnings("MemberName") + protected Matrix m_y; + @SuppressWarnings("MemberName") + protected Matrix m_u; + protected final Matrix m_measurementStdDevs; + + /** + * Create a SimLinearSystem. This simulator uses an {@link LinearSystem} to simulate + * the state of the system. In simulationPeriodic, users should first set inputs from motors, update + * the simulation and write simulated outputs to sensors. + * + * @param system The system being controlled. + * @param measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false. + */ + public LinearSystemSim(LinearSystem system, Matrix measurementStdDevs) { + this.m_plant = system; + this.m_measurementStdDevs = measurementStdDevs; + + m_x = new Matrix<>(new SimpleMatrix(system.getA().getNumRows(), 1)); + m_u = new Matrix<>(new SimpleMatrix(system.getB().getNumCols(), 1)); + m_y = new Matrix<>(new SimpleMatrix(system.getC().getNumRows(), 1)); + } + + protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { + return m_plant.calculateX(currentXhat, u, dtSeconds); + } + + /** + * Updates the simulation. + * + * @param dtSeconds The time that's passed since the last {@link #update(double)} call. + */ + @SuppressWarnings("LocalVariableName") + public void update(double dtSeconds) { + // Update X. By default, this is the linear system dynamics X = Ax + Bu + m_x = updateX(m_x, m_u, dtSeconds); + + // y = cx + du + m_y = m_plant.calculateY(m_x, m_u); + if (m_measurementStdDevs != null) { + m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs)); + } + } + + public Matrix getY() { + return m_y; + } + + public double getY(int row) { + return m_y.get(row, 0); + } + + public void setInput(Matrix u) { + this.m_u = u; + } + + public void setInput(int row, double value) { + m_u.set(row, 0, value); + } + + public void setInput(double... u) { + if (u.length != m_u.getNumRows()) { + throw new MatrixDimensionException("Malformed input! Got " + u.length + + " elements instead of " + m_u.getNumRows()); + } + m_u = new Matrix<>(new SimpleMatrix(m_u.getNumRows(), 1, true, u)); + } + + public double getInput(int row) { + return m_u.get(row, 0); + } + + public Matrix getInput() { + return m_u; + } + + public Matrix getOutput() { + return m_y; + } + + public double getOutput(int row) { + return m_y.get(row, 0); + } + + public void resetState(Matrix state) { + m_x = state; + } + + /** + * Get the current drawn by this simulated system. Override this method to add current + * calculation. + * + * @return The currently drawn current. + */ + public double getCurrentDrawAmps() { + return 0.0; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java new file mode 100644 index 0000000000..7a371b89bb --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java @@ -0,0 +1,213 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.RungeKutta; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; + +public class SingleJointedArmSim extends LinearSystemSim { + + @SuppressWarnings("MemberName") + private final double m_r; + private final double m_maxAngleRads; + private final double m_minAngleRads; + private final double m_armMass; + private final DCMotor m_motor; + private final double m_gearing; + private final boolean m_simulateGravity; + + /** + * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate + * the state of the system. In simulationPeriodic, users should first set inputs from motors, update + * the simulation and write simulated outputs to sensors. + * + * @param armSystem The arm system being controlled. + * @param motor DCMotor representing the motor driving the arm. + * @param G The gear ratio of the arm (numbers greater than 1 + * represent reductions). + * @param armMassKg The mass of the arm. + * @param armLengthMeters The distance from the pivot of the arm to its center of mass. + * This number is not the same as the overall length of the arm. + * @param minAngleRads The min angle the arm can reach, with 0 being measured from + * horizontal. The simulation will not allow motion past this. + * @param maxAngleRads The max angle the arm can reach, with 0 being measured from + * horizontal. The simulation will not allow motion past this. + * @param posMeasurementStdDev Standard deviations of noise in the arm's position measurement. + * @param simulateGravity If the affects of gravity should be simulated. + */ + public SingleJointedArmSim(LinearSystem armSystem, + DCMotor motor, double G, + double armMassKg, + double armLengthMeters, double minAngleRads, double maxAngleRads, + double posMeasurementStdDev, boolean simulateGravity) { + super(armSystem, VecBuilder.fill(posMeasurementStdDev)); + this.m_armMass = armMassKg; + this.m_r = armLengthMeters; + this.m_minAngleRads = minAngleRads; + this.m_maxAngleRads = maxAngleRads; + this.m_motor = motor; + this.m_gearing = G; + this.m_simulateGravity = simulateGravity; + } + + /** + * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate + * the state of the system. In simulationPeriodic, users should first set inputs from motors, update + * the simulation and write simulated outputs to sensors. + * + * @param motor DCMotor representing the motor driving the arm. + * @param G The gear ratio of the arm (numbers greater than 1 + * represent reductions). + * @param armMassKg The mass of the arm. + * @param armLengthMeters The distance from the pivot of the arm to its center of mass. + * @param minAngleRads The min angle the arm can reach, with 0 being measured from + * horizontal. The simulation will not allow motion past this. + * @param maxAngleRads The max angle the arm can reach, with 0 being measured from + * horizontal. The simulation will not allow motion past this. + * @param posMeasurementStdDev Standard deviations of noise in the arm's position measurement. + * @param simulateGravity If the affects of gravity should be simulated. + */ + public SingleJointedArmSim(DCMotor motor, double jKgSquaredMeters, + double G, double armMassKg, double armLengthMeters, + double minAngleRads, double maxAngleRads, + double posMeasurementStdDev, boolean simulateGravity) { + this(LinearSystemId.createSingleJointedArmSystem(motor, jKgSquaredMeters, G), + motor, G, + armMassKg, armLengthMeters, minAngleRads, maxAngleRads, + posMeasurementStdDev, simulateGravity); + } + + /** + * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate + * the state of the system. In simulationPeriodic, users should first set inputs from motors, update + * the simulation and write simulated outputs to sensors. + * + * @param motor DCMotor representing the motor driving the arm. + * @param G The gear ratio of the arm (numbers greater than 1 + * represent reductions). + * @param armMassKg The mass of the arm. + * @param armLengthMeters The distance from the pivot of the arm to its center of mass. + * @param minAngleRads The min angle the arm can reach, with 0 being measured from + * horizontal. The simulation will not allow motion past this. + * @param maxAngleRads The max angle the arm can reach, with 0 being measured from + * horizontal. The simulation will not allow motion past this. + * @param positionMeasurementStdDevs Standard deviations of noise in the arm's position measurement. + * @param simulateGravity If the affects of gravity should be simulated. + */ + public SingleJointedArmSim(DCMotor motor, + double G, double armMassKg, double armLengthMeters, + double minAngleRads, double maxAngleRads, + double positionMeasurementStdDevs, boolean simulateGravity) { + this(LinearSystemId.createSingleJointedArmSystem(motor, + 1.0 / 3.0 * armMassKg * armLengthMeters * armLengthMeters, G), + motor, G, + armMassKg, armLengthMeters, minAngleRads, maxAngleRads, + positionMeasurementStdDevs, simulateGravity); + } + + + /** + * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate + * the state of the system. In simulationPeriodic, users should first set inputs from motors, update + * the simulation and write simulated outputs to sensors. + * + * @param motor DCMotor representing the motor driving the arm. + * @param G The gear ratio of the arm (numbers greater than 1 + * represent reductions). + * @param armMassKg The mass of the arm. + * @param armLengthMeters The distance from the pivot of the arm to its center of mass. + * @param minAngleRads The min angle the arm can reach, with 0 being measured from + * horizontal. The simulation will not allow motion past this. + * @param maxAngleRads The max angle the arm can reach, with 0 being measured from + * horizontal. The simulation will not allow motion past this. + * @param positionMeasurementStdDevs Standard deviations of noise in the arm's position measurement. + */ + public SingleJointedArmSim(DCMotor motor, + double G, double armMassKg, double armLengthMeters, + double minAngleRads, double maxAngleRads, + double positionMeasurementStdDevs) { + this(LinearSystemId.createSingleJointedArmSystem(motor, + 1.0 / 3.0 * armMassKg * armLengthMeters * armLengthMeters, G), + motor, G, + armMassKg, armLengthMeters, minAngleRads, maxAngleRads, + positionMeasurementStdDevs, true); + } + + public boolean hasHitLowerLimit(Matrix x) { + return x.get(0, 0) < this.m_minAngleRads; + } + + public boolean hasHitUpperLimit(Matrix x) { + return x.get(0, 0) > this.m_maxAngleRads; + } + + @Override + protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { + /* + Horizontal case: + Torque = f * r = I * alpha + alpha = f * r / I + since f=mg, + alpha = m g r / I + + Multiply RHS by cos(theta) to account for the arm angle + + This acceleration is added to the linear system dynamics x-dot = Ax + Bu + We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I]] + */ + Matrix updatedXhat = RungeKutta.rungeKutta( + this::dynamics, + currentXhat, u, dtSeconds); + + // We check for collision after updating xhat + if (hasHitLowerLimit(updatedXhat)) { + return VecBuilder.fill(m_minAngleRads, 0); + } else if (hasHitUpperLimit(updatedXhat)) { + return VecBuilder.fill(m_maxAngleRads, 0); + } + + return updatedXhat; + } + + public double getAngleRads() { + return m_x.get(0, 0); + } + + public double getVelocityRadPerSec() { + return m_x.get(1, 0); + } + + public double getInputVoltageVolts() { + return m_u.get(0, 0); + } + + @Override + public double getCurrentDrawAmps() { + // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is + // spinning 10x faster than the output + var motorVelocity = m_x.get(1, 0) * m_gearing; + return m_motor.getCurrent(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); + } + + private Matrix dynamics(Matrix x, Matrix u_) { + Matrix xdot = (m_plant.getA().times(x)) + .plus(m_plant.getB().times(u_)); + + if (m_simulateGravity) { + xdot = xdot.plus(VecBuilder.fill(0, + m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) * Math.cos(x.get(0, 0)))); + } + return xdot; + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ArmSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ArmSimTest.java new file mode 100644 index 0000000000..c519c73e9f --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ArmSimTest.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.VecBuilder; +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class ArmSimTest { + SingleJointedArmSim m_sim = new SingleJointedArmSim( + DCMotor.getVex775Pro(2), + 100, + 10 / 2.2, + Units.inchesToMeters(19.0 / 2.0), + -Math.PI, + 0.0, 0.0); + + @Test + public void testArmDisabled() { + m_sim.resetState(VecBuilder.fill(0.0, 0.0)); + + for (int i = 0; i < 12 / 0.02; i++) { + m_sim.setInput(0.0); + m_sim.update(0.020); + } + + // the arm should swing down + assertEquals(-Math.PI / 2.0, m_sim.getAngleRads(), 0.1); + } +} 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 new file mode 100644 index 0000000000..68c9c6e4bd --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java @@ -0,0 +1,146 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.wpilibj.controller.LinearPlantInversionFeedforward; +import edu.wpi.first.wpilibj.controller.RamseteController; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.system.RungeKutta; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.Vector; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N7; + +import org.junit.jupiter.api.Test; + +import java.util.List; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class DifferentialDrivetrainSimTest { + @Test + public void testConvergence() { + var motor = DCMotor.getNEO(2); + var plant = LinearSystemId.createDrivetrainVelocitySystem( + motor, + 50, + Units.inchesToMeters(2), + Units.inchesToMeters(12), + 0.5, + 1.0); + + var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); + var sim = new DifferentialDrivetrainSim(plant, + motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2)); + + var feedforward = new LinearPlantInversionFeedforward<>(plant, 0.020); + var ramsete = new RamseteController(); + feedforward.reset(VecBuilder.fill(0, 0)); + + // ground truth + Matrix groundTruthX = new Vector<>(Nat.N7()); + + var traj = TrajectoryGenerator.generateTrajectory( + new Pose2d(), + List.of(), + new Pose2d(2, 2, new Rotation2d()), + new TrajectoryConfig(1, 1) + .addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1))); + + for (double t = 0; t < traj.getTotalTimeSeconds(); t += 0.020) { + var state = traj.sample(0.020); + var ramseteOut = ramsete.calculate(sim.getEstimatedPosition(), state); + + var wheelSpeeds = kinematics.toWheelSpeeds(ramseteOut); + + var voltages = feedforward.calculate( + VecBuilder.fill(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond)); + + // Sim periodic code + sim.setInputs(voltages.get(0, 0), voltages.get(1, 0)); + sim.update(0.020); + + // Update our ground truth + groundTruthX = RungeKutta.rungeKutta(sim::getDynamics, groundTruthX, voltages, 0.020); + } + + assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0), + sim.getState(DifferentialDrivetrainSim.State.kX)); + assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kY.value, 0), + sim.getState(DifferentialDrivetrainSim.State.kY)); + assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kHeading.value, 0), + sim.getState(DifferentialDrivetrainSim.State.kHeading)); + } + + @Test + public void testCurrent() { + var motor = DCMotor.getNEO(2); + var plant = LinearSystemId.createDrivetrainVelocitySystem( + motor, + 50, + Units.inchesToMeters(2), + Units.inchesToMeters(12), + 0.5, + 1.0); + var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); + var sim = new DifferentialDrivetrainSim(plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2)); + + sim.setInputs(-12, -12); + for (int i = 0; i < 10; i++) { + sim.update(0.020); + } + assertTrue(sim.getCurrentDrawAmps() > 0); + + sim.setInputs(12, 12); + for (int i = 0; i < 20; i++) { + sim.update(0.020); + } + assertTrue(sim.getCurrentDrawAmps() > 0); + + sim.setInputs(-12, 12); + for (int i = 0; i < 30; i++) { + sim.update(0.020); + } + assertTrue(sim.getCurrentDrawAmps() > 0); + } + + @Test + public void testWHeee() { + var motor = DCMotor.getNEO(2); + var plant = LinearSystemId.createDrivetrainVelocitySystem( + motor, + 50, + Units.inchesToMeters(2), + Units.inchesToMeters(12), + 2.0, + 5.0); + + var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); + var sim = new DifferentialDrivetrainSim(plant, motor, 5, kinematics.trackWidthMeters, Units.inchesToMeters(2)); + + sim.setInputs(2, 4); + + for (int i = 0; i < 10000; i++) { + sim.update(0.020); + } + + System.out.println(sim.getEstimatedPosition()); + assertTrue(Math.abs(sim.getEstimatedPosition().getX()) < 10000); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java new file mode 100644 index 0000000000..e127821491 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java @@ -0,0 +1,82 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpiutil.math.VecBuilder; +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +public class ElevatorSimTest { + @Test + @SuppressWarnings("LocalVariableName") + public void testStateSpaceSimWithElevator() { + + var controller = new PIDController(10, 0, 0); + + var sim = new ElevatorSim(DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67, + 0.0, 3.0, VecBuilder.fill(0.01)); + + var motor = new PWMVictorSPX(0); + var encoder = new Encoder(0, 1); + var encoderSim = new EncoderSim(encoder); + + for (int i = 0; i < 100; i++) { + controller.setSetpoint(2.0); + + double nextVoltage = controller.calculate(encoderSim.getDistance()); + + double currentBatteryVoltage = RobotController.getBatteryVoltage(); + motor.set(nextVoltage / currentBatteryVoltage); + + // ------ SimulationPeriodic() happens after user code ------- + + var u = VecBuilder.fill(motor.get() * currentBatteryVoltage); + sim.setInput(u); + sim.update(0.020); + var y = sim.getOutput(); + encoderSim.setDistance(y.get(0, 0)); + } + + assertEquals(controller.getSetpoint(), sim.getPositionMeters(), 0.2); + } + + @Test + public void testMinMax() { + var plant = LinearSystemId.createElevatorSystem( + DCMotor.getVex775Pro(4), + 8.0, + 0.75 * 25.4 / 1000.0, + 14.67); + + var sim = new ElevatorSim(plant, VecBuilder.fill(0.01), + DCMotor.getVex775Pro(4), + 14.67, 0.75 * 25.4 / 1000.0, 0.0, 1.0); + + for (int i = 0; i < 100; i++) { + sim.setInput(VecBuilder.fill(0)); + sim.update(0.020); + var height = sim.getPositionMeters(); + assertTrue(height >= -0.05); + } + + for (int i = 0; i < 100; i++) { + sim.setInput(VecBuilder.fill(12.0)); + sim.update(0.020); + var height = sim.getPositionMeters(); + assertTrue(height <= 1.05); + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java new file mode 100644 index 0000000000..31777a6a82 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armsimulation; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java new file mode 100644 index 0000000000..4eb77bbba4 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java @@ -0,0 +1,101 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.armsimulation; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.util.Units; + +/** + * This is a sample program to demonstrate the use of elevator simulation with existing code. + */ +public class Robot extends TimedRobot { + private static final int kMotorPort = 0; + private static final int kEncoderAChannel = 0; + private static final int kEncoderBChannel = 1; + private static final int kJoystickPort = 0; + + // The P gain for the PID controller that drives this arm. + private static final double kArmKp = 5.0; + + // distance per pulse = (angle per revolution) / (pulses per revolution) + // = (2 * PI rads) / (4096 pulses) + private static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096; + + // The arm gearbox represents a gerbox containing two Vex 775pro motors. + private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2); + + // Standard classes for controlling our elevator + private final PIDController m_controller = new PIDController(kArmKp, 0, 0); + private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel); + private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort); + private final Joystick m_joystick = new Joystick(kJoystickPort); + + // Simulation classes help us simulate what's going on, including gravity. + private static final double m_armReduction = 100; + private static final double m_armMass = 5.0; // Kilograms + private static final double m_armLength = Units.inchesToMeters(30); + // This arm sim represents an arm that can travel from -180 degrees (rotated straight backwards) + // to 0 degrees (rotated straight forwards). + private final SingleJointedArmSim m_armSim = new SingleJointedArmSim(m_armGearbox, + m_armReduction, + m_armMass, + m_armLength, + Units.degreesToRadians(-180), + Units.degreesToRadians(0), + Units.degreesToRadians(0.5) // Add noise with a standard deviation of 0.5 degrees + ); + private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); + + @Override + public void robotInit() { + m_encoder.setDistancePerPulse(kArmEncoderDistPerPulse); + } + + @Override + public void simulationPeriodic() { + // In this method, we update our simulation of what our elevator is doing + // First, we set our "inputs" (voltages) + m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage()); + + // Next, we update it. The standard loop time is 20ms. + m_armSim.update(0.020); + + // Finally, we set our simulated encoder's readings and simulated battery voltage + m_encoderSim.setDistance(m_armSim.getAngleRads()); + // SimBattery estimates loaded battery voltages + RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps())); + } + + @Override + public void teleopPeriodic() { + if (m_joystick.getTrigger()) { + // Here, we run PID control like normal, with a constant setpoint of 30in. + var pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(0)); + m_motor.setVoltage(pidOutput); + } else { + // Otherwise, we disable the motor. + m_motor.set(0.0); + } + } + + @Override + public void disabledInit() { + // This just makes sure that our simulation code knows that the motor's off. + m_motor.set(0.0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java new file mode 100644 index 0000000000..22cde2ed7f --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.elevatorsimulation; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java new file mode 100644 index 0000000000..22f653cfe4 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java @@ -0,0 +1,100 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.elevatorsimulation; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.VecBuilder; + +/** + * This is a sample program to demonstrate the use of elevator simulation with existing code. + */ +public class Robot extends TimedRobot { + private static final int kMotorPort = 0; + private static final int kEncoderAChannel = 0; + private static final int kEncoderBChannel = 1; + private static final int kJoystickPort = 0; + + private static final double kElevatorKp = 5.0; + private static final double kElevatorGearing = 10.0; + private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0); + private static final double kCarriageMass = 4.0; // kg + + private static final double kMinElevatorHeight = 0.0; + private static final double kMaxElevatorHeight = Units.inchesToMeters(50); + + // distance per pulse = (distance per revolution) / (pulses per revolution) + // = (Pi * D) / ppr + private static final double kElevatorEncoderDistPerPulse = 2.0 * Math.PI * kElevatorDrumRadius / 4096; + + private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4); + + // Standard classes for controlling our elevator + private final PIDController m_controller = new PIDController(kElevatorKp, 0, 0); + private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel); + private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort); + private final Joystick m_joystick = new Joystick(kJoystickPort); + + // Simulation classes help us simulate what's going on, including gravity. + private final ElevatorSim m_elevatorSim = new ElevatorSim(m_elevatorGearbox, + kCarriageMass, + kElevatorDrumRadius, + kElevatorGearing, + kMinElevatorHeight, + kMaxElevatorHeight, + VecBuilder.fill(0.01)); + private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); + + @Override + public void robotInit() { + m_encoder.setDistancePerPulse(kElevatorEncoderDistPerPulse); + } + + @Override + public void simulationPeriodic() { + // In this method, we update our simulation of what our elevator is doing + // First, we set our "inputs" (voltages) + m_elevatorSim.setInput(m_motor.get() * RobotController.getBatteryVoltage()); + + // Next, we update it. The standard loop time is 20ms. + m_elevatorSim.update(0.020); + + // Finally, we set our simulated encoder's readings and simulated battery voltage + m_encoderSim.setDistance(m_elevatorSim.getPositionMeters()); + // SimBattery estimates loaded battery voltages + RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps())); + } + + @Override + public void teleopPeriodic() { + if (m_joystick.getTrigger()) { + // Here, we run PID control like normal, with a constant setpoint of 30in. + double pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.inchesToMeters(30)); + m_motor.setVoltage(pidOutput); + } else { + // Otherwise, we disable the motor. + m_motor.set(0.0); + } + } + + @Override + public void disabledInit() { + // This just makes sure that our simulation code knows that the motor's off. + m_motor.set(0.0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index 47264f5bbd..0f41a1f156 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -631,5 +631,54 @@ "gradlebase": "java", "mainclass": "Main", "commandversion": 2 + }, + { + "name": "StateSpaceSimulation", + "description": "An example of drivetrain simulation in combination with a RAMSETE path following controller.", + "tags": [ + "Drivetrain", + "State space", + "Digital", + "Sensors", + "Actuators", + "Joystick", + "Simulation" + ], + "foldername": "statespacedifferentialdrivesimulation", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "ElevatorSimulation", + "description": "Demonstrates the use of physics simulation with a simple elevator.", + "tags": [ + "Elevator", + "State space", + "Digital", + "Sensors", + "Simulation", + "Physics" + ], + "foldername": "elevatorsimulation", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "ArmSimulation", + "description": "Demonstrates the use of physics simulation with a simple single-jointedarm.", + "tags": [ + "Arm", + "State space", + "Digital", + "Sensors", + "Simulation", + "Physics" + ], + "foldername": "armsimulation", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java new file mode 100644 index 0000000000..b86bd3f19c --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java @@ -0,0 +1,88 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation; + +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpiutil.math.numbers.N2; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final class DriveConstants { + public static final int kLeftMotor1Port = 0; + public static final int kLeftMotor2Port = 1; + public static final int kRightMotor1Port = 2; + public static final int kRightMotor2Port = 3; + + public static final int[] kLeftEncoderPorts = new int[]{0, 1}; + public static final int[] kRightEncoderPorts = new int[]{2, 3}; + public static final boolean kLeftEncoderReversed = false; + public static final boolean kRightEncoderReversed = true; + + public static final double kTrackwidthMeters = 0.69; + public static final DifferentialDriveKinematics kDriveKinematics = + new DifferentialDriveKinematics(kTrackwidthMeters); + + public static final int kEncoderCPR = 1024; + public static final double kWheelDiameterMeters = 0.15; + public static final double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR; + + public static final boolean kGyroReversed = true; + + // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! + // These characterization values MUST be determined either experimentally or theoretically + // for *your* robot's drive. + // The Robot Characterization Toolsuite provides a convenient tool for obtaining these + // values for your robot. + public static final double ksVolts = 0.22; + public static final double kvVoltSecondsPerMeter = 1.98; + public static final double kaVoltSecondsSquaredPerMeter = 0.2; + + // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! + // These characterization values MUST be determined either experimentally or theoretically + // for *your* robot's drive. + // These two values are "angular" kV and kA + public static final double kvVoltSecondsPerRadian = 1.5; + public static final double kaVoltSecondsSquaredPerRadian = 0.3; + + public static final LinearSystem kDrivetrainPlant = + LinearSystemId.identifyDrivetrainSystem(kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter, + kvVoltSecondsPerRadian, kaVoltSecondsSquaredPerRadian); + + // Example values only -- use what's on your physical robot! + public static final DCMotor kDriveGearbox = DCMotor.getCIM(2); + public static final double kDriveGearing = 8; + + // Example value only - as above, this must be tuned for your drive! + public static final double kPDriveVel = 0.1; + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 0; + } + + public static final class AutoConstants { + public static final double kMaxSpeedMetersPerSecond = 3; + public static final double kMaxAccelerationMetersPerSecondSquared = 6; + + // Reasonable baseline values for a RAMSETE follower in units of meters and seconds + public static final double kRamseteB = 2; + public static final double kRamseteZeta = 0.7; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java new file mode 100644 index 0000000000..5b041fb4d9 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java new file mode 100644 index 0000000000..485dfc3be6 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * This is a sample program to demonstrate the use of state-space classes in robot simulation. + * This robot has a flywheel, elevator, arm and differential drivetrain, and interfaces with + * the sim GUI's {@link edu.wpi.first.wpilibj.simulation.Field2d} class. + */ +public class Robot extends TimedRobot { + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + @Override + public void simulationPeriodic() { + // Here we calculate the battery voltage based on drawn current. + // As our robot draws more power from the battery its voltage drops. + // The estimated voltage is highly dependent on the battery's internal + // resistance. + double drawCurrent = m_robotContainer.getRobotDrive().getDrawnCurrentAmps(); + double loadedVoltage = BatterySim.calculateDefaultBatteryLoadedVoltage(drawCurrent); + RoboRioSim.setVInVoltage(loadedVoltage); + } + + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + } + + @Override + public void autonomousInit() { + m_robotContainer.getAutonomousCommand().schedule(); + } + + @Override + public void disabledInit() { + CommandScheduler.getInstance().cancelAll(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java new file mode 100644 index 0000000000..f63fcbdad6 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java @@ -0,0 +1,136 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.controller.RamseteController; +import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.subsystems.DriveSubsystem; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.trajectory.Trajectory; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RamseteCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import java.util.List; + +import static edu.wpi.first.wpilibj.XboxController.Button; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the Robot + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + + // The driver's controller + XboxController m_driverController = new XboxController(Constants.OIConstants.kDriverControllerPort); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + + // Configure default commands + // Set the default drive command to split-stick arcade drive + m_robotDrive.setDefaultCommand( + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + new RunCommand(() -> m_robotDrive + .arcadeDrive(-m_driverController.getY(GenericHID.Hand.kRight), + m_driverController.getX(GenericHID.Hand.kLeft)), m_robotDrive)); + + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * {@link JoystickButton}. + */ + private void configureButtonBindings() { + // Drive at half speed when the right bumper is held + new JoystickButton(m_driverController, Button.kBumperRight.value) + .whenPressed(() -> m_robotDrive.setMaxOutput(0.5)) + .whenReleased(() -> m_robotDrive.setMaxOutput(1)); + + } + + public DriveSubsystem getRobotDrive() { + return m_robotDrive; + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + + // Create a voltage constraint to ensure we don't accelerate too fast + var autoVoltageConstraint = + new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward(Constants.DriveConstants.ksVolts, + Constants.DriveConstants.kvVoltSecondsPerMeter, + Constants.DriveConstants.kaVoltSecondsSquaredPerMeter), + Constants.DriveConstants.kDriveKinematics, + 7); + + // Create config for trajectory + TrajectoryConfig config = + new TrajectoryConfig(Constants.AutoConstants.kMaxSpeedMetersPerSecond, + Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared) + // Add kinematics to ensure max speed is actually obeyed + .setKinematics(Constants.DriveConstants.kDriveKinematics) + // Apply the voltage constraint + .addConstraint(autoVoltageConstraint); + + // An example trajectory to follow. All units in meters. + Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( + // Start at the origin facing the +X direction + new Pose2d(0, 0, new Rotation2d(0)), + // Pass through these two interior waypoints, making an 's' curve path + List.of(), + // End 3 meters straight ahead of where we started, facing forward + new Pose2d(6, 6, new Rotation2d(0)), + // Pass config + config + ); + + RamseteCommand ramseteCommand = new RamseteCommand( + exampleTrajectory, + m_robotDrive::getPose, + new RamseteController(Constants.AutoConstants.kRamseteB, Constants.AutoConstants.kRamseteZeta), + new SimpleMotorFeedforward(Constants.DriveConstants.ksVolts, + Constants.DriveConstants.kvVoltSecondsPerMeter, + Constants.DriveConstants.kaVoltSecondsSquaredPerMeter), + Constants.DriveConstants.kDriveKinematics, + m_robotDrive::getWheelSpeeds, + new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0), + new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0), + // RamseteCommand passes volts to the callback + m_robotDrive::tankDriveVolts, + m_robotDrive + ); + + // Run path following command, then stop at the end. + return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0)); + } +} 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 new file mode 100644 index 0000000000..90620533fb --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java @@ -0,0 +1,257 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.subsystems; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.wpilibj.ADXRS450_Gyro; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.Constants; +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.wpilibj.simulation.EncoderSim; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; +import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; +import edu.wpi.first.wpilibj.simulation.Field2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class DriveSubsystem extends SubsystemBase { + // The motors on the left side of the drive. + private final SpeedControllerGroup m_leftMotors = + new SpeedControllerGroup(new PWMVictorSPX(Constants.DriveConstants.kLeftMotor1Port), + new PWMVictorSPX(Constants.DriveConstants.kLeftMotor2Port)); + + // The motors on the right side of the drive. + private final SpeedControllerGroup m_rightMotors = + new SpeedControllerGroup(new PWMVictorSPX(Constants.DriveConstants.kRightMotor1Port), + new PWMVictorSPX(Constants.DriveConstants.kRightMotor2Port)); + + // The robot's drive + private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); + + // The left-side drive encoder + private final Encoder m_leftEncoder = + new Encoder(Constants.DriveConstants.kLeftEncoderPorts[0], + Constants.DriveConstants.kLeftEncoderPorts[1], + Constants.DriveConstants.kLeftEncoderReversed); + + // The right-side drive encoder + private final Encoder m_rightEncoder = + new Encoder(Constants.DriveConstants.kRightEncoderPorts[0], + Constants.DriveConstants.kRightEncoderPorts[1], + Constants.DriveConstants.kRightEncoderReversed); + + // The gyro sensor + private final Gyro m_gyro = new ADXRS450_Gyro(); + + // Odometry class for tracking robot pose + private final DifferentialDriveOdometry m_odometry; + + // These classes help us simulate our drivetrain + public DifferentialDrivetrainSim m_drivetrainSimulator; + private EncoderSim m_leftEncoderSim; + private EncoderSim m_rightEncoderSim; + // The Field2d class simulates the field in the sim GUI. Note that we can have only one + // instance! + private Field2d m_fieldSim; + private SimDouble m_gyroAngleSim; + + /** + * Creates a new DriveSubsystem. + */ + public DriveSubsystem() { + // Sets the distance per pulse for the encoders + m_leftEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse); + + resetEncoders(); + m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading())); + + if (RobotBase.isSimulation()) { // If our robot is simulated + // This class simulates our drivetrain's motion around the field. + m_drivetrainSimulator = new DifferentialDrivetrainSim( + Constants.DriveConstants.kDrivetrainPlant, + Constants.DriveConstants.kDriveGearbox, + Constants.DriveConstants.kDriveGearing, + Constants.DriveConstants.kTrackwidthMeters, + Constants.DriveConstants.kWheelDiameterMeters / 2.0); + + // The encoder and gyro angle sims let us set simulated sensor readings + m_leftEncoderSim = new EncoderSim(m_leftEncoder); + m_rightEncoderSim = new EncoderSim(m_rightEncoder); + m_gyroAngleSim = + new SimDeviceSim("ADXRS450_Gyro" + "[" + SPI.Port.kOnboardCS0.value + "]") + .getDouble("Angle"); + + // the Field2d class lets us visualize our robot in the simulation GUI. + m_fieldSim = new Field2d(); + } + } + + @Override + public void periodic() { + // Update the odometry in the periodic block + m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(), + m_rightEncoder.getDistance()); + } + + @Override + public void simulationPeriodic() { + // To update our simulation, we set motor voltage inputs, update the simulation, + // and write the simulated positions and velocities to our simulated encoder and gyro. + // We negate the right side so that positive voltages make the right side + // move forward. + m_drivetrainSimulator.setInputs(m_leftMotors.get() * RobotController.getBatteryVoltage(), + -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_gyroAngleSim.set(-m_drivetrainSimulator.getHeading().getDegrees()); + + m_fieldSim.setRobotPose(getPose()); + } + + /** + * Returns the current being drawn by the drivetrain. This works in SIMULATION ONLY! + * If you want it to work elsewhere, use the code in + * {@link DifferentialDrivetrainSim#getCurrentDrawAmps()} + * + * @return The drawn current in Amps. + */ + public double getDrawnCurrentAmps() { + return m_drivetrainSimulator.getCurrentDrawAmps(); + } + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + public Pose2d getPose() { + return m_odometry.getPoseMeters(); + } + + /** + * Returns the current wheel speeds of the robot. + * + * @return The current wheel speeds. + */ + public DifferentialDriveWheelSpeeds getWheelSpeeds() { + return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()); + } + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + public void resetOdometry(Pose2d pose) { + resetEncoders(); + m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); + } + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public void arcadeDrive(double fwd, double rot) { + m_drive.arcadeDrive(fwd, rot); + } + + /** + * Controls the left and right sides of the drive directly with voltages. + * + * @param leftVolts the commanded left output + * @param rightVolts the commanded right output + */ + public void tankDriveVolts(double leftVolts, double rightVolts) { + var batteryVoltage = RobotController.getBatteryVoltage(); + if (Math.max(Math.abs(leftVolts), Math.abs(rightVolts)) + > batteryVoltage) { + leftVolts *= batteryVoltage / 12.0; + rightVolts *= batteryVoltage / 12.0; + } + m_leftMotors.setVoltage(leftVolts); + m_rightMotors.setVoltage(-rightVolts); + m_drive.feed(); + } + + /** + * Resets the drive encoders to currently read a position of 0. + */ + public void resetEncoders() { + m_leftEncoder.reset(); + m_rightEncoder.reset(); + } + + /** + * Gets the average distance of the two encoders. + * + * @return the average of the two encoder readings + */ + public double getAverageEncoderDistance() { + return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.0; + } + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + public Encoder getLeftEncoder() { + return m_leftEncoder; + } + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + public Encoder getRightEncoder() { + return m_rightEncoder; + } + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + public void setMaxOutput(double maxOutput) { + m_drive.setMaxOutput(maxOutput); + } + + /** + * Zeroes the heading of the robot. + */ + public void zeroHeading() { + m_gyro.reset(); + } + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from -180 to 180 + */ + public double getHeading() { + return Math.IEEEremainder(m_gyro.getAngle(), 360) * (Constants.DriveConstants.kGyroReversed ? -1.0 : 1.0); + } + +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java index f6836a3de3..85cbff4ec4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java @@ -50,11 +50,14 @@ public class Robot extends TimedRobot { Units.feetToMeters(3.0), Units.feetToMeters(6.0)); // Max elevator speed and acceleration. private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State(); - // The plant holds a state-space model of our elevator. This system has the following properties: - // - // States: [position, velocity], in meters and meters per second. - // Inputs (what we can "put in"): [voltage], in volts. - // Outputs (what we can measure): [position], in meters. + /* The plant holds a state-space model of our elevator. This system has the following properties: + + States: [position, velocity], in meters and meters per second. + Inputs (what we can "put in"): [voltage], in volts. + Outputs (what we can measure): [position], in meters. + + This elevator is driven by two NEO motors. + */ private final LinearSystem m_elevatorPlant = LinearSystemId.createElevatorSystem( DCMotor.getNEO(2), kCarriageMass, diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java index a38629f930..2e95e3f570 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java @@ -48,10 +48,21 @@ public class DCMotor { this.m_rOhms = nominalVoltageVolts / stallCurrentAmps; this.m_KvRadPerSecPerVolt = freeSpeedRadPerSec / (nominalVoltageVolts - m_rOhms - * freeCurrentAmps); + * freeCurrentAmps); this.m_KtNMPerAmp = stallTorqueNewtonMeters / stallCurrentAmps; } + /** + * Estimate the current being drawn by this motor. + * + * @param speedRadiansPerSec The speed of the rotor. + * @param voltageInputVolts The input voltage. + */ + public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) { + return -1.0 / m_KvRadPerSecPerVolt / m_rOhms * speedRadiansPerSec + + 1.0 / m_rOhms * voltageInputVolts; + } + /** * Return a gearbox of CIM motors. * @@ -59,8 +70,8 @@ public class DCMotor { */ public static DCMotor getCIM(int numMotors) { return new DCMotor(12, - 2.42 * numMotors, 133, - 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310)); + 2.42 * numMotors, 133, + 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310)); } /** @@ -70,8 +81,8 @@ public class DCMotor { */ public static DCMotor getVex775Pro(int numMotors) { return gearbox(new DCMotor(12, - 0.71, 134, - 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730)), numMotors); + 0.71, 134, + 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730)), numMotors); } /** @@ -81,7 +92,7 @@ public class DCMotor { */ public static DCMotor getNEO(int numMotors) { return gearbox(new DCMotor(12, 2.6, - 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676)), numMotors); + 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676)), numMotors); } /** @@ -91,7 +102,7 @@ public class DCMotor { */ public static DCMotor getMiniCIM(int numMotors) { return gearbox(new DCMotor(12, 1.41, 89, 3, - Units.rotationsPerMinuteToRadiansPerSecond(5840)), numMotors); + Units.rotationsPerMinuteToRadiansPerSecond(5840)), numMotors); } /** @@ -101,7 +112,7 @@ public class DCMotor { */ public static DCMotor getBag(int numMotors) { return gearbox(new DCMotor(12, 0.43, 53, 1.8, - Units.rotationsPerMinuteToRadiansPerSecond(13180)), numMotors); + Units.rotationsPerMinuteToRadiansPerSecond(13180)), numMotors); } /** @@ -111,7 +122,7 @@ public class DCMotor { */ public static DCMotor getAndymarkRs775_125(int numMotors) { return gearbox(new DCMotor(12, 0.28, 18, 1.6, - Units.rotationsPerMinuteToRadiansPerSecond(5800.0)), numMotors); + Units.rotationsPerMinuteToRadiansPerSecond(5800.0)), numMotors); } /** @@ -121,7 +132,7 @@ public class DCMotor { */ public static DCMotor getBanebotsRs775(int numMotors) { return gearbox(new DCMotor(12, 0.72, 97, 2.7, - Units.rotationsPerMinuteToRadiansPerSecond(13050.0)), numMotors); + Units.rotationsPerMinuteToRadiansPerSecond(13050.0)), numMotors); } /** @@ -131,7 +142,7 @@ public class DCMotor { */ public static DCMotor getAndymark9015(int numMotors) { return gearbox(new DCMotor(12, 0.36, 71, 3.7, - Units.rotationsPerMinuteToRadiansPerSecond(14270.0)), numMotors); + Units.rotationsPerMinuteToRadiansPerSecond(14270.0)), numMotors); } /** @@ -141,7 +152,7 @@ public class DCMotor { */ public static DCMotor getBanebotsRs550(int numMotors) { return gearbox(new DCMotor(12, 0.38, 84, 0.4, - Units.rotationsPerMinuteToRadiansPerSecond(19000.0)), numMotors); + Units.rotationsPerMinuteToRadiansPerSecond(19000.0)), numMotors); } /** @@ -151,7 +162,7 @@ public class DCMotor { */ public static DCMotor getNeo550(int numMotors) { return gearbox(new DCMotor(12, 0.97, 100, 1.4, - Units.rotationsPerMinuteToRadiansPerSecond(11000.0)), numMotors); + Units.rotationsPerMinuteToRadiansPerSecond(11000.0)), numMotors); } /** @@ -161,11 +172,11 @@ public class DCMotor { */ public static DCMotor getFalcon500(int numMotors) { return gearbox(new DCMotor(12, 4.69, 257, 1.5, - Units.rotationsPerMinuteToRadiansPerSecond(6380.0)), numMotors); + Units.rotationsPerMinuteToRadiansPerSecond(6380.0)), numMotors); } private static DCMotor gearbox(DCMotor motor, double numMotors) { return new DCMotor(motor.m_nominalVoltageVolts, motor.m_stallTorqueNewtonMeters * numMotors, - motor.m_stallCurrentAmps, motor.m_freeCurrentAmps, motor.m_freeSpeedRadPerSec); + motor.m_stallCurrentAmps, motor.m_freeCurrentAmps, motor.m_freeSpeedRadPerSec); } } diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java index ebd7782b7f..eac6260ddb 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java @@ -65,7 +65,9 @@ public final class LinearSystemId { } /** - * Create a state-space model of a differential drive drivetrain. + * Create a state-space model of a differential drive drivetrain. In this model, the + * states are [v_left, v_right]^T, inputs are [V_left, V_right]^T and outputs are + * [v_left, v_right]^T. * * @param motor the gearbox representing the motors driving the drivetrain. * @param massKg the mass of the robot. diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index 620f0f0fbb..b206b88d94 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -187,8 +187,8 @@ class LinearSystemId { * @param J Moment of inertia. */ static LinearSystem<2, 2, 2> DrivetrainVelocitySystem( - DCMotor motor, units::kilogram_t m, units::meter_t r, units::meter_t rb, - units::kilogram_square_meter_t J, double G) { + const DCMotor& motor, units::kilogram_t m, units::meter_t r, + units::meter_t rb, units::kilogram_square_meter_t J, double G) { auto C1 = -std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * units::math::pow<2>(r)); auto C2 = G * motor.Kt / (motor.R * r); diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java index 18182d0b53..75a29e3b95 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java @@ -37,15 +37,12 @@ class ControlAffinePlantInversionFeedforwardTest { @SuppressWarnings("LocalVariableName") @Test void testCalculateState() { - Matrix B = VecBuilder.fill(0, 1); - ControlAffinePlantInversionFeedforward feedforward = - new ControlAffinePlantInversionFeedforward( - Nat.N2(), - Nat.N1(), - this::getStateDynamics, - B, - 0.02); + new ControlAffinePlantInversionFeedforward( + Nat.N2(), + Nat.N1(), + this::getDynamics, + 0.02); assertEquals(48.0, feedforward.calculate( VecBuilder.fill(2, 2),