Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add physics simulation support with state-space (#2615)

Browse Source 
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 <cttewari@gmail.com>
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2020-09-20 09:39:52 -07:00
committed by GitHub
parent 0503225928
commit b61f08d3fa
43 changed files with 3787 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

121
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp Normal file
View File

@@ -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 <frc/system/plant/LinearSystemId.h>
#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<double>(), rightVoltage.to<double>();
}
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<double, 7, 1> 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<double>()),
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<double>()),
units::volt_t(m_u(1))) *
wpi::sgn(m_u(1));
return loadIleft + loadIRight;
}
Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::Dynamics(
const Eigen::Matrix<double, 7, 1>& x,
const Eigen::Matrix<double, 2, 1>& 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<double, 4, 2> 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<double, 4, 4> 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<double, 7, 1> 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<double>();
xdot.block<4, 1>(3, 0) = A * x.block<4, 1>(3, 0) + B * u;
return xdot;
}

79
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp Normal file
View File

@@ -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 <wpi/MathExtras.h>
#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<double, 1>& 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<double, 2, 1>& x) const {
return x(0) < m_minHeight.to<double>();
}
bool ElevatorSim::HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const {
return x(0) > m_maxHeight.to<double>();
}
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<double, 2, 1> ElevatorSim::UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
auto updatedXhat = RungeKutta(
[&](const Eigen::Matrix<double, 2, 1>& x,
const Eigen::Matrix<double, 1, 1>& u_)
-> Eigen::Matrix<double, 2, 1> {
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<double>(), 0.0);
} else if (HasHitUpperLimit(updatedXhat)) {
return MakeMatrix<2, 1>(m_maxHeight.to<double>(), 0.0);
}
return updatedXhat;
}

41
wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp Normal file
View File

@@ -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 <wpi/MathExtras.h>
#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<double, 1>& 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<double, 1>& 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));
}

109
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp Normal file
View File

@@ -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 <cmath>
#include <units/voltage.h>
#include <wpi/MathExtras.h>
#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<double, 1>& 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<double, 1>& 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<double, 1>& 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<double, 2, 1>& x) const {
return x(0) < m_minAngle.to<double>();
}
bool SingleJointedArmSim::HasHitUpperLimit(
const Eigen::Matrix<double, 2, 1>& x) const {
return x(0) > m_maxAngle.to<double>();
}
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<double, 2, 1> SingleJointedArmSim::UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& 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<double, 2, 1> {
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<double>());
},
currentXhat, u, dt);
// Check for collisions.
if (HasHitLowerLimit(updatedXhat)) {
return MakeMatrix<2, 1>(m_minAngle.to<double>(), 0.0);
} else if (HasHitUpperLimit(updatedXhat)) {
return MakeMatrix<2, 1>(m_maxAngle.to<double>(), 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));
}

55
wpilibc/src/main/native/include/frc/simulation/BatterySim.h Normal file
View File

@@ -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 <numeric>
#include <units/current.h>
#include <units/impedance.h>
#include <units/voltage.h>
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<units::ampere_t> 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<units::ampere_t> currents) {
return Calculate(12_V, 0.02_Ohm, currents);
}
};
} // namespace frc::sim

218
wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h Normal file
View File

@@ -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 <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/DCMotor.h>
#include <Eigen/Core>
#include <units/length.h>
#include <units/moment_of_inertia.h>
#include <units/time.h>
#include <units/voltage.h>
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<double, 7, 1> 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<double, 7, 1> Dynamics(const Eigen::Matrix<double, 7, 1>& x,
const Eigen::Matrix<double, 2, 1>& 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<double, 7, 1> m_x;
Eigen::Matrix<double, 2, 1> m_u;
};
} // namespace frc::sim

103
wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h Normal file
View File

@@ -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 <array>
#include <units/length.h>
#include <units/mass.h>
#include <units/velocity.h>
#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<double, 1>& 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<double, 2, 1>& 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<double, 2, 1>& 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<double, 2, 1> UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& 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

73
wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h Normal file
View File

@@ -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 <units/angular_velocity.h>
#include <units/moment_of_inertia.h>
#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<double, 1>& 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<double, 1>& 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

136
wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h Normal file
View File

@@ -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 <array>
#include <Eigen/Core>
#include <units/current.h>
#include <units/time.h>
#include "frc/StateSpaceUtil.h"
#include "frc/system/LinearSystem.h"
namespace frc::sim {
/**
* Represents a simulated generic linear system.
*/
template <int States, int Inputs, int Outputs>
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<States, Inputs, Outputs>& system,
bool addNoise = false,
const std::array<double, Outputs>& measurementStdDevs = {0.0})
: m_plant(system),
m_shouldAddNoise(addNoise),
m_measurementStdDevs(measurementStdDevs) {
m_x = Eigen::Matrix<double, States, 1>::Zero();
m_y = Eigen::Matrix<double, Outputs, 1>::Zero();
m_u = Eigen::Matrix<double, Inputs, 1>::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<double, Outputs, 1>& 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<Outputs>(m_measurementStdDevs);
}
}
/**
* Sets the system inputs (usually voltages).
*
* @param u The system inputs.
*/
void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) { m_u = u; }
/**
* Resets the system state.
*
* @param state The state to reset to.
*/
void ResetState(const Eigen::Matrix<double, States, 1>& 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<double, States, 1> UpdateX(
const Eigen::Matrix<double, States, 1>& currentXhat,
const Eigen::Matrix<double, Inputs, 1>& 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<States, Inputs, Outputs> m_plant;
bool m_shouldAddNoise;
Eigen::Matrix<double, States, 1> m_x;
Eigen::Matrix<double, Outputs, 1> m_y;
Eigen::Matrix<double, Inputs, 1> m_u;
std::array<double, Outputs> m_measurementStdDevs;
};
} // namespace frc::sim

139
wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h Normal file
View File

@@ -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 <array>
#include <units/angle.h>
#include <units/length.h>
#include <units/mass.h>
#include <units/moment_of_inertia.h>
#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<double, 1>& 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<double, 1>& 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<double, 1>& 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<double, 2, 1>& 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<double, 2, 1>& 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<double, 2, 1> UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& 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

69
wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp Normal file
View File

@@ -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 <iostream>
#include <units/time.h>
#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<double>(), 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);
}
}

25
wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp Normal file
View File

@@ -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 <wpi/math>
#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<double>(), -wpi::math::pi / 2, 0.01);
}

57
wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp Normal file
View File

@@ -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 <iostream>
#include <units/angular_acceleration.h>
#include <units/angular_velocity.h>
#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<units::radian> 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<double>());
sim.SetInput(frc::MakeMatrix<1, 1>(
motor.Get() * frc::RobotController::GetInputVoltage()));
sim.Update(20_ms);
encoderSim.SetRate(sim.GetAngularVelocity().to<double>());
}
ASSERT_TRUE(std::abs(200 - encoder.GetRate()) < 0.1);
}

102
wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp Normal file
View File

@@ -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 <frc/Encoder.h>
#include <frc/GenericHID.h>
#include <frc/Joystick.h>
#include <frc/PWMVictorSPX.h>
#include <frc/RobotController.h>
#include <frc/StateSpaceUtil.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
#include <frc/simulation/BatterySim.h>
#include <frc/simulation/EncoderSim.h>
#include <frc/simulation/RoboRioSim.h>
#include <frc/simulation/SingleJointedArmSim.h>
#include <frc/system/plant/LinearSystemId.h>
#include <units/angle.h>
#include <units/moment_of_inertia.h>
#include <wpi/math>
/**
* 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<double>()}};
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<double>());
// SimBattery estimates loaded battery voltages
frc::sim::RoboRioSim::SetVInVoltage(
frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()})
.to<double>());
}
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<double>());
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<Robot>(); }
#endif

58
wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Drivetrain.cpp Normal file
View File

@@ -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 <frc/RobotController.h>
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>());
double rightOutput = m_rightPIDController.Calculate(
m_rightEncoder.GetRate(), speeds.right.to<double>());
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<double>());
m_fieldSim.SetRobotPose(m_odometry.GetPose());
}

54
wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/cpp/Robot.cpp Normal file
View File

@@ -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 <frc/SlewRateLimiter.h>
#include <frc/TimedRobot.h>
#include <frc/XboxController.h>
#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<units::scalar> m_speedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
Drivetrain m_drive;
};
#ifndef RUNNING_FRC_TESTS
int main() { return frc::StartRobot<Robot>(); }
#endif

97
wpilibcExamples/src/main/cpp/examples/DifferentialDriveSimulation/include/Drivetrain.h Normal file
View File

@@ -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 <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/PWMVictorSPX.h>
#include <frc/SpeedControllerGroup.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/kinematics/DifferentialDriveOdometry.h>
#include <frc/simulation/AnalogGyroSim.h>
#include <frc/simulation/DifferentialDrivetrainSim.h>
#include <frc/simulation/EncoderSim.h>
#include <frc/simulation/Field2d.h>
#include <frc/system/plant/LinearSystemId.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/length.h>
#include <units/velocity.h>
#include <wpi/math>
/**
* 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<units::meters> 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};
};

109
wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp Normal file
View File

@@ -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 <frc/Encoder.h>
#include <frc/GenericHID.h>
#include <frc/Joystick.h>
#include <frc/PWMVictorSPX.h>
#include <frc/RobotController.h>
#include <frc/StateSpaceUtil.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
#include <frc/simulation/BatterySim.h>
#include <frc/simulation/ElevatorSim.h>
#include <frc/simulation/EncoderSim.h>
#include <frc/simulation/RoboRioSim.h>
#include <frc/system/plant/LinearSystemId.h>
#include <units/angle.h>
#include <units/moment_of_inertia.h>
#include <wpi/math>
/**
* 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<double>() / 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<double>());
// SimBattery estimates loaded battery voltages
frc::sim::RoboRioSim::SetVInVoltage(
frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()})
.to<double>());
}
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<double>());
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<Robot>(); }
#endif

50
wpilibcExamples/src/main/cpp/examples/examples.json
View File

@@ -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
}
]

52
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java Normal file
View File

@@ -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);
}
}

303
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java Normal file
View File

@@ -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.
*
* <p>Our state-space system is:
*
* <p>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.)
*
* <p>u = [[voltage_l, voltage_r]]^T This is typically the control input of the last timestep
* from a LTVDiffDriveController.
*
* <p>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<N2, N1> m_u;
@SuppressWarnings("MemberName")
private Matrix<N7, N1> m_x;
private final double m_rb;
private final LinearSystem<N2, N2, N2> 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<N2, N2, N2> 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<N7, N1> 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<N7, N1> getDynamics(Matrix<N7, N1> x, Matrix<N2, N1> 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));
}
}

121
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java Normal file
View File

@@ -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<N2, N1, N1> {
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<N1, N1> 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<N2, N1, N1> elevatorPlant,
Matrix<N1, N1> 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<N2, N1> x) {
return x.get(0, 0) < this.m_minHeightMeters;
}
public boolean hasHitUpperLimit(Matrix<N2, N1> x) {
return x.get(0, 0) > this.m_maxHeightMeters;
}
@Override
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> 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);
}
}

79
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java Normal file
View File

@@ -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<N1, N1, N1> {
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<N1, N1, N1> flywheelPlant, DCMotor flywheelGearbox,
double gearing, Matrix<N1, N1> 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<N1, N1> 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));
}
}

137
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java Normal file
View File

@@ -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.
*
* <p>Call {@link #setInput(double...)} with the inputs to the system (usually voltage).
*
* <p>Call {@link #update} to update the simulation.
*
* <p>Set simulated sensor readings with the simulated positions in {@link #getOutput(int)}
*
* @param <States> The number of states of the system.
* @param <Inputs> The number of inputs to the system.
* @param <Outputs> The number of outputs of the system.
*/
@SuppressWarnings("ClassTypeParameterName")
public class LinearSystemSim<States extends Num, Inputs extends Num,
Outputs extends Num> {
protected final LinearSystem<States, Inputs, Outputs> m_plant;
@SuppressWarnings("MemberName")
protected Matrix<States, N1> m_x;
@SuppressWarnings("MemberName")
protected Matrix<Outputs, N1> m_y;
@SuppressWarnings("MemberName")
protected Matrix<Inputs, N1> m_u;
protected final Matrix<Outputs, N1> 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<States, Inputs, Outputs> system, Matrix<Outputs, N1> 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<States, N1> updateX(Matrix<States, N1> currentXhat, Matrix<Inputs, N1> 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<Outputs, N1> getY() {
return m_y;
}
public double getY(int row) {
return m_y.get(row, 0);
}
public void setInput(Matrix<Inputs, N1> 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<Inputs, N1> getInput() {
return m_u;
}
public Matrix<Outputs, N1> getOutput() {
return m_y;
}
public double getOutput(int row) {
return m_y.get(row, 0);
}
public void resetState(Matrix<States, N1> 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;
}
}

213
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java Normal file
View File

@@ -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<N2, N1, N1> {
@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<N2, N1, N1> 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<N2, N1> x) {
return x.get(0, 0) < this.m_minAngleRads;
}
public boolean hasHitUpperLimit(Matrix<N2, N1> x) {
return x.get(0, 0) > this.m_maxAngleRads;
}
@Override
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> 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<N2, N1> 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<N2, N1> dynamics(Matrix<N2, N1> x, Matrix<N1, N1> u_) {
Matrix<N2, N1> 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;
}
}

38
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ArmSimTest.java Normal file
View File

@@ -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);
}
}

146
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java Normal file
View File

@@ -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<N7, N1> 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);
}
}

82
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java Normal file
View File

@@ -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);
}
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java Normal file
View File

@@ -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.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

101
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java Normal file
View File

@@ -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);
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java Normal file
View File

@@ -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.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

100
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java Normal file
View File

@@ -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);
}
}

49
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
View File

@@ -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
}
]

88
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java Normal file
View File

@@ -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.
*
* <p>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<N2, N2, N2> 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;
}
}

29
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java Normal file
View File

@@ -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.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

59
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java Normal file
View File

@@ -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();
}
}

136
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java Normal file
View File

@@ -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));
}
}

257
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java Normal file
View File

@@ -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);
}
}

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
View File

@@ -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<N2, N1, N1> m_elevatorPlant = LinearSystemId.createElevatorSystem(
DCMotor.getNEO(2),
kCarriageMass,

41
wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java
View File

@@ -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);
}
}

4
wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java
View File

@@ -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.

4
wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
View File

@@ -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);

13
wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java
View File

@@ -37,15 +37,12 @@ class ControlAffinePlantInversionFeedforwardTest {
@SuppressWarnings("LocalVariableName")
@Test
void testCalculateState() {
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
new ControlAffinePlantInversionFeedforward<N2, N1>(
Nat.N2(),
Nat.N1(),
this::getStateDynamics,
B,
0.02);
new ControlAffinePlantInversionFeedforward<N2, N1>(
Nat.N2(),
Nat.N1(),
this::getDynamics,
0.02);
assertEquals(48.0, feedforward.calculate(
VecBuilder.fill(2, 2),