mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[wpimath] Reorganize LinearSystem factories (#8468)
This commit is contained in:
@@ -181,8 +181,6 @@ task generateJavaDocs(type: Javadoc) {
|
||||
"-org.wpilib.math.spline.struct," +
|
||||
"-org.wpilib.math.system.proto," +
|
||||
"-org.wpilib.math.system.struct," +
|
||||
"-org.wpilib.math.system.plant.proto," +
|
||||
"-org.wpilib.math.system.plant.struct," +
|
||||
"-org.wpilib.math.trajectory.proto," +
|
||||
"-org.wpilib.math.trajectory.struct," +
|
||||
"-org.wpilib.command3.proto," +
|
||||
|
||||
@@ -8,7 +8,7 @@
|
||||
|
||||
#include "wpi/math/controller/LinearQuadraticRegulator.hpp"
|
||||
#include "wpi/math/system/LinearSystem.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/sysid/analysis/FeedbackControllerPreset.hpp"
|
||||
#include "wpi/units/acceleration.hpp"
|
||||
#include "wpi/units/velocity.hpp"
|
||||
@@ -16,13 +16,14 @@
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
using Kv_t = decltype(1_V / 1_mps);
|
||||
using Ka_t = decltype(1_V / 1_mps_sq);
|
||||
using Matrix1d = Eigen::Matrix<double, 1, 1>;
|
||||
|
||||
FeedbackGains sysid::CalculatePositionFeedbackGains(
|
||||
const FeedbackControllerPreset& preset, const LQRParameters& params,
|
||||
double Kv, double Ka) {
|
||||
using Kv_t = decltype(1_V / 1_mps);
|
||||
using Ka_t = decltype(1_V / 1_mps_sq);
|
||||
|
||||
if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
|
||||
return {0.0, 0.0};
|
||||
}
|
||||
@@ -43,9 +44,7 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
|
||||
return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
|
||||
}
|
||||
|
||||
auto system =
|
||||
wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meters>(
|
||||
Kv_t{Kv}, Ka_t{Ka});
|
||||
auto system = wpi::math::Models::ElevatorFromSysId(Kv_t{Kv}, Ka_t{Ka});
|
||||
|
||||
wpi::math::LinearQuadraticRegulator<2, 1> controller{
|
||||
system, {params.qp, params.qv}, {params.r}, preset.period};
|
||||
@@ -61,6 +60,9 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
|
||||
FeedbackGains sysid::CalculateVelocityFeedbackGains(
|
||||
const FeedbackControllerPreset& preset, const LQRParameters& params,
|
||||
double Kv, double Ka, double encFactor) {
|
||||
using Kv_t = decltype(1_V / 1_rad_per_s);
|
||||
using Ka_t = decltype(1_V / 1_rad_per_s_sq);
|
||||
|
||||
if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
|
||||
return {0.0, 0.0};
|
||||
}
|
||||
@@ -72,9 +74,7 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains(
|
||||
return {0.0, 0.0};
|
||||
}
|
||||
|
||||
auto system =
|
||||
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meters>(
|
||||
Kv_t{Kv}, Ka_t{Ka});
|
||||
auto system = wpi::math::Models::FlywheelFromSysId(Kv_t{Kv}, Ka_t{Ka});
|
||||
wpi::math::LinearQuadraticRegulator<1, 1> controller{
|
||||
system, {params.qv}, {params.r}, preset.period};
|
||||
controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
|
||||
|
||||
@@ -7,8 +7,8 @@
|
||||
#include <utility>
|
||||
|
||||
#include "wpi/math/random/Normal.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/math/system/NumericalIntegration.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/util/StateSpaceUtil.hpp"
|
||||
#include "wpi/system/RobotController.hpp"
|
||||
#include "wpi/util/MathExtras.hpp"
|
||||
@@ -39,7 +39,7 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim(
|
||||
wpi::units::meter_t wheelRadius, wpi::units::meter_t trackwidth,
|
||||
const std::array<double, 7>& measurementStdDevs)
|
||||
: DifferentialDrivetrainSim(
|
||||
wpi::math::LinearSystemId::DrivetrainVelocitySystem(
|
||||
wpi::math::Models::DifferentialDriveFromPhysicalConstants(
|
||||
driveMotor, mass, wheelRadius, trackwidth / 2.0, J, gearing),
|
||||
trackwidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {}
|
||||
|
||||
|
||||
@@ -4,8 +4,8 @@
|
||||
|
||||
#include "wpi/simulation/ElevatorSim.hpp"
|
||||
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/math/system/NumericalIntegration.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/system/RobotController.hpp"
|
||||
#include "wpi/util/MathExtras.hpp"
|
||||
|
||||
@@ -33,7 +33,7 @@ ElevatorSim::ElevatorSim(const wpi::math::DCMotor& gearbox, double gearing,
|
||||
wpi::units::meter_t maxHeight, bool simulateGravity,
|
||||
wpi::units::meter_t startingHeight,
|
||||
const std::array<double, 2>& measurementStdDevs)
|
||||
: ElevatorSim(wpi::math::LinearSystemId::ElevatorSystem(
|
||||
: ElevatorSim(wpi::math::Models::ElevatorFromPhysicalConstants(
|
||||
gearbox, carriageMass, drumRadius, gearing),
|
||||
gearbox, minHeight, maxHeight, simulateGravity,
|
||||
startingHeight, measurementStdDevs) {}
|
||||
@@ -48,9 +48,9 @@ ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t<Distance>(1)) kV,
|
||||
wpi::units::meter_t maxHeight, bool simulateGravity,
|
||||
wpi::units::meter_t startingHeight,
|
||||
const std::array<double, 2>& measurementStdDevs)
|
||||
: ElevatorSim(wpi::math::LinearSystemId::IdentifyPositionSystem(kV, kA),
|
||||
gearbox, minHeight, maxHeight, simulateGravity,
|
||||
startingHeight, measurementStdDevs) {}
|
||||
: ElevatorSim(wpi::math::Models::ElevatorFromSysId(kV, kA), gearbox,
|
||||
minHeight, maxHeight, simulateGravity, startingHeight,
|
||||
measurementStdDevs) {}
|
||||
|
||||
void ElevatorSim::SetState(wpi::units::meter_t position,
|
||||
wpi::units::meters_per_second_t velocity) {
|
||||
|
||||
@@ -6,8 +6,8 @@
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/math/system/NumericalIntegration.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/system/RobotController.hpp"
|
||||
#include "wpi/units/voltage.hpp"
|
||||
#include "wpi/util/MathExtras.hpp"
|
||||
@@ -38,10 +38,11 @@ SingleJointedArmSim::SingleJointedArmSim(
|
||||
wpi::units::radian_t minAngle, wpi::units::radian_t maxAngle,
|
||||
bool simulateGravity, wpi::units::radian_t startingAngle,
|
||||
const std::array<double, 2>& measurementStdDevs)
|
||||
: SingleJointedArmSim(wpi::math::LinearSystemId::SingleJointedArmSystem(
|
||||
gearbox, moi, gearing),
|
||||
gearbox, gearing, armLength, minAngle, maxAngle,
|
||||
simulateGravity, startingAngle, measurementStdDevs) {}
|
||||
: SingleJointedArmSim(
|
||||
wpi::math::Models::SingleJointedArmFromPhysicalConstants(gearbox, moi,
|
||||
gearing),
|
||||
gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity,
|
||||
startingAngle, measurementStdDevs) {}
|
||||
|
||||
void SingleJointedArmSim::SetState(wpi::units::radian_t angle,
|
||||
wpi::units::radians_per_second_t velocity) {
|
||||
|
||||
@@ -4,8 +4,8 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/LinearSystem.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/simulation/LinearSystemSim.hpp"
|
||||
#include "wpi/units/angle.hpp"
|
||||
#include "wpi/units/angular_acceleration.hpp"
|
||||
@@ -22,12 +22,11 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> {
|
||||
/**
|
||||
* Creates a simulated DC motor mechanism.
|
||||
*
|
||||
* @param plant The linear system representing the DC motor. This
|
||||
* system can be created with wpi::math::LinearSystemId::DCMotorSystem(). If
|
||||
* wpi::math::LinearSystemId::DCMotorSystem(kV, kA) is used, the distance unit
|
||||
* must be radians.
|
||||
* @param gearbox The type of and number of motors in the DC motor
|
||||
* gearbox.
|
||||
* @param plant The linear system representing the DC motor. This system can
|
||||
* be created with wpi::math::Models::ElevatorFromPhysicalConstants(). If
|
||||
* wpi::math::Models::ElevatorFromSysId(kV, kA) is used, the distance unit
|
||||
* must be radians.
|
||||
* @param gearbox The type of and number of motors in the DC motor gearbox.
|
||||
* @param measurementStdDevs The standard deviation of the measurement noise.
|
||||
*/
|
||||
DCMotorSim(const wpi::math::LinearSystem<2, 1, 2>& plant,
|
||||
|
||||
@@ -4,13 +4,15 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
|
||||
#include "wpi/math/geometry/Pose2d.hpp"
|
||||
#include "wpi/math/geometry/Rotation2d.hpp"
|
||||
#include "wpi/math/linalg/EigenCore.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/LinearSystem.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
#include "wpi/units/moment_of_inertia.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
#include "wpi/units/velocity.hpp"
|
||||
#include "wpi/units/voltage.hpp"
|
||||
|
||||
namespace wpi::sim {
|
||||
@@ -21,24 +23,22 @@ class DifferentialDrivetrainSim {
|
||||
* Creates a simulated differential drivetrain.
|
||||
*
|
||||
* @param plant The wpi::math::LinearSystem representing the robot's
|
||||
* drivetrain. This system can be created with
|
||||
* wpi::math::LinearSystemId::DrivetrainVelocitySystem() or
|
||||
* wpi::math::LinearSystemId::IdentifyDrivetrainSystem().
|
||||
* @param trackwidth The robot's trackwidth.
|
||||
* @param driveMotor A wpi::math::DCMotor representing the left side of the
|
||||
* drivetrain.
|
||||
* drivetrain. This system can be created with
|
||||
* wpi::math::Models::DifferentialDriveFromPhysicalConstants() or
|
||||
* wpi::math::Models::DifferentialDriveFromSysId().
|
||||
* @param trackwidth The robot's trackwidth.
|
||||
* @param driveMotor A wpi::math::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 plant.
|
||||
* input. This must be the same ratio as the ratio used to identify or
|
||||
* create the plant.
|
||||
* @param wheelRadius The radius of the wheels on the drivetrain, in meters.
|
||||
* @param measurementStdDevs Standard deviations for measurements, in the form
|
||||
* [x, y, heading, left velocity, right velocity,
|
||||
* left distance, right distance]ᵀ. Can be omitted
|
||||
* if no noise is desired. Gyro standard deviations
|
||||
* of 0.0001 radians, velocity standard deviations
|
||||
* of 0.05 m/s, and position measurement standard
|
||||
* deviations of 0.005 meters are a reasonable
|
||||
* starting point.
|
||||
* [x, y, heading, left velocity, right velocity, left distance, right
|
||||
* distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
|
||||
* deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s,
|
||||
* and position measurement standard deviations of 0.005 meters are a
|
||||
* reasonable starting point.
|
||||
*/
|
||||
DifferentialDrivetrainSim(
|
||||
wpi::math::LinearSystem<2, 2, 2> plant, wpi::units::meter_t trackwidth,
|
||||
@@ -49,25 +49,22 @@ class DifferentialDrivetrainSim {
|
||||
/**
|
||||
* Creates a simulated differential drivetrain.
|
||||
*
|
||||
* @param driveMotor A wpi::math::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 plant.
|
||||
* @param J The moment of inertia of the drivetrain about its
|
||||
* center.
|
||||
* @param mass The mass of the drivebase.
|
||||
* @param driveMotor A wpi::math::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 plant.
|
||||
* @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 trackwidth, or distance between left and
|
||||
* right wheels.
|
||||
* @param trackwidth The robot's trackwidth, or distance between left and
|
||||
* right wheels.
|
||||
* @param measurementStdDevs Standard deviations for measurements, in the form
|
||||
* [x, y, heading, left velocity, right velocity,
|
||||
* left distance, right distance]ᵀ. Can be omitted
|
||||
* if no noise is desired. Gyro standard deviations
|
||||
* of 0.0001 radians, velocity standard deviations
|
||||
* of 0.05 m/s, and position measurement standard
|
||||
* deviations of 0.005 meters are a reasonable
|
||||
* starting point.
|
||||
* [x, y, heading, left velocity, right velocity, left distance, right
|
||||
* distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
|
||||
* deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s,
|
||||
* and position measurement standard deviations of 0.005 meters are a
|
||||
* reasonable starting point.
|
||||
*/
|
||||
DifferentialDrivetrainSim(
|
||||
wpi::math::DCMotor driveMotor, double gearing,
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
|
||||
#include <array>
|
||||
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/simulation/LinearSystemSim.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
#include "wpi/units/mass.hpp"
|
||||
@@ -31,15 +31,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> {
|
||||
/**
|
||||
* Constructs a simulated elevator mechanism.
|
||||
*
|
||||
* @param plant The linear system that represents the elevator.
|
||||
* This system can be created with
|
||||
* wpi::math::LinearSystemId::ElevatorSystem().
|
||||
* @param gearbox The type of and number of motors in your
|
||||
* elevator gearbox.
|
||||
* @param minHeight The minimum allowed height of the elevator.
|
||||
* @param maxHeight The maximum allowed height of the elevator.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingHeight The starting height of the elevator.
|
||||
* @param plant The linear system that represents the elevator. This system
|
||||
* can be created with wpi::math::Models::ElevatorFromPhysicalConstants().
|
||||
* @param gearbox The type of and number of motors in your elevator gearbox.
|
||||
* @param minHeight The minimum allowed height of the elevator.
|
||||
* @param maxHeight The maximum allowed height of the elevator.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingHeight The starting height of the elevator.
|
||||
* @param measurementStdDevs The standard deviation of the measurements.
|
||||
*/
|
||||
ElevatorSim(const wpi::math::LinearSystem<2, 1, 2>& plant,
|
||||
@@ -51,17 +49,15 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> {
|
||||
/**
|
||||
* Constructs a simulated elevator mechanism.
|
||||
*
|
||||
* @param gearbox The type of and number of motors in your
|
||||
* elevator gearbox.
|
||||
* @param gearing The gearing of the elevator (numbers greater
|
||||
* than 1 represent reductions).
|
||||
* @param carriageMass The mass of the elevator carriage.
|
||||
* @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 simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingHeight The starting height of the elevator.
|
||||
* @param gearbox The type of and number of motors in your elevator gearbox.
|
||||
* @param gearing The gearing of the elevator (numbers greater than 1
|
||||
* represent reductions).
|
||||
* @param carriageMass The mass of the elevator carriage.
|
||||
* @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 simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingHeight The starting height of the elevator.
|
||||
* @param measurementStdDevs The standard deviation of the measurements.
|
||||
*/
|
||||
ElevatorSim(const wpi::math::DCMotor& gearbox, double gearing,
|
||||
@@ -74,14 +70,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> {
|
||||
/**
|
||||
* Constructs a simulated elevator mechanism.
|
||||
*
|
||||
* @param kV The velocity gain.
|
||||
* @param kA The acceleration gain.
|
||||
* @param gearbox The type of and number of motors in your
|
||||
* elevator gearbox.
|
||||
* @param minHeight The minimum allowed height of the elevator.
|
||||
* @param maxHeight The maximum allowed height of the elevator.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingHeight The starting height of the elevator.
|
||||
* @param kV The velocity gain.
|
||||
* @param kA The acceleration gain.
|
||||
* @param gearbox The type of and number of motors in your elevator gearbox.
|
||||
* @param minHeight The minimum allowed height of the elevator.
|
||||
* @param maxHeight The maximum allowed height of the elevator.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingHeight The starting height of the elevator.
|
||||
* @param measurementStdDevs The standard deviation of the measurements.
|
||||
*/
|
||||
template <typename Distance>
|
||||
@@ -98,6 +93,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> {
|
||||
/**
|
||||
* Sets the elevator's state. The new position will be limited between the
|
||||
* minimum and maximum allowed heights.
|
||||
*
|
||||
* @param position The new position
|
||||
* @param velocity The new velocity
|
||||
*/
|
||||
@@ -167,8 +163,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> {
|
||||
* 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.
|
||||
* @param u The system inputs (voltage).
|
||||
* @param dt The time difference between controller updates.
|
||||
*/
|
||||
wpi::math::Vectord<2> UpdateX(const wpi::math::Vectord<2>& currentXhat,
|
||||
const wpi::math::Vectord<1>& u,
|
||||
|
||||
@@ -4,8 +4,8 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/LinearSystem.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/simulation/LinearSystemSim.hpp"
|
||||
#include "wpi/units/angular_acceleration.hpp"
|
||||
#include "wpi/units/angular_velocity.hpp"
|
||||
@@ -21,12 +21,10 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
|
||||
/**
|
||||
* Creates a simulated flywheel mechanism.
|
||||
*
|
||||
* @param plant The linear system representing the flywheel. This
|
||||
* system can be created with
|
||||
* wpi::math::LinearSystemId::FlywheelSystem() or
|
||||
* wpi::math::LinearSystemId::IdentifyVelocitySystem().
|
||||
* @param gearbox The type of and number of motors in the flywheel
|
||||
* gearbox.
|
||||
* @param plant The linear system representing the flywheel. This system can
|
||||
* be created with wpi::math::Models::FlywheelFromPhysicalConstants() or
|
||||
* wpi::math::Models::FlywheelFromSysId().
|
||||
* @param gearbox The type of and number of motors in the flywheel gearbox.
|
||||
* @param measurementStdDevs The standard deviation of the measurement noise.
|
||||
*/
|
||||
FlywheelSim(const wpi::math::LinearSystem<1, 1, 1>& plant,
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
|
||||
#include <array>
|
||||
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/simulation/LinearSystemSim.hpp"
|
||||
#include "wpi/units/angle.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
@@ -22,17 +22,16 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> {
|
||||
/**
|
||||
* Creates a simulated arm mechanism.
|
||||
*
|
||||
* @param system The system representing this arm. This system can
|
||||
* be created with
|
||||
* wpi::math::LinearSystemId::SingleJointedArmSystem().
|
||||
* @param gearbox The type and number of motors on the arm gearbox.
|
||||
* @param gearing The gear ratio of the arm (numbers greater than 1
|
||||
* represent reductions).
|
||||
* @param armLength The length of the arm.
|
||||
* @param minAngle The minimum angle that the arm is capable of.
|
||||
* @param maxAngle The maximum angle that the arm is capable of.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingAngle The initial position of the arm.
|
||||
* @param system The system representing this arm. This system can be created
|
||||
* with wpi::math::Models::SingleJointedArmFromPhysicalConstants().
|
||||
* @param gearbox The type and number of motors on the arm gearbox.
|
||||
* @param gearing The gear ratio of the arm (numbers greater than 1 represent
|
||||
* reductions).
|
||||
* @param armLength The length of the arm.
|
||||
* @param minAngle The minimum angle that the arm is capable of.
|
||||
* @param maxAngle The maximum angle that the arm is capable of.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingAngle The initial position of the arm.
|
||||
* @param measurementStdDevs The standard deviations of the measurements.
|
||||
*/
|
||||
SingleJointedArmSim(const wpi::math::LinearSystem<2, 1, 2>& system,
|
||||
@@ -46,16 +45,16 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> {
|
||||
/**
|
||||
* Creates a simulated arm mechanism.
|
||||
*
|
||||
* @param gearbox The type and number of motors on the arm gearbox.
|
||||
* @param gearing The gear ratio of the arm (numbers greater than 1
|
||||
* represent reductions).
|
||||
* @param moi The moment of inertia of the arm. This can be
|
||||
* calculated from CAD software.
|
||||
* @param armLength The length of the arm.
|
||||
* @param minAngle The minimum angle that the arm is capable of.
|
||||
* @param maxAngle The maximum angle that the arm is capable of.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingAngle The initial position of the arm.
|
||||
* @param gearbox The type and number of motors on the arm gearbox.
|
||||
* @param gearing The gear ratio of the arm (numbers greater than 1 represent
|
||||
* reductions).
|
||||
* @param moi The moment of inertia of the arm. This can be calculated from
|
||||
* CAD software.
|
||||
* @param armLength The length of the arm.
|
||||
* @param minAngle The minimum angle that the arm is capable of.
|
||||
* @param maxAngle The maximum angle that the arm is capable of.
|
||||
* @param simulateGravity Whether gravity should be simulated or not.
|
||||
* @param startingAngle The initial position of the arm.
|
||||
* @param measurementStdDevs The standard deviation of the measurement noise.
|
||||
*/
|
||||
SingleJointedArmSim(
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
#include "wpi/hardware/motor/PWMVictorSPX.hpp"
|
||||
#include "wpi/hardware/rotation/Encoder.hpp"
|
||||
#include "wpi/math/controller/PIDController.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/simulation/BatterySim.hpp"
|
||||
#include "wpi/simulation/EncoderSim.hpp"
|
||||
#include "wpi/simulation/RoboRioSim.hpp"
|
||||
@@ -17,7 +17,7 @@
|
||||
|
||||
TEST(DCMotorSimTest, VoltageSteadyState) {
|
||||
wpi::math::DCMotor gearbox = wpi::math::DCMotor::NEO(1);
|
||||
auto plant = wpi::math::LinearSystemId::DCMotorSystem(
|
||||
auto plant = wpi::math::Models::SingleJointedArmFromPhysicalConstants(
|
||||
wpi::math::DCMotor::NEO(1), wpi::units::kilogram_square_meter_t{0.0005},
|
||||
1.0);
|
||||
wpi::sim::DCMotorSim sim{plant, gearbox};
|
||||
@@ -64,7 +64,7 @@ TEST(DCMotorSimTest, VoltageSteadyState) {
|
||||
|
||||
TEST(DCMotorSimTest, PositionFeedbackControl) {
|
||||
wpi::math::DCMotor gearbox = wpi::math::DCMotor::NEO(1);
|
||||
auto plant = wpi::math::LinearSystemId::DCMotorSystem(
|
||||
auto plant = wpi::math::Models::SingleJointedArmFromPhysicalConstants(
|
||||
wpi::math::DCMotor::NEO(1), wpi::units::kilogram_square_meter_t{0.0005},
|
||||
1.0);
|
||||
wpi::sim::DCMotorSim sim{plant, gearbox};
|
||||
|
||||
@@ -9,9 +9,9 @@
|
||||
#include "wpi/math/controller/LTVUnicycleController.hpp"
|
||||
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"
|
||||
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/math/system/NumericalIntegration.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
|
||||
#include "wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp"
|
||||
#include "wpi/units/current.hpp"
|
||||
@@ -20,7 +20,7 @@
|
||||
|
||||
TEST(DifferentialDrivetrainSimTest, Convergence) {
|
||||
auto motor = wpi::math::DCMotor::NEO(2);
|
||||
auto plant = wpi::math::LinearSystemId::DrivetrainVelocitySystem(
|
||||
auto plant = wpi::math::Models::DifferentialDriveFromPhysicalConstants(
|
||||
motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
|
||||
|
||||
wpi::math::DifferentialDriveKinematics kinematics{24_in};
|
||||
@@ -73,7 +73,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) {
|
||||
|
||||
TEST(DifferentialDrivetrainSimTest, Current) {
|
||||
auto motor = wpi::math::DCMotor::NEO(2);
|
||||
auto plant = wpi::math::LinearSystemId::DrivetrainVelocitySystem(
|
||||
auto plant = wpi::math::Models::DifferentialDriveFromPhysicalConstants(
|
||||
motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
|
||||
|
||||
wpi::math::DifferentialDriveKinematics kinematics{24_in};
|
||||
@@ -100,7 +100,7 @@ TEST(DifferentialDrivetrainSimTest, Current) {
|
||||
|
||||
TEST(DifferentialDrivetrainSimTest, ModelStability) {
|
||||
auto motor = wpi::math::DCMotor::NEO(2);
|
||||
auto plant = wpi::math::LinearSystemId::DrivetrainVelocitySystem(
|
||||
auto plant = wpi::math::Models::DifferentialDriveFromPhysicalConstants(
|
||||
motor, 50_kg, 2_in, 12_in, 2_kg_sq_m, 5.0);
|
||||
|
||||
wpi::math::DifferentialDriveKinematics kinematics{24_in};
|
||||
|
||||
@@ -9,9 +9,9 @@
|
||||
#include "wpi/hardware/motor/PWMVictorSPX.hpp"
|
||||
#include "wpi/hardware/rotation/Encoder.hpp"
|
||||
#include "wpi/math/controller/PIDController.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/math/system/NumericalIntegration.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/simulation/EncoderSim.hpp"
|
||||
#include "wpi/system/RobotController.hpp"
|
||||
#include "wpi/units/math.hpp"
|
||||
@@ -92,7 +92,7 @@ TEST(ElevatorSimTest, Stability) {
|
||||
}
|
||||
|
||||
wpi::math::LinearSystem<2, 1, 1> system =
|
||||
wpi::math::LinearSystemId::ElevatorSystem(
|
||||
wpi::math::Models::ElevatorFromPhysicalConstants(
|
||||
wpi::math::DCMotor::Vex775Pro(4), 4_kg, 0.5_in, 100)
|
||||
.Slice(0);
|
||||
EXPECT_NEAR_UNITS(wpi::units::meter_t{system.CalculateX(
|
||||
|
||||
@@ -8,7 +8,7 @@
|
||||
#include "wpi/hardware/rotation/Encoder.hpp"
|
||||
#include "wpi/math/controller/PIDController.hpp"
|
||||
#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/simulation/BatterySim.hpp"
|
||||
#include "wpi/simulation/DifferentialDrivetrainSim.hpp"
|
||||
#include "wpi/simulation/ElevatorSim.hpp"
|
||||
@@ -23,8 +23,8 @@
|
||||
|
||||
TEST(StateSpaceSimTest, FlywheelSim) {
|
||||
const wpi::math::LinearSystem<1, 1, 1> plant =
|
||||
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::radian>(
|
||||
0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq);
|
||||
wpi::math::Models::FlywheelFromSysId(0.02_V / 1_rad_per_s,
|
||||
0.01_V / 1_rad_per_s_sq);
|
||||
wpi::sim::FlywheelSim sim{plant, wpi::math::DCMotor::NEO(2)};
|
||||
wpi::math::PIDController controller{0.2, 0.0, 0.0};
|
||||
wpi::math::SimpleMotorFeedforward<wpi::units::radian> feedforward{
|
||||
|
||||
@@ -6,6 +6,10 @@
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "wpi/math/util/ComputerVisionUtil.hpp"
|
||||
#include "wpi/smartdashboard/SmartDashboard.hpp"
|
||||
#include "wpi/system/RobotController.hpp"
|
||||
|
||||
Drivetrain::Drivetrain() {
|
||||
m_leftLeader.AddFollower(m_leftFollower);
|
||||
m_rightLeader.AddFollower(m_rightFollower);
|
||||
|
||||
@@ -16,19 +16,14 @@
|
||||
#include "wpi/math/estimator/DifferentialDrivePoseEstimator.hpp"
|
||||
#include "wpi/math/geometry/Pose2d.hpp"
|
||||
#include "wpi/math/geometry/Pose3d.hpp"
|
||||
#include "wpi/math/geometry/Quaternion.hpp"
|
||||
#include "wpi/math/geometry/Transform3d.hpp"
|
||||
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/util/ComputerVisionUtil.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/nt/DoubleArrayTopic.hpp"
|
||||
#include "wpi/nt/NetworkTableInstance.hpp"
|
||||
#include "wpi/simulation/DifferentialDrivetrainSim.hpp"
|
||||
#include "wpi/simulation/EncoderSim.hpp"
|
||||
#include "wpi/smartdashboard/Field2d.hpp"
|
||||
#include "wpi/smartdashboard/SmartDashboard.hpp"
|
||||
#include "wpi/system/RobotController.hpp"
|
||||
#include "wpi/system/Timer.hpp"
|
||||
#include "wpi/units/angle.hpp"
|
||||
#include "wpi/units/angular_velocity.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
@@ -174,7 +169,7 @@ class Drivetrain {
|
||||
wpi::Field2d m_fieldSim;
|
||||
wpi::Field2d m_fieldApproximation;
|
||||
wpi::math::LinearSystem<2, 2, 2> m_drivetrainSystem =
|
||||
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
|
||||
wpi::math::Models::DifferentialDriveFromSysId(
|
||||
1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
|
||||
wpi::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
|
||||
m_drivetrainSystem, kTrackwidth, wpi::math::DCMotor::CIM(2), 8, 2_in};
|
||||
|
||||
@@ -8,7 +8,7 @@
|
||||
#include "wpi/hardware/rotation/Encoder.hpp"
|
||||
#include "wpi/math/controller/BangBangController.hpp"
|
||||
#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/simulation/EncoderSim.hpp"
|
||||
#include "wpi/simulation/FlywheelSim.hpp"
|
||||
#include "wpi/smartdashboard/SmartDashboard.hpp"
|
||||
@@ -96,7 +96,7 @@ class Robot : public wpi::TimedRobot {
|
||||
|
||||
wpi::math::DCMotor m_gearbox = wpi::math::DCMotor::NEO(1);
|
||||
wpi::math::LinearSystem<1, 1, 1> m_plant{
|
||||
wpi::math::LinearSystemId::FlywheelSystem(
|
||||
wpi::math::Models::FlywheelFromPhysicalConstants(
|
||||
m_gearbox, kFlywheelMomentOfInertia, kFlywheelGearing)};
|
||||
|
||||
wpi::sim::FlywheelSim m_flywheelSim{m_plant, m_gearbox};
|
||||
|
||||
@@ -14,7 +14,6 @@
|
||||
#include "wpi/math/estimator/MecanumDrivePoseEstimator.hpp"
|
||||
#include "wpi/math/geometry/Translation2d.hpp"
|
||||
#include "wpi/math/kinematics/MecanumDriveKinematics.hpp"
|
||||
#include "wpi/math/kinematics/MecanumDriveOdometry.hpp"
|
||||
#include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
|
||||
|
||||
/**
|
||||
|
||||
@@ -13,12 +13,11 @@
|
||||
#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
|
||||
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
|
||||
#include "wpi/math/kinematics/DifferentialDriveOdometry.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/simulation/DifferentialDrivetrainSim.hpp"
|
||||
#include "wpi/simulation/EncoderSim.hpp"
|
||||
#include "wpi/smartdashboard/Field2d.hpp"
|
||||
#include "wpi/smartdashboard/SmartDashboard.hpp"
|
||||
#include "wpi/units/angle.hpp"
|
||||
#include "wpi/units/angular_velocity.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
#include "wpi/units/velocity.hpp"
|
||||
@@ -104,7 +103,7 @@ class Drivetrain {
|
||||
wpi::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
|
||||
wpi::Field2d m_fieldSim;
|
||||
wpi::math::LinearSystem<2, 2, 2> m_drivetrainSystem =
|
||||
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
|
||||
wpi::math::Models::DifferentialDriveFromSysId(
|
||||
1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
|
||||
wpi::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
|
||||
m_drivetrainSystem, kTrackwidth, wpi::math::DCMotor::CIM(2), 8, 2_in};
|
||||
|
||||
@@ -4,16 +4,15 @@
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include "wpi/drive/DifferentialDrive.hpp"
|
||||
#include "wpi/driverstation/XboxController.hpp"
|
||||
#include "wpi/framework/TimedRobot.hpp"
|
||||
#include "wpi/hardware/motor/PWMSparkMax.hpp"
|
||||
#include "wpi/hardware/rotation/Encoder.hpp"
|
||||
#include "wpi/math/controller/LinearQuadraticRegulator.hpp"
|
||||
#include "wpi/math/estimator/KalmanFilter.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/LinearSystemLoop.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/math/trajectory/TrapezoidProfile.hpp"
|
||||
#include "wpi/units/angle.hpp"
|
||||
#include "wpi/units/moment_of_inertia.hpp"
|
||||
@@ -47,7 +46,7 @@ class Robot : public wpi::TimedRobot {
|
||||
// Inputs (what we can "put in"): [voltage], in volts.
|
||||
// Outputs (what we can measure): [position], in radians.
|
||||
wpi::math::LinearSystem<2, 1, 1> m_armPlant =
|
||||
wpi::math::LinearSystemId::SingleJointedArmSystem(
|
||||
wpi::math::Models::SingleJointedArmFromPhysicalConstants(
|
||||
wpi::math::DCMotor::NEO(2), kArmMOI, kArmGearing)
|
||||
.Slice(0);
|
||||
|
||||
|
||||
@@ -4,16 +4,15 @@
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include "wpi/drive/DifferentialDrive.hpp"
|
||||
#include "wpi/driverstation/XboxController.hpp"
|
||||
#include "wpi/framework/TimedRobot.hpp"
|
||||
#include "wpi/hardware/motor/PWMSparkMax.hpp"
|
||||
#include "wpi/hardware/rotation/Encoder.hpp"
|
||||
#include "wpi/math/controller/LinearQuadraticRegulator.hpp"
|
||||
#include "wpi/math/estimator/KalmanFilter.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/LinearSystemLoop.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/math/trajectory/TrapezoidProfile.hpp"
|
||||
#include "wpi/units/acceleration.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
@@ -44,7 +43,7 @@ class Robot : public wpi::TimedRobot {
|
||||
// Inputs (what we can "put in"): [voltage], in volts.
|
||||
// Outputs (what we can measure): [position], in meters.
|
||||
wpi::math::LinearSystem<2, 1, 1> m_elevatorPlant =
|
||||
wpi::math::LinearSystemId::ElevatorSystem(
|
||||
wpi::math::Models::ElevatorFromPhysicalConstants(
|
||||
wpi::math::DCMotor::NEO(2), kCarriageMass, kDrumRadius, kGearRatio)
|
||||
.Slice(0);
|
||||
|
||||
|
||||
@@ -4,17 +4,15 @@
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include "wpi/drive/DifferentialDrive.hpp"
|
||||
#include "wpi/driverstation/DriverStation.hpp"
|
||||
#include "wpi/driverstation/XboxController.hpp"
|
||||
#include "wpi/framework/TimedRobot.hpp"
|
||||
#include "wpi/hardware/motor/PWMSparkMax.hpp"
|
||||
#include "wpi/hardware/rotation/Encoder.hpp"
|
||||
#include "wpi/math/controller/LinearQuadraticRegulator.hpp"
|
||||
#include "wpi/math/estimator/KalmanFilter.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/LinearSystemLoop.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/units/angular_velocity.hpp"
|
||||
|
||||
/**
|
||||
@@ -42,9 +40,9 @@ class Robot : public wpi::TimedRobot {
|
||||
// Inputs (what we can "put in"): [voltage], in volts.
|
||||
// Outputs (what we can measure): [velocity], in radians per second.
|
||||
wpi::math::LinearSystem<1, 1, 1> m_flywheelPlant =
|
||||
wpi::math::LinearSystemId::FlywheelSystem(wpi::math::DCMotor::NEO(2),
|
||||
kFlywheelMomentOfInertia,
|
||||
kFlywheelGearing);
|
||||
wpi::math::Models::FlywheelFromPhysicalConstants(
|
||||
wpi::math::DCMotor::NEO(2), kFlywheelMomentOfInertia,
|
||||
kFlywheelGearing);
|
||||
|
||||
// The observer fuses our encoder data and voltage inputs to reject noise.
|
||||
wpi::math::KalmanFilter<1, 1, 1> m_observer{
|
||||
|
||||
@@ -4,8 +4,6 @@
|
||||
|
||||
#include <numbers>
|
||||
|
||||
#include "wpi/drive/DifferentialDrive.hpp"
|
||||
#include "wpi/driverstation/DriverStation.hpp"
|
||||
#include "wpi/driverstation/XboxController.hpp"
|
||||
#include "wpi/framework/TimedRobot.hpp"
|
||||
#include "wpi/hardware/motor/PWMSparkMax.hpp"
|
||||
@@ -13,8 +11,7 @@
|
||||
#include "wpi/math/controller/LinearQuadraticRegulator.hpp"
|
||||
#include "wpi/math/estimator/KalmanFilter.hpp"
|
||||
#include "wpi/math/system/LinearSystemLoop.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
|
||||
/**
|
||||
* This is a sample program to demonstrate how to use a state-space controller
|
||||
@@ -42,8 +39,7 @@ class Robot : public wpi::TimedRobot {
|
||||
//
|
||||
// The Kv and Ka constants are found using the FRC Characterization toolsuite.
|
||||
wpi::math::LinearSystem<1, 1, 1> m_flywheelPlant =
|
||||
wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::radian>(
|
||||
kFlywheelKv, kFlywheelKa);
|
||||
wpi::math::Models::FlywheelFromSysId(kFlywheelKv, kFlywheelKa);
|
||||
|
||||
// The observer fuses our encoder data and voltage inputs to reject noise.
|
||||
wpi::math::KalmanFilter<1, 1, 1> m_observer{
|
||||
|
||||
@@ -11,7 +11,6 @@
|
||||
#include "wpi/math/estimator/SwerveDrivePoseEstimator.hpp"
|
||||
#include "wpi/math/geometry/Translation2d.hpp"
|
||||
#include "wpi/math/kinematics/SwerveDriveKinematics.hpp"
|
||||
#include "wpi/math/kinematics/SwerveDriveOdometry.hpp"
|
||||
|
||||
/**
|
||||
* Represents a swerve drive style drivetrain.
|
||||
|
||||
@@ -10,7 +10,7 @@
|
||||
#include "Robot.hpp"
|
||||
#include "wpi/hal/DriverStationTypes.h"
|
||||
#include "wpi/hal/simulation/MockHooks.h"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/simulation/AnalogInputSim.hpp"
|
||||
#include "wpi/simulation/DriverStationSim.hpp"
|
||||
#include "wpi/simulation/ElevatorSim.hpp"
|
||||
|
||||
@@ -7,8 +7,8 @@ package org.wpilib.simulation;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.system.RobotController;
|
||||
|
||||
/** Represents a simulated DC motor mechanism. */
|
||||
@@ -26,10 +26,9 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
|
||||
* Creates a simulated DC motor mechanism.
|
||||
*
|
||||
* @param plant The linear system representing the DC motor. This system can be created with
|
||||
* {@link org.wpilib.math.system.plant.LinearSystemId#createDCMotorSystem(DCMotor, double,
|
||||
* double)} or {@link org.wpilib.math.system.plant.LinearSystemId#createDCMotorSystem(double,
|
||||
* double)}. If {@link org.wpilib.math.system.plant.LinearSystemId#createDCMotorSystem(double,
|
||||
* double)} is used, the distance unit must be radians.
|
||||
* {@link org.wpilib.math.system.Models#singleJointedArmFromPhysicalConstants(DCMotor, double,
|
||||
* double)} or {@link org.wpilib.math.system.Models#singleJointedArmFromSysId(double,
|
||||
* double)}.
|
||||
* @param gearbox The type of and number of motors in the DC motor gearbox.
|
||||
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
|
||||
* noise is desired. If present must have 2 elements. The first element is for position. The
|
||||
|
||||
@@ -12,10 +12,10 @@ import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.numbers.N7;
|
||||
import org.wpilib.math.random.Normal;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.system.NumericalIntegration;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.util.Nat;
|
||||
import org.wpilib.math.util.StateSpaceUtil;
|
||||
import org.wpilib.math.util.Units;
|
||||
@@ -77,7 +77,7 @@ public class DifferentialDrivetrainSim {
|
||||
double trackwidth,
|
||||
Matrix<N7, N1> measurementStdDevs) {
|
||||
this(
|
||||
LinearSystemId.createDrivetrainVelocitySystem(
|
||||
Models.differentialDriveFromPhysicalConstants(
|
||||
driveMotor, mass, wheelRadius, trackwidth / 2.0, j, gearing),
|
||||
driveMotor,
|
||||
gearing,
|
||||
@@ -91,10 +91,9 @@ public class DifferentialDrivetrainSim {
|
||||
*
|
||||
* @param plant The {@link LinearSystem} representing the robot's drivetrain. This system can be
|
||||
* created with {@link
|
||||
* org.wpilib.math.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor, double,
|
||||
* org.wpilib.math.system.Models#differentialDriveFromPhysicalConstants(DCMotor, double,
|
||||
* double, double, double, double)} or {@link
|
||||
* org.wpilib.math.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double,
|
||||
* double, double)}.
|
||||
* org.wpilib.math.system.Models#differentialDriveFromSysId(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.
|
||||
|
||||
@@ -8,10 +8,10 @@ import org.wpilib.math.linalg.Matrix;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.system.NumericalIntegration;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.system.RobotController;
|
||||
|
||||
/** Represents a simulated elevator mechanism. */
|
||||
@@ -32,8 +32,8 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
|
||||
* Creates a simulated elevator mechanism.
|
||||
*
|
||||
* @param plant The linear system that represents the elevator. This system can be created with
|
||||
* {@link org.wpilib.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double,
|
||||
* double, double)}.
|
||||
* {@link org.wpilib.math.system.Models#elevatorFromPhysicalConstants(DCMotor, double, double,
|
||||
* double)}.
|
||||
* @param gearbox The type of and number of motors in the elevator gearbox.
|
||||
* @param minHeight The min allowable height of the elevator in meters.
|
||||
* @param maxHeight The max allowable height of the elevator in meters.
|
||||
@@ -83,7 +83,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
|
||||
double startingHeight,
|
||||
double... measurementStdDevs) {
|
||||
this(
|
||||
LinearSystemId.identifyPositionSystem(kV, kA),
|
||||
Models.elevatorFromSysId(kV, kA),
|
||||
gearbox,
|
||||
minHeight,
|
||||
maxHeight,
|
||||
@@ -117,7 +117,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
|
||||
double startingHeight,
|
||||
double... measurementStdDevs) {
|
||||
this(
|
||||
LinearSystemId.createElevatorSystem(gearbox, carriageMass, drumRadius, gearing),
|
||||
Models.elevatorFromPhysicalConstants(gearbox, carriageMass, drumRadius, gearing),
|
||||
gearbox,
|
||||
minHeight,
|
||||
maxHeight,
|
||||
|
||||
@@ -6,9 +6,9 @@ package org.wpilib.simulation;
|
||||
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.system.RobotController;
|
||||
|
||||
/** Represents a simulated flywheel mechanism. */
|
||||
@@ -26,9 +26,8 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
|
||||
* Creates a simulated flywheel mechanism.
|
||||
*
|
||||
* @param plant The linear system that represents the flywheel. Use either {@link
|
||||
* LinearSystemId#createFlywheelSystem(DCMotor, double, double)} if using physical constants
|
||||
* or {@link LinearSystemId#identifyVelocitySystem(double, double)} if using system
|
||||
* characterization.
|
||||
* Models#flywheelFromPhysicalConstants(DCMotor, double, double)} if using physical constants
|
||||
* or {@link Models#flywheelFromSysId(double, double)} if using system characterization.
|
||||
* @param gearbox The type of and number of motors in the flywheel gearbox.
|
||||
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
|
||||
* noise is desired. If present must have 1 element for velocity.
|
||||
|
||||
@@ -8,10 +8,10 @@ import org.wpilib.math.linalg.Matrix;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.system.NumericalIntegration;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.system.RobotController;
|
||||
|
||||
/** Represents a simulated single jointed arm mechanism. */
|
||||
@@ -38,7 +38,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
|
||||
* Creates a simulated arm mechanism.
|
||||
*
|
||||
* @param plant The linear system that represents the arm. This system can be created with {@link
|
||||
* org.wpilib.math.system.plant.LinearSystemId#createSingleJointedArmSystem(DCMotor, double,
|
||||
* org.wpilib.math.system.Models#singleJointedArmFromPhysicalConstants(DCMotor, double,
|
||||
* double)}.
|
||||
* @param gearbox The type of and number of motors in the arm gearbox.
|
||||
* @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
|
||||
@@ -97,7 +97,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
|
||||
double startingAngleRads,
|
||||
double... measurementStdDevs) {
|
||||
this(
|
||||
LinearSystemId.createSingleJointedArmSystem(gearbox, j, gearing),
|
||||
Models.singleJointedArmFromPhysicalConstants(gearbox, j, gearing),
|
||||
gearbox,
|
||||
gearing,
|
||||
armLength,
|
||||
|
||||
@@ -12,9 +12,9 @@ import org.wpilib.hardware.rotation.Encoder;
|
||||
import org.wpilib.math.controller.PIDController;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.system.RobotController;
|
||||
|
||||
class DCMotorSimTest {
|
||||
@@ -23,7 +23,8 @@ class DCMotorSimTest {
|
||||
RoboRioSim.resetData();
|
||||
|
||||
DCMotor gearbox = DCMotor.getNEO(1);
|
||||
LinearSystem<N2, N1, N2> plant = LinearSystemId.createDCMotorSystem(gearbox, 0.0005, 1);
|
||||
LinearSystem<N2, N1, N2> plant =
|
||||
Models.singleJointedArmFromPhysicalConstants(gearbox, 0.0005, 1);
|
||||
DCMotorSim sim = new DCMotorSim(plant, gearbox);
|
||||
|
||||
try (var motor = new PWMVictorSPX(0);
|
||||
@@ -64,7 +65,8 @@ class DCMotorSimTest {
|
||||
RoboRioSim.resetData();
|
||||
|
||||
DCMotor gearbox = DCMotor.getNEO(1);
|
||||
LinearSystem<N2, N1, N2> plant = LinearSystemId.createDCMotorSystem(gearbox, 0.0005, 1);
|
||||
LinearSystem<N2, N1, N2> plant =
|
||||
Models.singleJointedArmFromPhysicalConstants(gearbox, 0.0005, 1);
|
||||
DCMotorSim sim = new DCMotorSim(plant, gearbox);
|
||||
|
||||
try (var motor = new PWMVictorSPX(0);
|
||||
|
||||
@@ -19,9 +19,9 @@ import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.linalg.Vector;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N7;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.system.NumericalIntegration;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.trajectory.TrajectoryConfig;
|
||||
import org.wpilib.math.trajectory.TrajectoryGenerator;
|
||||
import org.wpilib.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
|
||||
@@ -33,7 +33,7 @@ class DifferentialDrivetrainSimTest {
|
||||
void testConvergence() {
|
||||
var motor = DCMotor.getNEO(2);
|
||||
var plant =
|
||||
LinearSystemId.createDrivetrainVelocitySystem(
|
||||
Models.differentialDriveFromPhysicalConstants(
|
||||
motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5, 1.0);
|
||||
|
||||
var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
|
||||
@@ -97,7 +97,7 @@ class DifferentialDrivetrainSimTest {
|
||||
void testCurrent() {
|
||||
var motor = DCMotor.getNEO(2);
|
||||
var plant =
|
||||
LinearSystemId.createDrivetrainVelocitySystem(
|
||||
Models.differentialDriveFromPhysicalConstants(
|
||||
motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5, 1.0);
|
||||
var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
|
||||
var sim =
|
||||
@@ -127,7 +127,7 @@ class DifferentialDrivetrainSimTest {
|
||||
void testModelStability() {
|
||||
var motor = DCMotor.getNEO(2);
|
||||
var plant =
|
||||
LinearSystemId.createDrivetrainVelocitySystem(
|
||||
Models.differentialDriveFromPhysicalConstants(
|
||||
motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 2.0, 5.0);
|
||||
|
||||
var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
|
||||
|
||||
@@ -12,8 +12,8 @@ import org.wpilib.hardware.motor.PWMVictorSPX;
|
||||
import org.wpilib.hardware.rotation.Encoder;
|
||||
import org.wpilib.math.controller.PIDController;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.util.Units;
|
||||
import org.wpilib.system.RobotController;
|
||||
|
||||
@@ -117,7 +117,7 @@ class ElevatorSimTest {
|
||||
}
|
||||
|
||||
var system =
|
||||
LinearSystemId.createElevatorSystem(
|
||||
Models.elevatorFromPhysicalConstants(
|
||||
DCMotor.getVex775Pro(4), 4, Units.inchesToMeters(0.5), 100);
|
||||
assertEquals(
|
||||
system.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0),
|
||||
|
||||
@@ -8,7 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.util.Units;
|
||||
|
||||
class SingleJointedArmSimTest {
|
||||
|
||||
@@ -8,7 +8,7 @@ import org.wpilib.examples.armsimulation.Constants;
|
||||
import org.wpilib.hardware.motor.PWMSparkMax;
|
||||
import org.wpilib.hardware.rotation.Encoder;
|
||||
import org.wpilib.math.controller.PIDController;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.util.Units;
|
||||
import org.wpilib.simulation.BatterySim;
|
||||
import org.wpilib.simulation.EncoderSim;
|
||||
|
||||
@@ -21,9 +21,9 @@ import org.wpilib.math.kinematics.DifferentialDriveKinematics;
|
||||
import org.wpilib.math.kinematics.DifferentialDriveWheelSpeeds;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.util.ComputerVisionUtil;
|
||||
import org.wpilib.math.util.Units;
|
||||
import org.wpilib.networktables.DoubleArrayEntry;
|
||||
@@ -93,7 +93,7 @@ public class Drivetrain {
|
||||
private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder);
|
||||
private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
|
||||
private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
|
||||
LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
|
||||
Models.differentialDriveFromSysId(1.98, 0.2, 1.5, 0.3);
|
||||
private final DifferentialDrivetrainSim m_drivetrainSimulator =
|
||||
new DifferentialDrivetrainSim(
|
||||
m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null);
|
||||
|
||||
@@ -9,7 +9,7 @@ import org.wpilib.hardware.motor.PWMSparkMax;
|
||||
import org.wpilib.hardware.rotation.Encoder;
|
||||
import org.wpilib.math.controller.ElevatorFeedforward;
|
||||
import org.wpilib.math.controller.PIDController;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.trajectory.ExponentialProfile;
|
||||
import org.wpilib.math.util.Units;
|
||||
import org.wpilib.simulation.BatterySim;
|
||||
|
||||
@@ -9,7 +9,7 @@ import org.wpilib.hardware.motor.PWMSparkMax;
|
||||
import org.wpilib.hardware.rotation.Encoder;
|
||||
import org.wpilib.math.controller.ElevatorFeedforward;
|
||||
import org.wpilib.math.controller.ProfiledPIDController;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.trajectory.TrapezoidProfile;
|
||||
import org.wpilib.simulation.BatterySim;
|
||||
import org.wpilib.simulation.ElevatorSim;
|
||||
|
||||
@@ -11,9 +11,9 @@ import org.wpilib.hardware.rotation.Encoder;
|
||||
import org.wpilib.math.controller.BangBangController;
|
||||
import org.wpilib.math.controller.SimpleMotorFeedforward;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.util.Units;
|
||||
import org.wpilib.simulation.EncoderSim;
|
||||
import org.wpilib.simulation.FlywheelSim;
|
||||
@@ -60,7 +60,7 @@ public class Robot extends TimedRobot {
|
||||
private final DCMotor m_gearbox = DCMotor.getNEO(1);
|
||||
|
||||
private final LinearSystem<N1, N1, N1> m_plant =
|
||||
LinearSystemId.createFlywheelSystem(m_gearbox, kFlywheelGearing, kFlywheelMomentOfInertia);
|
||||
Models.flywheelFromPhysicalConstants(m_gearbox, kFlywheelGearing, kFlywheelMomentOfInertia);
|
||||
|
||||
private final FlywheelSim m_flywheelSim = new FlywheelSim(m_plant, m_gearbox);
|
||||
private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
|
||||
|
||||
@@ -15,9 +15,9 @@ import org.wpilib.math.kinematics.DifferentialDriveKinematics;
|
||||
import org.wpilib.math.kinematics.DifferentialDriveOdometry;
|
||||
import org.wpilib.math.kinematics.DifferentialDriveWheelSpeeds;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.simulation.DifferentialDrivetrainSim;
|
||||
import org.wpilib.simulation.EncoderSim;
|
||||
import org.wpilib.smartdashboard.Field2d;
|
||||
@@ -62,7 +62,7 @@ public class Drivetrain {
|
||||
private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
|
||||
private final Field2d m_fieldSim = new Field2d();
|
||||
private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
|
||||
LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
|
||||
Models.differentialDriveFromSysId(1.98, 0.2, 1.5, 0.3);
|
||||
private final DifferentialDrivetrainSim m_drivetrainSimulator =
|
||||
new DifferentialDrivetrainSim(
|
||||
m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null);
|
||||
|
||||
@@ -13,10 +13,10 @@ import org.wpilib.math.estimator.KalmanFilter;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.LinearSystemLoop;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.trajectory.TrapezoidProfile;
|
||||
import org.wpilib.math.util.Nat;
|
||||
import org.wpilib.math.util.Units;
|
||||
@@ -53,7 +53,7 @@ public class Robot extends TimedRobot {
|
||||
// Inputs (what we can "put in"): [voltage], in volts.
|
||||
// Outputs (what we can measure): [position], in radians.
|
||||
private final LinearSystem<N2, N1, N2> m_armPlant =
|
||||
LinearSystemId.createSingleJointedArmSystem(DCMotor.getNEO(2), kArmMOI, kArmGearing);
|
||||
Models.singleJointedArmFromPhysicalConstants(DCMotor.getNEO(2), kArmMOI, kArmGearing);
|
||||
|
||||
// The observer fuses our encoder data and voltage inputs to reject noise.
|
||||
@SuppressWarnings("unchecked")
|
||||
|
||||
@@ -13,10 +13,10 @@ import org.wpilib.math.estimator.KalmanFilter;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.LinearSystemLoop;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.trajectory.TrapezoidProfile;
|
||||
import org.wpilib.math.util.Nat;
|
||||
import org.wpilib.math.util.Units;
|
||||
@@ -58,7 +58,7 @@ public class Robot extends TimedRobot {
|
||||
This elevator is driven by two NEO motors.
|
||||
*/
|
||||
private final LinearSystem<N2, N1, N2> m_elevatorPlant =
|
||||
LinearSystemId.createElevatorSystem(
|
||||
Models.elevatorFromPhysicalConstants(
|
||||
DCMotor.getNEO(2), kCarriageMass, kDrumRadius, kElevatorGearing);
|
||||
|
||||
// The observer fuses our encoder data and voltage inputs to reject noise.
|
||||
|
||||
@@ -12,10 +12,10 @@ import org.wpilib.math.controller.LinearQuadraticRegulator;
|
||||
import org.wpilib.math.estimator.KalmanFilter;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.LinearSystemLoop;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.util.Nat;
|
||||
import org.wpilib.math.util.Units;
|
||||
|
||||
@@ -42,7 +42,7 @@ public class Robot extends TimedRobot {
|
||||
// Inputs (what we can "put in"): [voltage], in volts.
|
||||
// Outputs (what we can measure): [velocity], in radians per second.
|
||||
private final LinearSystem<N1, N1, N1> m_flywheelPlant =
|
||||
LinearSystemId.createFlywheelSystem(
|
||||
Models.flywheelFromPhysicalConstants(
|
||||
DCMotor.getNEO(2), kFlywheelMomentOfInertia, kFlywheelGearing);
|
||||
|
||||
// The observer fuses our encoder data and voltage inputs to reject noise.
|
||||
|
||||
@@ -14,7 +14,7 @@ import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.LinearSystemLoop;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.util.Nat;
|
||||
import org.wpilib.math.util.Units;
|
||||
|
||||
@@ -43,7 +43,7 @@ public class Robot extends TimedRobot {
|
||||
//
|
||||
// The Kv and Ka constants are found using the FRC Characterization toolsuite.
|
||||
private final LinearSystem<N1, N1, N1> m_flywheelPlant =
|
||||
LinearSystemId.identifyVelocitySystem(kFlywheelKv, kFlywheelKa);
|
||||
Models.flywheelFromSysId(kFlywheelKv, kFlywheelKa);
|
||||
|
||||
// The observer fuses our encoder data and voltage inputs to reject noise.
|
||||
private final KalmanFilter<N1, N1, N1> m_observer =
|
||||
|
||||
@@ -14,7 +14,7 @@ import org.junit.jupiter.api.parallel.ResourceLock;
|
||||
import org.wpilib.hardware.hal.HAL;
|
||||
import org.wpilib.hardware.hal.HAL.SimPeriodicBeforeCallback;
|
||||
import org.wpilib.hardware.hal.RobotMode;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.util.Units;
|
||||
import org.wpilib.simulation.AnalogInputSim;
|
||||
import org.wpilib.simulation.DriverStationSim;
|
||||
|
||||
28
wpimath/robotpy_pybind_build_info.bzl
generated
28
wpimath/robotpy_pybind_build_info.bzl
generated
@@ -889,6 +889,16 @@ def wpimath_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu
|
||||
("wpi::math::SplineParameterizer", "wpi__math__SplineParameterizer.hpp"),
|
||||
],
|
||||
),
|
||||
struct(
|
||||
class_name = "DCMotor",
|
||||
yml_file = "semiwrap/DCMotor.yml",
|
||||
header_root = "$(execpath :robotpy-native-wpimath.copy_headers)",
|
||||
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/system/DCMotor.hpp",
|
||||
tmpl_class_names = [],
|
||||
trampolines = [
|
||||
("wpi::math::DCMotor", "wpi__math__DCMotor.hpp"),
|
||||
],
|
||||
),
|
||||
struct(
|
||||
class_name = "LinearSystem",
|
||||
yml_file = "semiwrap/LinearSystem.yml",
|
||||
@@ -929,23 +939,13 @@ def wpimath_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu
|
||||
],
|
||||
),
|
||||
struct(
|
||||
class_name = "DCMotor",
|
||||
yml_file = "semiwrap/DCMotor.yml",
|
||||
class_name = "Models",
|
||||
yml_file = "semiwrap/Models.yml",
|
||||
header_root = "$(execpath :robotpy-native-wpimath.copy_headers)",
|
||||
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/system/plant/DCMotor.hpp",
|
||||
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/system/Models.hpp",
|
||||
tmpl_class_names = [],
|
||||
trampolines = [
|
||||
("wpi::math::DCMotor", "wpi__math__DCMotor.hpp"),
|
||||
],
|
||||
),
|
||||
struct(
|
||||
class_name = "LinearSystemId",
|
||||
yml_file = "semiwrap/LinearSystemId.yml",
|
||||
header_root = "$(execpath :robotpy-native-wpimath.copy_headers)",
|
||||
header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/system/plant/LinearSystemId.hpp",
|
||||
tmpl_class_names = [],
|
||||
trampolines = [
|
||||
("wpi::math::LinearSystemId", "wpi__math__LinearSystemId.hpp"),
|
||||
("wpi::math::Models", "wpi__math__Models.hpp"),
|
||||
],
|
||||
),
|
||||
struct(
|
||||
|
||||
@@ -1,621 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
// Code generated by protocol buffer compiler. Do not edit!
|
||||
package org.wpilib.math.proto;
|
||||
|
||||
import java.io.IOException;
|
||||
import us.hebi.quickbuf.Descriptors;
|
||||
import us.hebi.quickbuf.FieldName;
|
||||
import us.hebi.quickbuf.InvalidProtocolBufferException;
|
||||
import us.hebi.quickbuf.JsonSink;
|
||||
import us.hebi.quickbuf.JsonSource;
|
||||
import us.hebi.quickbuf.MessageFactory;
|
||||
import us.hebi.quickbuf.ProtoMessage;
|
||||
import us.hebi.quickbuf.ProtoSink;
|
||||
import us.hebi.quickbuf.ProtoSource;
|
||||
import us.hebi.quickbuf.ProtoUtil;
|
||||
import us.hebi.quickbuf.RepeatedByte;
|
||||
|
||||
public final class Plant {
|
||||
private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(607,
|
||||
"CgtwbGFudC5wcm90bxIJd3BpLnByb3RvIsQBCg9Qcm90b2J1ZkRDTW90b3ISJwoPbm9taW5hbF92b2x0" +
|
||||
"YWdlGAEgASgBUg5ub21pbmFsVm9sdGFnZRIhCgxzdGFsbF90b3JxdWUYAiABKAFSC3N0YWxsVG9ycXVl" +
|
||||
"EiMKDXN0YWxsX2N1cnJlbnQYAyABKAFSDHN0YWxsQ3VycmVudBIhCgxmcmVlX2N1cnJlbnQYBCABKAFS" +
|
||||
"C2ZyZWVDdXJyZW50Eh0KCmZyZWVfc3BlZWQYBSABKAFSCWZyZWVTcGVlZEIXChVvcmcud3BpbGliLm1h" +
|
||||
"dGgucHJvdG9K3AIKBhIEAAAMAQoICgEMEgMAABIKCAoBAhIDAgASCggKAQgSAwQALgoJCgIIARIDBAAu" +
|
||||
"CgoKAgQAEgQGAAwBCgoKAwQAARIDBggXCgsKBAQAAgASAwcCHQoMCgUEAAIABRIDBwIICgwKBQQAAgAB" +
|
||||
"EgMHCRgKDAoFBAACAAMSAwcbHAoLCgQEAAIBEgMIAhoKDAoFBAACAQUSAwgCCAoMCgUEAAIBARIDCAkV" +
|
||||
"CgwKBQQAAgEDEgMIGBkKCwoEBAACAhIDCQIbCgwKBQQAAgIFEgMJAggKDAoFBAACAgESAwkJFgoMCgUE" +
|
||||
"AAICAxIDCRkaCgsKBAQAAgMSAwoCGgoMCgUEAAIDBRIDCgIICgwKBQQAAgMBEgMKCRUKDAoFBAACAwMS" +
|
||||
"AwoYGQoLCgQEAAIEEgMLAhgKDAoFBAACBAUSAwsCCAoMCgUEAAIEARIDCwkTCgwKBQQAAgQDEgMLFhdi" +
|
||||
"BnByb3RvMw==");
|
||||
|
||||
static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("plant.proto", "wpi.proto", descriptorData);
|
||||
|
||||
static final Descriptors.Descriptor wpi_proto_ProtobufDCMotor_descriptor = descriptor.internalContainedType(27, 196, "ProtobufDCMotor", "wpi.proto.ProtobufDCMotor");
|
||||
|
||||
/**
|
||||
* @return this proto file's descriptor.
|
||||
*/
|
||||
public static Descriptors.FileDescriptor getDescriptor() {
|
||||
return descriptor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Protobuf type {@code ProtobufDCMotor}
|
||||
*/
|
||||
public static final class ProtobufDCMotor extends ProtoMessage<ProtobufDCMotor> implements Cloneable {
|
||||
private static final long serialVersionUID = 0L;
|
||||
|
||||
/**
|
||||
* <code>optional double nominal_voltage = 1;</code>
|
||||
*/
|
||||
private double nominalVoltage;
|
||||
|
||||
/**
|
||||
* <code>optional double stall_torque = 2;</code>
|
||||
*/
|
||||
private double stallTorque;
|
||||
|
||||
/**
|
||||
* <code>optional double stall_current = 3;</code>
|
||||
*/
|
||||
private double stallCurrent;
|
||||
|
||||
/**
|
||||
* <code>optional double free_current = 4;</code>
|
||||
*/
|
||||
private double freeCurrent;
|
||||
|
||||
/**
|
||||
* <code>optional double free_speed = 5;</code>
|
||||
*/
|
||||
private double freeSpeed;
|
||||
|
||||
private ProtobufDCMotor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @return a new empty instance of {@code ProtobufDCMotor}
|
||||
*/
|
||||
public static ProtobufDCMotor newInstance() {
|
||||
return new ProtobufDCMotor();
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double nominal_voltage = 1;</code>
|
||||
* @return whether the nominalVoltage field is set
|
||||
*/
|
||||
public boolean hasNominalVoltage() {
|
||||
return (bitField0_ & 0x00000001) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double nominal_voltage = 1;</code>
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor clearNominalVoltage() {
|
||||
bitField0_ &= ~0x00000001;
|
||||
nominalVoltage = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double nominal_voltage = 1;</code>
|
||||
* @return the nominalVoltage
|
||||
*/
|
||||
public double getNominalVoltage() {
|
||||
return nominalVoltage;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double nominal_voltage = 1;</code>
|
||||
* @param value the nominalVoltage to set
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor setNominalVoltage(final double value) {
|
||||
bitField0_ |= 0x00000001;
|
||||
nominalVoltage = value;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_torque = 2;</code>
|
||||
* @return whether the stallTorque field is set
|
||||
*/
|
||||
public boolean hasStallTorque() {
|
||||
return (bitField0_ & 0x00000002) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_torque = 2;</code>
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor clearStallTorque() {
|
||||
bitField0_ &= ~0x00000002;
|
||||
stallTorque = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_torque = 2;</code>
|
||||
* @return the stallTorque
|
||||
*/
|
||||
public double getStallTorque() {
|
||||
return stallTorque;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_torque = 2;</code>
|
||||
* @param value the stallTorque to set
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor setStallTorque(final double value) {
|
||||
bitField0_ |= 0x00000002;
|
||||
stallTorque = value;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_current = 3;</code>
|
||||
* @return whether the stallCurrent field is set
|
||||
*/
|
||||
public boolean hasStallCurrent() {
|
||||
return (bitField0_ & 0x00000004) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_current = 3;</code>
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor clearStallCurrent() {
|
||||
bitField0_ &= ~0x00000004;
|
||||
stallCurrent = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_current = 3;</code>
|
||||
* @return the stallCurrent
|
||||
*/
|
||||
public double getStallCurrent() {
|
||||
return stallCurrent;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_current = 3;</code>
|
||||
* @param value the stallCurrent to set
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor setStallCurrent(final double value) {
|
||||
bitField0_ |= 0x00000004;
|
||||
stallCurrent = value;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_current = 4;</code>
|
||||
* @return whether the freeCurrent field is set
|
||||
*/
|
||||
public boolean hasFreeCurrent() {
|
||||
return (bitField0_ & 0x00000008) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_current = 4;</code>
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor clearFreeCurrent() {
|
||||
bitField0_ &= ~0x00000008;
|
||||
freeCurrent = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_current = 4;</code>
|
||||
* @return the freeCurrent
|
||||
*/
|
||||
public double getFreeCurrent() {
|
||||
return freeCurrent;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_current = 4;</code>
|
||||
* @param value the freeCurrent to set
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor setFreeCurrent(final double value) {
|
||||
bitField0_ |= 0x00000008;
|
||||
freeCurrent = value;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_speed = 5;</code>
|
||||
* @return whether the freeSpeed field is set
|
||||
*/
|
||||
public boolean hasFreeSpeed() {
|
||||
return (bitField0_ & 0x00000010) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_speed = 5;</code>
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor clearFreeSpeed() {
|
||||
bitField0_ &= ~0x00000010;
|
||||
freeSpeed = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_speed = 5;</code>
|
||||
* @return the freeSpeed
|
||||
*/
|
||||
public double getFreeSpeed() {
|
||||
return freeSpeed;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_speed = 5;</code>
|
||||
* @param value the freeSpeed to set
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor setFreeSpeed(final double value) {
|
||||
bitField0_ |= 0x00000010;
|
||||
freeSpeed = value;
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor copyFrom(final ProtobufDCMotor other) {
|
||||
cachedSize = other.cachedSize;
|
||||
if ((bitField0_ | other.bitField0_) != 0) {
|
||||
bitField0_ = other.bitField0_;
|
||||
nominalVoltage = other.nominalVoltage;
|
||||
stallTorque = other.stallTorque;
|
||||
stallCurrent = other.stallCurrent;
|
||||
freeCurrent = other.freeCurrent;
|
||||
freeSpeed = other.freeSpeed;
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor mergeFrom(final ProtobufDCMotor other) {
|
||||
if (other.isEmpty()) {
|
||||
return this;
|
||||
}
|
||||
cachedSize = -1;
|
||||
if (other.hasNominalVoltage()) {
|
||||
setNominalVoltage(other.nominalVoltage);
|
||||
}
|
||||
if (other.hasStallTorque()) {
|
||||
setStallTorque(other.stallTorque);
|
||||
}
|
||||
if (other.hasStallCurrent()) {
|
||||
setStallCurrent(other.stallCurrent);
|
||||
}
|
||||
if (other.hasFreeCurrent()) {
|
||||
setFreeCurrent(other.freeCurrent);
|
||||
}
|
||||
if (other.hasFreeSpeed()) {
|
||||
setFreeSpeed(other.freeSpeed);
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor clear() {
|
||||
if (isEmpty()) {
|
||||
return this;
|
||||
}
|
||||
cachedSize = -1;
|
||||
bitField0_ = 0;
|
||||
nominalVoltage = 0D;
|
||||
stallTorque = 0D;
|
||||
stallCurrent = 0D;
|
||||
freeCurrent = 0D;
|
||||
freeSpeed = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor clearQuick() {
|
||||
if (isEmpty()) {
|
||||
return this;
|
||||
}
|
||||
cachedSize = -1;
|
||||
bitField0_ = 0;
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean equals(Object o) {
|
||||
if (o == this) {
|
||||
return true;
|
||||
}
|
||||
if (!(o instanceof ProtobufDCMotor)) {
|
||||
return false;
|
||||
}
|
||||
ProtobufDCMotor other = (ProtobufDCMotor) o;
|
||||
return bitField0_ == other.bitField0_
|
||||
&& (!hasNominalVoltage() || ProtoUtil.isEqual(nominalVoltage, other.nominalVoltage))
|
||||
&& (!hasStallTorque() || ProtoUtil.isEqual(stallTorque, other.stallTorque))
|
||||
&& (!hasStallCurrent() || ProtoUtil.isEqual(stallCurrent, other.stallCurrent))
|
||||
&& (!hasFreeCurrent() || ProtoUtil.isEqual(freeCurrent, other.freeCurrent))
|
||||
&& (!hasFreeSpeed() || ProtoUtil.isEqual(freeSpeed, other.freeSpeed));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void writeTo(final ProtoSink output) throws IOException {
|
||||
if ((bitField0_ & 0x00000001) != 0) {
|
||||
output.writeRawByte((byte) 9);
|
||||
output.writeDoubleNoTag(nominalVoltage);
|
||||
}
|
||||
if ((bitField0_ & 0x00000002) != 0) {
|
||||
output.writeRawByte((byte) 17);
|
||||
output.writeDoubleNoTag(stallTorque);
|
||||
}
|
||||
if ((bitField0_ & 0x00000004) != 0) {
|
||||
output.writeRawByte((byte) 25);
|
||||
output.writeDoubleNoTag(stallCurrent);
|
||||
}
|
||||
if ((bitField0_ & 0x00000008) != 0) {
|
||||
output.writeRawByte((byte) 33);
|
||||
output.writeDoubleNoTag(freeCurrent);
|
||||
}
|
||||
if ((bitField0_ & 0x00000010) != 0) {
|
||||
output.writeRawByte((byte) 41);
|
||||
output.writeDoubleNoTag(freeSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
protected int computeSerializedSize() {
|
||||
int size = 0;
|
||||
if ((bitField0_ & 0x00000001) != 0) {
|
||||
size += 9;
|
||||
}
|
||||
if ((bitField0_ & 0x00000002) != 0) {
|
||||
size += 9;
|
||||
}
|
||||
if ((bitField0_ & 0x00000004) != 0) {
|
||||
size += 9;
|
||||
}
|
||||
if ((bitField0_ & 0x00000008) != 0) {
|
||||
size += 9;
|
||||
}
|
||||
if ((bitField0_ & 0x00000010) != 0) {
|
||||
size += 9;
|
||||
}
|
||||
return size;
|
||||
}
|
||||
|
||||
@Override
|
||||
@SuppressWarnings("fallthrough")
|
||||
public ProtobufDCMotor mergeFrom(final ProtoSource input) throws IOException {
|
||||
// Enabled Fall-Through Optimization (QuickBuffers)
|
||||
int tag = input.readTag();
|
||||
while (true) {
|
||||
switch (tag) {
|
||||
case 9: {
|
||||
// nominalVoltage
|
||||
nominalVoltage = input.readDouble();
|
||||
bitField0_ |= 0x00000001;
|
||||
tag = input.readTag();
|
||||
if (tag != 17) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
case 17: {
|
||||
// stallTorque
|
||||
stallTorque = input.readDouble();
|
||||
bitField0_ |= 0x00000002;
|
||||
tag = input.readTag();
|
||||
if (tag != 25) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
case 25: {
|
||||
// stallCurrent
|
||||
stallCurrent = input.readDouble();
|
||||
bitField0_ |= 0x00000004;
|
||||
tag = input.readTag();
|
||||
if (tag != 33) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
case 33: {
|
||||
// freeCurrent
|
||||
freeCurrent = input.readDouble();
|
||||
bitField0_ |= 0x00000008;
|
||||
tag = input.readTag();
|
||||
if (tag != 41) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
case 41: {
|
||||
// freeSpeed
|
||||
freeSpeed = input.readDouble();
|
||||
bitField0_ |= 0x00000010;
|
||||
tag = input.readTag();
|
||||
if (tag != 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
case 0: {
|
||||
return this;
|
||||
}
|
||||
default: {
|
||||
if (!input.skipField(tag)) {
|
||||
return this;
|
||||
}
|
||||
tag = input.readTag();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void writeTo(final JsonSink output) throws IOException {
|
||||
output.beginObject();
|
||||
if ((bitField0_ & 0x00000001) != 0) {
|
||||
output.writeDouble(FieldNames.nominalVoltage, nominalVoltage);
|
||||
}
|
||||
if ((bitField0_ & 0x00000002) != 0) {
|
||||
output.writeDouble(FieldNames.stallTorque, stallTorque);
|
||||
}
|
||||
if ((bitField0_ & 0x00000004) != 0) {
|
||||
output.writeDouble(FieldNames.stallCurrent, stallCurrent);
|
||||
}
|
||||
if ((bitField0_ & 0x00000008) != 0) {
|
||||
output.writeDouble(FieldNames.freeCurrent, freeCurrent);
|
||||
}
|
||||
if ((bitField0_ & 0x00000010) != 0) {
|
||||
output.writeDouble(FieldNames.freeSpeed, freeSpeed);
|
||||
}
|
||||
output.endObject();
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor mergeFrom(final JsonSource input) throws IOException {
|
||||
if (!input.beginObject()) {
|
||||
return this;
|
||||
}
|
||||
while (!input.isAtEnd()) {
|
||||
switch (input.readFieldHash()) {
|
||||
case 1374862050:
|
||||
case 173092603: {
|
||||
if (input.isAtField(FieldNames.nominalVoltage)) {
|
||||
if (!input.trySkipNullValue()) {
|
||||
nominalVoltage = input.readDouble();
|
||||
bitField0_ |= 0x00000001;
|
||||
}
|
||||
} else {
|
||||
input.skipUnknownField();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 2075810250:
|
||||
case 1238615945: {
|
||||
if (input.isAtField(FieldNames.stallTorque)) {
|
||||
if (!input.trySkipNullValue()) {
|
||||
stallTorque = input.readDouble();
|
||||
bitField0_ |= 0x00000002;
|
||||
}
|
||||
} else {
|
||||
input.skipUnknownField();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case -2105262663:
|
||||
case 2006484954: {
|
||||
if (input.isAtField(FieldNames.stallCurrent)) {
|
||||
if (!input.trySkipNullValue()) {
|
||||
stallCurrent = input.readDouble();
|
||||
bitField0_ |= 0x00000004;
|
||||
}
|
||||
} else {
|
||||
input.skipUnknownField();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 1024355693:
|
||||
case 240406182: {
|
||||
if (input.isAtField(FieldNames.freeCurrent)) {
|
||||
if (!input.trySkipNullValue()) {
|
||||
freeCurrent = input.readDouble();
|
||||
bitField0_ |= 0x00000008;
|
||||
}
|
||||
} else {
|
||||
input.skipUnknownField();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case -444654277:
|
||||
case -552732492: {
|
||||
if (input.isAtField(FieldNames.freeSpeed)) {
|
||||
if (!input.trySkipNullValue()) {
|
||||
freeSpeed = input.readDouble();
|
||||
bitField0_ |= 0x00000010;
|
||||
}
|
||||
} else {
|
||||
input.skipUnknownField();
|
||||
}
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
input.skipUnknownField();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
input.endObject();
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor clone() {
|
||||
return new ProtobufDCMotor().copyFrom(this);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isEmpty() {
|
||||
return ((bitField0_) == 0);
|
||||
}
|
||||
|
||||
public static ProtobufDCMotor parseFrom(final byte[] data) throws
|
||||
InvalidProtocolBufferException {
|
||||
return ProtoMessage.mergeFrom(new ProtobufDCMotor(), data).checkInitialized();
|
||||
}
|
||||
|
||||
public static ProtobufDCMotor parseFrom(final ProtoSource input) throws IOException {
|
||||
return ProtoMessage.mergeFrom(new ProtobufDCMotor(), input).checkInitialized();
|
||||
}
|
||||
|
||||
public static ProtobufDCMotor parseFrom(final JsonSource input) throws IOException {
|
||||
return ProtoMessage.mergeFrom(new ProtobufDCMotor(), input).checkInitialized();
|
||||
}
|
||||
|
||||
/**
|
||||
* @return factory for creating ProtobufDCMotor messages
|
||||
*/
|
||||
public static MessageFactory<ProtobufDCMotor> getFactory() {
|
||||
return ProtobufDCMotorFactory.INSTANCE;
|
||||
}
|
||||
|
||||
/**
|
||||
* @return this type's descriptor.
|
||||
*/
|
||||
public static Descriptors.Descriptor getDescriptor() {
|
||||
return Plant.wpi_proto_ProtobufDCMotor_descriptor;
|
||||
}
|
||||
|
||||
private enum ProtobufDCMotorFactory implements MessageFactory<ProtobufDCMotor> {
|
||||
INSTANCE;
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor create() {
|
||||
return ProtobufDCMotor.newInstance();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Contains name constants used for serializing JSON
|
||||
*/
|
||||
static class FieldNames {
|
||||
static final FieldName nominalVoltage = FieldName.forField("nominalVoltage", "nominal_voltage");
|
||||
|
||||
static final FieldName stallTorque = FieldName.forField("stallTorque", "stall_torque");
|
||||
|
||||
static final FieldName stallCurrent = FieldName.forField("stallCurrent", "stall_current");
|
||||
|
||||
static final FieldName freeCurrent = FieldName.forField("freeCurrent", "free_current");
|
||||
|
||||
static final FieldName freeSpeed = FieldName.forField("freeSpeed", "free_speed");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -18,25 +18,36 @@ import us.hebi.quickbuf.ProtoUtil;
|
||||
import us.hebi.quickbuf.RepeatedByte;
|
||||
|
||||
public final class System {
|
||||
private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(829,
|
||||
"CgxzeXN0ZW0ucHJvdG8SCXdwaS5wcm90bxoNd3BpbWF0aC5wcm90byKZAgoUUHJvdG9idWZMaW5lYXJT" +
|
||||
"eXN0ZW0SHQoKbnVtX3N0YXRlcxgBIAEoDVIJbnVtU3RhdGVzEh0KCm51bV9pbnB1dHMYAiABKA1SCW51" +
|
||||
"bUlucHV0cxIfCgtudW1fb3V0cHV0cxgDIAEoDVIKbnVtT3V0cHV0cxInCgFhGAQgASgLMhkud3BpLnBy" +
|
||||
"b3RvLlByb3RvYnVmTWF0cml4UgFhEicKAWIYBSABKAsyGS53cGkucHJvdG8uUHJvdG9idWZNYXRyaXhS" +
|
||||
"AWISJwoBYxgGIAEoCzIZLndwaS5wcm90by5Qcm90b2J1Zk1hdHJpeFIBYxInCgFkGAcgASgLMhkud3Bp" +
|
||||
"LnByb3RvLlByb3RvYnVmTWF0cml4UgFkQhcKFW9yZy53cGlsaWIubWF0aC5wcm90b0rVAwoGEgQAABAB" +
|
||||
"CggKAQwSAwAAEgoICgECEgMCABIKCQoCAwASAwQAFwoICgEIEgMGAC4KCQoCCAESAwYALgoKCgIEABIE" +
|
||||
"CAAQAQoKCgMEAAESAwgIHAoLCgQEAAIAEgMJAhgKDAoFBAACAAUSAwkCCAoMCgUEAAIAARIDCQkTCgwK" +
|
||||
"BQQAAgADEgMJFhcKCwoEBAACARIDCgIYCgwKBQQAAgEFEgMKAggKDAoFBAACAQESAwoJEwoMCgUEAAIB" +
|
||||
"AxIDChYXCgsKBAQAAgISAwsCGQoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCRQKDAoFBAACAgMSAwsX" +
|
||||
"GAoLCgQEAAIDEgMMAhcKDAoFBAACAwYSAwwCEAoMCgUEAAIDARIDDBESCgwKBQQAAgMDEgMMFRYKCwoE" +
|
||||
"BAACBBIDDQIXCgwKBQQAAgQGEgMNAhAKDAoFBAACBAESAw0REgoMCgUEAAIEAxIDDRUWCgsKBAQAAgUS" +
|
||||
"Aw4CFwoMCgUEAAIFBhIDDgIQCgwKBQQAAgUBEgMOERIKDAoFBAACBQMSAw4VFgoLCgQEAAIGEgMPAhcK" +
|
||||
"DAoFBAACBgYSAw8CEAoMCgUEAAIGARIDDxESCgwKBQQAAgYDEgMPFRZiBnByb3RvMw==");
|
||||
private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1327,
|
||||
"CgxzeXN0ZW0ucHJvdG8SCXdwaS5wcm90bxoNd3BpbWF0aC5wcm90byLEAQoPUHJvdG9idWZEQ01vdG9y" +
|
||||
"EicKD25vbWluYWxfdm9sdGFnZRgBIAEoAVIObm9taW5hbFZvbHRhZ2USIQoMc3RhbGxfdG9ycXVlGAIg" +
|
||||
"ASgBUgtzdGFsbFRvcnF1ZRIjCg1zdGFsbF9jdXJyZW50GAMgASgBUgxzdGFsbEN1cnJlbnQSIQoMZnJl" +
|
||||
"ZV9jdXJyZW50GAQgASgBUgtmcmVlQ3VycmVudBIdCgpmcmVlX3NwZWVkGAUgASgBUglmcmVlU3BlZWQi" +
|
||||
"mQIKFFByb3RvYnVmTGluZWFyU3lzdGVtEh0KCm51bV9zdGF0ZXMYASABKA1SCW51bVN0YXRlcxIdCgpu" +
|
||||
"dW1faW5wdXRzGAIgASgNUgludW1JbnB1dHMSHwoLbnVtX291dHB1dHMYAyABKA1SCm51bU91dHB1dHMS" +
|
||||
"JwoBYRgEIAEoCzIZLndwaS5wcm90by5Qcm90b2J1Zk1hdHJpeFIBYRInCgFiGAUgASgLMhkud3BpLnBy" +
|
||||
"b3RvLlByb3RvYnVmTWF0cml4UgFiEicKAWMYBiABKAsyGS53cGkucHJvdG8uUHJvdG9idWZNYXRyaXhS" +
|
||||
"AWMSJwoBZBgHIAEoCzIZLndwaS5wcm90by5Qcm90b2J1Zk1hdHJpeFIBZEIXChVvcmcud3BpbGliLm1h" +
|
||||
"dGgucHJvdG9KgAYKBhIEAAAYAQoICgEMEgMAABIKCAoBAhIDAgASCgkKAgMAEgMEABcKCAoBCBIDBgAu" +
|
||||
"CgkKAggBEgMGAC4KCgoCBAASBAgADgEKCgoDBAABEgMICBcKCwoEBAACABIDCQIdCgwKBQQAAgAFEgMJ" +
|
||||
"AggKDAoFBAACAAESAwkJGAoMCgUEAAIAAxIDCRscCgsKBAQAAgESAwoCGgoMCgUEAAIBBRIDCgIICgwK" +
|
||||
"BQQAAgEBEgMKCRUKDAoFBAACAQMSAwoYGQoLCgQEAAICEgMLAhsKDAoFBAACAgUSAwsCCAoMCgUEAAIC" +
|
||||
"ARIDCwkWCgwKBQQAAgIDEgMLGRoKCwoEBAACAxIDDAIaCgwKBQQAAgMFEgMMAggKDAoFBAACAwESAwwJ" +
|
||||
"FQoMCgUEAAIDAxIDDBgZCgsKBAQAAgQSAw0CGAoMCgUEAAIEBRIDDQIICgwKBQQAAgQBEgMNCRMKDAoF" +
|
||||
"BAACBAMSAw0WFwoKCgIEARIEEAAYAQoKCgMEAQESAxAIHAoLCgQEAQIAEgMRAhgKDAoFBAECAAUSAxEC" +
|
||||
"CAoMCgUEAQIAARIDEQkTCgwKBQQBAgADEgMRFhcKCwoEBAECARIDEgIYCgwKBQQBAgEFEgMSAggKDAoF" +
|
||||
"BAECAQESAxIJEwoMCgUEAQIBAxIDEhYXCgsKBAQBAgISAxMCGQoMCgUEAQICBRIDEwIICgwKBQQBAgIB" +
|
||||
"EgMTCRQKDAoFBAECAgMSAxMXGAoLCgQEAQIDEgMUAhcKDAoFBAECAwYSAxQCEAoMCgUEAQIDARIDFBES" +
|
||||
"CgwKBQQBAgMDEgMUFRYKCwoEBAECBBIDFQIXCgwKBQQBAgQGEgMVAhAKDAoFBAECBAESAxUREgoMCgUE",
|
||||
"AQIEAxIDFRUWCgsKBAQBAgUSAxYCFwoMCgUEAQIFBhIDFgIQCgwKBQQBAgUBEgMWERIKDAoFBAECBQMS" +
|
||||
"AxYVFgoLCgQEAQIGEgMXAhcKDAoFBAECBgYSAxcCEAoMCgUEAQIGARIDFxESCgwKBQQBAgYDEgMXFRZi" +
|
||||
"BnByb3RvMw==");
|
||||
|
||||
static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("system.proto", "wpi.proto", descriptorData, Wpimath.getDescriptor());
|
||||
|
||||
static final Descriptors.Descriptor wpi_proto_ProtobufLinearSystem_descriptor = descriptor.internalContainedType(43, 281, "ProtobufLinearSystem", "wpi.proto.ProtobufLinearSystem");
|
||||
static final Descriptors.Descriptor wpi_proto_ProtobufDCMotor_descriptor = descriptor.internalContainedType(43, 196, "ProtobufDCMotor", "wpi.proto.ProtobufDCMotor");
|
||||
|
||||
static final Descriptors.Descriptor wpi_proto_ProtobufLinearSystem_descriptor = descriptor.internalContainedType(242, 281, "ProtobufLinearSystem", "wpi.proto.ProtobufLinearSystem");
|
||||
|
||||
/**
|
||||
* @return this proto file's descriptor.
|
||||
@@ -45,6 +56,583 @@ public final class System {
|
||||
return descriptor;
|
||||
}
|
||||
|
||||
/**
|
||||
* Protobuf type {@code ProtobufDCMotor}
|
||||
*/
|
||||
public static final class ProtobufDCMotor extends ProtoMessage<ProtobufDCMotor> implements Cloneable {
|
||||
private static final long serialVersionUID = 0L;
|
||||
|
||||
/**
|
||||
* <code>optional double nominal_voltage = 1;</code>
|
||||
*/
|
||||
private double nominalVoltage;
|
||||
|
||||
/**
|
||||
* <code>optional double stall_torque = 2;</code>
|
||||
*/
|
||||
private double stallTorque;
|
||||
|
||||
/**
|
||||
* <code>optional double stall_current = 3;</code>
|
||||
*/
|
||||
private double stallCurrent;
|
||||
|
||||
/**
|
||||
* <code>optional double free_current = 4;</code>
|
||||
*/
|
||||
private double freeCurrent;
|
||||
|
||||
/**
|
||||
* <code>optional double free_speed = 5;</code>
|
||||
*/
|
||||
private double freeSpeed;
|
||||
|
||||
private ProtobufDCMotor() {
|
||||
}
|
||||
|
||||
/**
|
||||
* @return a new empty instance of {@code ProtobufDCMotor}
|
||||
*/
|
||||
public static ProtobufDCMotor newInstance() {
|
||||
return new ProtobufDCMotor();
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double nominal_voltage = 1;</code>
|
||||
* @return whether the nominalVoltage field is set
|
||||
*/
|
||||
public boolean hasNominalVoltage() {
|
||||
return (bitField0_ & 0x00000001) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double nominal_voltage = 1;</code>
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor clearNominalVoltage() {
|
||||
bitField0_ &= ~0x00000001;
|
||||
nominalVoltage = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double nominal_voltage = 1;</code>
|
||||
* @return the nominalVoltage
|
||||
*/
|
||||
public double getNominalVoltage() {
|
||||
return nominalVoltage;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double nominal_voltage = 1;</code>
|
||||
* @param value the nominalVoltage to set
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor setNominalVoltage(final double value) {
|
||||
bitField0_ |= 0x00000001;
|
||||
nominalVoltage = value;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_torque = 2;</code>
|
||||
* @return whether the stallTorque field is set
|
||||
*/
|
||||
public boolean hasStallTorque() {
|
||||
return (bitField0_ & 0x00000002) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_torque = 2;</code>
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor clearStallTorque() {
|
||||
bitField0_ &= ~0x00000002;
|
||||
stallTorque = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_torque = 2;</code>
|
||||
* @return the stallTorque
|
||||
*/
|
||||
public double getStallTorque() {
|
||||
return stallTorque;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_torque = 2;</code>
|
||||
* @param value the stallTorque to set
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor setStallTorque(final double value) {
|
||||
bitField0_ |= 0x00000002;
|
||||
stallTorque = value;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_current = 3;</code>
|
||||
* @return whether the stallCurrent field is set
|
||||
*/
|
||||
public boolean hasStallCurrent() {
|
||||
return (bitField0_ & 0x00000004) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_current = 3;</code>
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor clearStallCurrent() {
|
||||
bitField0_ &= ~0x00000004;
|
||||
stallCurrent = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_current = 3;</code>
|
||||
* @return the stallCurrent
|
||||
*/
|
||||
public double getStallCurrent() {
|
||||
return stallCurrent;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double stall_current = 3;</code>
|
||||
* @param value the stallCurrent to set
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor setStallCurrent(final double value) {
|
||||
bitField0_ |= 0x00000004;
|
||||
stallCurrent = value;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_current = 4;</code>
|
||||
* @return whether the freeCurrent field is set
|
||||
*/
|
||||
public boolean hasFreeCurrent() {
|
||||
return (bitField0_ & 0x00000008) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_current = 4;</code>
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor clearFreeCurrent() {
|
||||
bitField0_ &= ~0x00000008;
|
||||
freeCurrent = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_current = 4;</code>
|
||||
* @return the freeCurrent
|
||||
*/
|
||||
public double getFreeCurrent() {
|
||||
return freeCurrent;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_current = 4;</code>
|
||||
* @param value the freeCurrent to set
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor setFreeCurrent(final double value) {
|
||||
bitField0_ |= 0x00000008;
|
||||
freeCurrent = value;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_speed = 5;</code>
|
||||
* @return whether the freeSpeed field is set
|
||||
*/
|
||||
public boolean hasFreeSpeed() {
|
||||
return (bitField0_ & 0x00000010) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_speed = 5;</code>
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor clearFreeSpeed() {
|
||||
bitField0_ &= ~0x00000010;
|
||||
freeSpeed = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_speed = 5;</code>
|
||||
* @return the freeSpeed
|
||||
*/
|
||||
public double getFreeSpeed() {
|
||||
return freeSpeed;
|
||||
}
|
||||
|
||||
/**
|
||||
* <code>optional double free_speed = 5;</code>
|
||||
* @param value the freeSpeed to set
|
||||
* @return this
|
||||
*/
|
||||
public ProtobufDCMotor setFreeSpeed(final double value) {
|
||||
bitField0_ |= 0x00000010;
|
||||
freeSpeed = value;
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor copyFrom(final ProtobufDCMotor other) {
|
||||
cachedSize = other.cachedSize;
|
||||
if ((bitField0_ | other.bitField0_) != 0) {
|
||||
bitField0_ = other.bitField0_;
|
||||
nominalVoltage = other.nominalVoltage;
|
||||
stallTorque = other.stallTorque;
|
||||
stallCurrent = other.stallCurrent;
|
||||
freeCurrent = other.freeCurrent;
|
||||
freeSpeed = other.freeSpeed;
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor mergeFrom(final ProtobufDCMotor other) {
|
||||
if (other.isEmpty()) {
|
||||
return this;
|
||||
}
|
||||
cachedSize = -1;
|
||||
if (other.hasNominalVoltage()) {
|
||||
setNominalVoltage(other.nominalVoltage);
|
||||
}
|
||||
if (other.hasStallTorque()) {
|
||||
setStallTorque(other.stallTorque);
|
||||
}
|
||||
if (other.hasStallCurrent()) {
|
||||
setStallCurrent(other.stallCurrent);
|
||||
}
|
||||
if (other.hasFreeCurrent()) {
|
||||
setFreeCurrent(other.freeCurrent);
|
||||
}
|
||||
if (other.hasFreeSpeed()) {
|
||||
setFreeSpeed(other.freeSpeed);
|
||||
}
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor clear() {
|
||||
if (isEmpty()) {
|
||||
return this;
|
||||
}
|
||||
cachedSize = -1;
|
||||
bitField0_ = 0;
|
||||
nominalVoltage = 0D;
|
||||
stallTorque = 0D;
|
||||
stallCurrent = 0D;
|
||||
freeCurrent = 0D;
|
||||
freeSpeed = 0D;
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor clearQuick() {
|
||||
if (isEmpty()) {
|
||||
return this;
|
||||
}
|
||||
cachedSize = -1;
|
||||
bitField0_ = 0;
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean equals(Object o) {
|
||||
if (o == this) {
|
||||
return true;
|
||||
}
|
||||
if (!(o instanceof ProtobufDCMotor)) {
|
||||
return false;
|
||||
}
|
||||
ProtobufDCMotor other = (ProtobufDCMotor) o;
|
||||
return bitField0_ == other.bitField0_
|
||||
&& (!hasNominalVoltage() || ProtoUtil.isEqual(nominalVoltage, other.nominalVoltage))
|
||||
&& (!hasStallTorque() || ProtoUtil.isEqual(stallTorque, other.stallTorque))
|
||||
&& (!hasStallCurrent() || ProtoUtil.isEqual(stallCurrent, other.stallCurrent))
|
||||
&& (!hasFreeCurrent() || ProtoUtil.isEqual(freeCurrent, other.freeCurrent))
|
||||
&& (!hasFreeSpeed() || ProtoUtil.isEqual(freeSpeed, other.freeSpeed));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void writeTo(final ProtoSink output) throws IOException {
|
||||
if ((bitField0_ & 0x00000001) != 0) {
|
||||
output.writeRawByte((byte) 9);
|
||||
output.writeDoubleNoTag(nominalVoltage);
|
||||
}
|
||||
if ((bitField0_ & 0x00000002) != 0) {
|
||||
output.writeRawByte((byte) 17);
|
||||
output.writeDoubleNoTag(stallTorque);
|
||||
}
|
||||
if ((bitField0_ & 0x00000004) != 0) {
|
||||
output.writeRawByte((byte) 25);
|
||||
output.writeDoubleNoTag(stallCurrent);
|
||||
}
|
||||
if ((bitField0_ & 0x00000008) != 0) {
|
||||
output.writeRawByte((byte) 33);
|
||||
output.writeDoubleNoTag(freeCurrent);
|
||||
}
|
||||
if ((bitField0_ & 0x00000010) != 0) {
|
||||
output.writeRawByte((byte) 41);
|
||||
output.writeDoubleNoTag(freeSpeed);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
protected int computeSerializedSize() {
|
||||
int size = 0;
|
||||
if ((bitField0_ & 0x00000001) != 0) {
|
||||
size += 9;
|
||||
}
|
||||
if ((bitField0_ & 0x00000002) != 0) {
|
||||
size += 9;
|
||||
}
|
||||
if ((bitField0_ & 0x00000004) != 0) {
|
||||
size += 9;
|
||||
}
|
||||
if ((bitField0_ & 0x00000008) != 0) {
|
||||
size += 9;
|
||||
}
|
||||
if ((bitField0_ & 0x00000010) != 0) {
|
||||
size += 9;
|
||||
}
|
||||
return size;
|
||||
}
|
||||
|
||||
@Override
|
||||
@SuppressWarnings("fallthrough")
|
||||
public ProtobufDCMotor mergeFrom(final ProtoSource input) throws IOException {
|
||||
// Enabled Fall-Through Optimization (QuickBuffers)
|
||||
int tag = input.readTag();
|
||||
while (true) {
|
||||
switch (tag) {
|
||||
case 9: {
|
||||
// nominalVoltage
|
||||
nominalVoltage = input.readDouble();
|
||||
bitField0_ |= 0x00000001;
|
||||
tag = input.readTag();
|
||||
if (tag != 17) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
case 17: {
|
||||
// stallTorque
|
||||
stallTorque = input.readDouble();
|
||||
bitField0_ |= 0x00000002;
|
||||
tag = input.readTag();
|
||||
if (tag != 25) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
case 25: {
|
||||
// stallCurrent
|
||||
stallCurrent = input.readDouble();
|
||||
bitField0_ |= 0x00000004;
|
||||
tag = input.readTag();
|
||||
if (tag != 33) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
case 33: {
|
||||
// freeCurrent
|
||||
freeCurrent = input.readDouble();
|
||||
bitField0_ |= 0x00000008;
|
||||
tag = input.readTag();
|
||||
if (tag != 41) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
case 41: {
|
||||
// freeSpeed
|
||||
freeSpeed = input.readDouble();
|
||||
bitField0_ |= 0x00000010;
|
||||
tag = input.readTag();
|
||||
if (tag != 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
case 0: {
|
||||
return this;
|
||||
}
|
||||
default: {
|
||||
if (!input.skipField(tag)) {
|
||||
return this;
|
||||
}
|
||||
tag = input.readTag();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void writeTo(final JsonSink output) throws IOException {
|
||||
output.beginObject();
|
||||
if ((bitField0_ & 0x00000001) != 0) {
|
||||
output.writeDouble(FieldNames.nominalVoltage, nominalVoltage);
|
||||
}
|
||||
if ((bitField0_ & 0x00000002) != 0) {
|
||||
output.writeDouble(FieldNames.stallTorque, stallTorque);
|
||||
}
|
||||
if ((bitField0_ & 0x00000004) != 0) {
|
||||
output.writeDouble(FieldNames.stallCurrent, stallCurrent);
|
||||
}
|
||||
if ((bitField0_ & 0x00000008) != 0) {
|
||||
output.writeDouble(FieldNames.freeCurrent, freeCurrent);
|
||||
}
|
||||
if ((bitField0_ & 0x00000010) != 0) {
|
||||
output.writeDouble(FieldNames.freeSpeed, freeSpeed);
|
||||
}
|
||||
output.endObject();
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor mergeFrom(final JsonSource input) throws IOException {
|
||||
if (!input.beginObject()) {
|
||||
return this;
|
||||
}
|
||||
while (!input.isAtEnd()) {
|
||||
switch (input.readFieldHash()) {
|
||||
case 1374862050:
|
||||
case 173092603: {
|
||||
if (input.isAtField(FieldNames.nominalVoltage)) {
|
||||
if (!input.trySkipNullValue()) {
|
||||
nominalVoltage = input.readDouble();
|
||||
bitField0_ |= 0x00000001;
|
||||
}
|
||||
} else {
|
||||
input.skipUnknownField();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 2075810250:
|
||||
case 1238615945: {
|
||||
if (input.isAtField(FieldNames.stallTorque)) {
|
||||
if (!input.trySkipNullValue()) {
|
||||
stallTorque = input.readDouble();
|
||||
bitField0_ |= 0x00000002;
|
||||
}
|
||||
} else {
|
||||
input.skipUnknownField();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case -2105262663:
|
||||
case 2006484954: {
|
||||
if (input.isAtField(FieldNames.stallCurrent)) {
|
||||
if (!input.trySkipNullValue()) {
|
||||
stallCurrent = input.readDouble();
|
||||
bitField0_ |= 0x00000004;
|
||||
}
|
||||
} else {
|
||||
input.skipUnknownField();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 1024355693:
|
||||
case 240406182: {
|
||||
if (input.isAtField(FieldNames.freeCurrent)) {
|
||||
if (!input.trySkipNullValue()) {
|
||||
freeCurrent = input.readDouble();
|
||||
bitField0_ |= 0x00000008;
|
||||
}
|
||||
} else {
|
||||
input.skipUnknownField();
|
||||
}
|
||||
break;
|
||||
}
|
||||
case -444654277:
|
||||
case -552732492: {
|
||||
if (input.isAtField(FieldNames.freeSpeed)) {
|
||||
if (!input.trySkipNullValue()) {
|
||||
freeSpeed = input.readDouble();
|
||||
bitField0_ |= 0x00000010;
|
||||
}
|
||||
} else {
|
||||
input.skipUnknownField();
|
||||
}
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
input.skipUnknownField();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
input.endObject();
|
||||
return this;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor clone() {
|
||||
return new ProtobufDCMotor().copyFrom(this);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isEmpty() {
|
||||
return ((bitField0_) == 0);
|
||||
}
|
||||
|
||||
public static ProtobufDCMotor parseFrom(final byte[] data) throws
|
||||
InvalidProtocolBufferException {
|
||||
return ProtoMessage.mergeFrom(new ProtobufDCMotor(), data).checkInitialized();
|
||||
}
|
||||
|
||||
public static ProtobufDCMotor parseFrom(final ProtoSource input) throws IOException {
|
||||
return ProtoMessage.mergeFrom(new ProtobufDCMotor(), input).checkInitialized();
|
||||
}
|
||||
|
||||
public static ProtobufDCMotor parseFrom(final JsonSource input) throws IOException {
|
||||
return ProtoMessage.mergeFrom(new ProtobufDCMotor(), input).checkInitialized();
|
||||
}
|
||||
|
||||
/**
|
||||
* @return factory for creating ProtobufDCMotor messages
|
||||
*/
|
||||
public static MessageFactory<ProtobufDCMotor> getFactory() {
|
||||
return ProtobufDCMotorFactory.INSTANCE;
|
||||
}
|
||||
|
||||
/**
|
||||
* @return this type's descriptor.
|
||||
*/
|
||||
public static Descriptors.Descriptor getDescriptor() {
|
||||
return System.wpi_proto_ProtobufDCMotor_descriptor;
|
||||
}
|
||||
|
||||
private enum ProtobufDCMotorFactory implements MessageFactory<ProtobufDCMotor> {
|
||||
INSTANCE;
|
||||
|
||||
@Override
|
||||
public ProtobufDCMotor create() {
|
||||
return ProtobufDCMotor.newInstance();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Contains name constants used for serializing JSON
|
||||
*/
|
||||
static class FieldNames {
|
||||
static final FieldName nominalVoltage = FieldName.forField("nominalVoltage", "nominal_voltage");
|
||||
|
||||
static final FieldName stallTorque = FieldName.forField("stallTorque", "stall_torque");
|
||||
|
||||
static final FieldName stallCurrent = FieldName.forField("stallCurrent", "stall_current");
|
||||
|
||||
static final FieldName freeCurrent = FieldName.forField("freeCurrent", "free_current");
|
||||
|
||||
static final FieldName freeSpeed = FieldName.forField("freeSpeed", "free_speed");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Protobuf type {@code ProtobufLinearSystem}
|
||||
*/
|
||||
|
||||
@@ -1,92 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
/* Automatically generated nanopb constant definitions */
|
||||
/* Generated by nanopb-0.4.9 */
|
||||
|
||||
#include "plant.npb.h"
|
||||
#if PB_PROTO_HEADER_VERSION != 40
|
||||
#error Regenerate this file with the current version of nanopb generator.
|
||||
#endif
|
||||
|
||||
#include <span>
|
||||
#include <string_view>
|
||||
static const uint8_t file_descriptor[] {
|
||||
0x0a,0x0b,0x70,0x6c,0x61,0x6e,0x74,0x2e,0x70,0x72,
|
||||
0x6f,0x74,0x6f,0x12,0x09,0x77,0x70,0x69,0x2e,0x70,
|
||||
0x72,0x6f,0x74,0x6f,0x22,0xc4,0x01,0x0a,0x0f,0x50,
|
||||
0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x44,0x43,0x4d,
|
||||
0x6f,0x74,0x6f,0x72,0x12,0x27,0x0a,0x0f,0x6e,0x6f,
|
||||
0x6d,0x69,0x6e,0x61,0x6c,0x5f,0x76,0x6f,0x6c,0x74,
|
||||
0x61,0x67,0x65,0x18,0x01,0x20,0x01,0x28,0x01,0x52,
|
||||
0x0e,0x6e,0x6f,0x6d,0x69,0x6e,0x61,0x6c,0x56,0x6f,
|
||||
0x6c,0x74,0x61,0x67,0x65,0x12,0x21,0x0a,0x0c,0x73,
|
||||
0x74,0x61,0x6c,0x6c,0x5f,0x74,0x6f,0x72,0x71,0x75,
|
||||
0x65,0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x0b,0x73,
|
||||
0x74,0x61,0x6c,0x6c,0x54,0x6f,0x72,0x71,0x75,0x65,
|
||||
0x12,0x23,0x0a,0x0d,0x73,0x74,0x61,0x6c,0x6c,0x5f,
|
||||
0x63,0x75,0x72,0x72,0x65,0x6e,0x74,0x18,0x03,0x20,
|
||||
0x01,0x28,0x01,0x52,0x0c,0x73,0x74,0x61,0x6c,0x6c,
|
||||
0x43,0x75,0x72,0x72,0x65,0x6e,0x74,0x12,0x21,0x0a,
|
||||
0x0c,0x66,0x72,0x65,0x65,0x5f,0x63,0x75,0x72,0x72,
|
||||
0x65,0x6e,0x74,0x18,0x04,0x20,0x01,0x28,0x01,0x52,
|
||||
0x0b,0x66,0x72,0x65,0x65,0x43,0x75,0x72,0x72,0x65,
|
||||
0x6e,0x74,0x12,0x1d,0x0a,0x0a,0x66,0x72,0x65,0x65,
|
||||
0x5f,0x73,0x70,0x65,0x65,0x64,0x18,0x05,0x20,0x01,
|
||||
0x28,0x01,0x52,0x09,0x66,0x72,0x65,0x65,0x53,0x70,
|
||||
0x65,0x65,0x64,0x42,0x17,0x0a,0x15,0x6f,0x72,0x67,
|
||||
0x2e,0x77,0x70,0x69,0x6c,0x69,0x62,0x2e,0x6d,0x61,
|
||||
0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0xdc,
|
||||
0x02,0x0a,0x06,0x12,0x04,0x00,0x00,0x0c,0x01,0x0a,
|
||||
0x08,0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,
|
||||
0x08,0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a,
|
||||
0x08,0x0a,0x01,0x08,0x12,0x03,0x04,0x00,0x2e,0x0a,
|
||||
0x09,0x0a,0x02,0x08,0x01,0x12,0x03,0x04,0x00,0x2e,
|
||||
0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x06,0x00,
|
||||
0x0c,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,0x12,
|
||||
0x03,0x06,0x08,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x00,
|
||||
0x02,0x00,0x12,0x03,0x07,0x02,0x1d,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x07,0x02,
|
||||
0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x01,
|
||||
0x12,0x03,0x07,0x09,0x18,0x0a,0x0c,0x0a,0x05,0x04,
|
||||
0x00,0x02,0x00,0x03,0x12,0x03,0x07,0x1b,0x1c,0x0a,
|
||||
0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03,0x08,
|
||||
0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,
|
||||
0x05,0x12,0x03,0x08,0x02,0x08,0x0a,0x0c,0x0a,0x05,
|
||||
0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x08,0x09,0x15,
|
||||
0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03,0x12,
|
||||
0x03,0x08,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x00,
|
||||
0x02,0x02,0x12,0x03,0x09,0x02,0x1b,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x09,0x02,
|
||||
0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x01,
|
||||
0x12,0x03,0x09,0x09,0x16,0x0a,0x0c,0x0a,0x05,0x04,
|
||||
0x00,0x02,0x02,0x03,0x12,0x03,0x09,0x19,0x1a,0x0a,
|
||||
0x0b,0x0a,0x04,0x04,0x00,0x02,0x03,0x12,0x03,0x0a,
|
||||
0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x03,
|
||||
0x05,0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a,0x05,
|
||||
0x04,0x00,0x02,0x03,0x01,0x12,0x03,0x0a,0x09,0x15,
|
||||
0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x03,0x03,0x12,
|
||||
0x03,0x0a,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x00,
|
||||
0x02,0x04,0x12,0x03,0x0b,0x02,0x18,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x00,0x02,0x04,0x05,0x12,0x03,0x0b,0x02,
|
||||
0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x04,0x01,
|
||||
0x12,0x03,0x0b,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,
|
||||
0x00,0x02,0x04,0x03,0x12,0x03,0x0b,0x16,0x17,0x62,
|
||||
0x06,0x70,0x72,0x6f,0x74,0x6f,0x33,
|
||||
};
|
||||
static const char file_name[] = "plant.proto";
|
||||
static const char wpi_proto_ProtobufDCMotor_name[] = "wpi.proto.ProtobufDCMotor";
|
||||
std::string_view wpi_proto_ProtobufDCMotor::msg_name(void) noexcept { return wpi_proto_ProtobufDCMotor_name; }
|
||||
pb_filedesc_t wpi_proto_ProtobufDCMotor::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; }
|
||||
PB_BIND(wpi_proto_ProtobufDCMotor, wpi_proto_ProtobufDCMotor, AUTO)
|
||||
|
||||
|
||||
|
||||
#ifndef PB_CONVERT_DOUBLE_FLOAT
|
||||
/* On some platforms (such as AVR), double is really float.
|
||||
* To be able to encode/decode double on these platforms, you need.
|
||||
* to define PB_CONVERT_DOUBLE_FLOAT in pb.h or compiler command line.
|
||||
*/
|
||||
PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES)
|
||||
#endif
|
||||
|
||||
@@ -1,57 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
/* Automatically generated nanopb header */
|
||||
/* Generated by nanopb-0.4.9 */
|
||||
|
||||
#ifndef PB_WPI_PROTO_PLANT_NPB_H_INCLUDED
|
||||
#define PB_WPI_PROTO_PLANT_NPB_H_INCLUDED
|
||||
#include <pb.h>
|
||||
#include <span>
|
||||
#include <string_view>
|
||||
|
||||
#if PB_PROTO_HEADER_VERSION != 40
|
||||
#error Regenerate this file with the current version of nanopb generator.
|
||||
#endif
|
||||
|
||||
/* Struct definitions */
|
||||
typedef struct _wpi_proto_ProtobufDCMotor {
|
||||
static const pb_msgdesc_t* msg_descriptor(void) noexcept;
|
||||
static std::string_view msg_name(void) noexcept;
|
||||
static pb_filedesc_t file_descriptor(void) noexcept;
|
||||
|
||||
double nominal_voltage;
|
||||
double stall_torque;
|
||||
double stall_current;
|
||||
double free_current;
|
||||
double free_speed;
|
||||
} wpi_proto_ProtobufDCMotor;
|
||||
|
||||
|
||||
/* Initializer values for message structs */
|
||||
#define wpi_proto_ProtobufDCMotor_init_default {0, 0, 0, 0, 0}
|
||||
#define wpi_proto_ProtobufDCMotor_init_zero {0, 0, 0, 0, 0}
|
||||
|
||||
/* Field tags (for use in manual encoding/decoding) */
|
||||
#define wpi_proto_ProtobufDCMotor_nominal_voltage_tag 1
|
||||
#define wpi_proto_ProtobufDCMotor_stall_torque_tag 2
|
||||
#define wpi_proto_ProtobufDCMotor_stall_current_tag 3
|
||||
#define wpi_proto_ProtobufDCMotor_free_current_tag 4
|
||||
#define wpi_proto_ProtobufDCMotor_free_speed_tag 5
|
||||
|
||||
/* Struct field encoding specification for nanopb */
|
||||
#define wpi_proto_ProtobufDCMotor_FIELDLIST(X, a) \
|
||||
X(a, STATIC, SINGULAR, DOUBLE, nominal_voltage, 1) \
|
||||
X(a, STATIC, SINGULAR, DOUBLE, stall_torque, 2) \
|
||||
X(a, STATIC, SINGULAR, DOUBLE, stall_current, 3) \
|
||||
X(a, STATIC, SINGULAR, DOUBLE, free_current, 4) \
|
||||
X(a, STATIC, SINGULAR, DOUBLE, free_speed, 5)
|
||||
#define wpi_proto_ProtobufDCMotor_CALLBACK NULL
|
||||
#define wpi_proto_ProtobufDCMotor_DEFAULT NULL
|
||||
|
||||
/* Maximum encoded size of messages (where known) */
|
||||
#define WPI_PROTO_PLANT_NPB_H_MAX_SIZE wpi_proto_ProtobufDCMotor_size
|
||||
#define wpi_proto_ProtobufDCMotor_size 45
|
||||
|
||||
|
||||
#endif
|
||||
@@ -16,87 +16,143 @@ static const uint8_t file_descriptor[] {
|
||||
0x72,0x6f,0x74,0x6f,0x12,0x09,0x77,0x70,0x69,0x2e,
|
||||
0x70,0x72,0x6f,0x74,0x6f,0x1a,0x0d,0x77,0x70,0x69,
|
||||
0x6d,0x61,0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f,
|
||||
0x22,0x99,0x02,0x0a,0x14,0x50,0x72,0x6f,0x74,0x6f,
|
||||
0x62,0x75,0x66,0x4c,0x69,0x6e,0x65,0x61,0x72,0x53,
|
||||
0x79,0x73,0x74,0x65,0x6d,0x12,0x1d,0x0a,0x0a,0x6e,
|
||||
0x75,0x6d,0x5f,0x73,0x74,0x61,0x74,0x65,0x73,0x18,
|
||||
0x01,0x20,0x01,0x28,0x0d,0x52,0x09,0x6e,0x75,0x6d,
|
||||
0x53,0x74,0x61,0x74,0x65,0x73,0x12,0x1d,0x0a,0x0a,
|
||||
0x6e,0x75,0x6d,0x5f,0x69,0x6e,0x70,0x75,0x74,0x73,
|
||||
0x18,0x02,0x20,0x01,0x28,0x0d,0x52,0x09,0x6e,0x75,
|
||||
0x6d,0x49,0x6e,0x70,0x75,0x74,0x73,0x12,0x1f,0x0a,
|
||||
0x0b,0x6e,0x75,0x6d,0x5f,0x6f,0x75,0x74,0x70,0x75,
|
||||
0x74,0x73,0x18,0x03,0x20,0x01,0x28,0x0d,0x52,0x0a,
|
||||
0x6e,0x75,0x6d,0x4f,0x75,0x74,0x70,0x75,0x74,0x73,
|
||||
0x12,0x27,0x0a,0x01,0x61,0x18,0x04,0x20,0x01,0x28,
|
||||
0x22,0xc4,0x01,0x0a,0x0f,0x50,0x72,0x6f,0x74,0x6f,
|
||||
0x62,0x75,0x66,0x44,0x43,0x4d,0x6f,0x74,0x6f,0x72,
|
||||
0x12,0x27,0x0a,0x0f,0x6e,0x6f,0x6d,0x69,0x6e,0x61,
|
||||
0x6c,0x5f,0x76,0x6f,0x6c,0x74,0x61,0x67,0x65,0x18,
|
||||
0x01,0x20,0x01,0x28,0x01,0x52,0x0e,0x6e,0x6f,0x6d,
|
||||
0x69,0x6e,0x61,0x6c,0x56,0x6f,0x6c,0x74,0x61,0x67,
|
||||
0x65,0x12,0x21,0x0a,0x0c,0x73,0x74,0x61,0x6c,0x6c,
|
||||
0x5f,0x74,0x6f,0x72,0x71,0x75,0x65,0x18,0x02,0x20,
|
||||
0x01,0x28,0x01,0x52,0x0b,0x73,0x74,0x61,0x6c,0x6c,
|
||||
0x54,0x6f,0x72,0x71,0x75,0x65,0x12,0x23,0x0a,0x0d,
|
||||
0x73,0x74,0x61,0x6c,0x6c,0x5f,0x63,0x75,0x72,0x72,
|
||||
0x65,0x6e,0x74,0x18,0x03,0x20,0x01,0x28,0x01,0x52,
|
||||
0x0c,0x73,0x74,0x61,0x6c,0x6c,0x43,0x75,0x72,0x72,
|
||||
0x65,0x6e,0x74,0x12,0x21,0x0a,0x0c,0x66,0x72,0x65,
|
||||
0x65,0x5f,0x63,0x75,0x72,0x72,0x65,0x6e,0x74,0x18,
|
||||
0x04,0x20,0x01,0x28,0x01,0x52,0x0b,0x66,0x72,0x65,
|
||||
0x65,0x43,0x75,0x72,0x72,0x65,0x6e,0x74,0x12,0x1d,
|
||||
0x0a,0x0a,0x66,0x72,0x65,0x65,0x5f,0x73,0x70,0x65,
|
||||
0x65,0x64,0x18,0x05,0x20,0x01,0x28,0x01,0x52,0x09,
|
||||
0x66,0x72,0x65,0x65,0x53,0x70,0x65,0x65,0x64,0x22,
|
||||
0x99,0x02,0x0a,0x14,0x50,0x72,0x6f,0x74,0x6f,0x62,
|
||||
0x75,0x66,0x4c,0x69,0x6e,0x65,0x61,0x72,0x53,0x79,
|
||||
0x73,0x74,0x65,0x6d,0x12,0x1d,0x0a,0x0a,0x6e,0x75,
|
||||
0x6d,0x5f,0x73,0x74,0x61,0x74,0x65,0x73,0x18,0x01,
|
||||
0x20,0x01,0x28,0x0d,0x52,0x09,0x6e,0x75,0x6d,0x53,
|
||||
0x74,0x61,0x74,0x65,0x73,0x12,0x1d,0x0a,0x0a,0x6e,
|
||||
0x75,0x6d,0x5f,0x69,0x6e,0x70,0x75,0x74,0x73,0x18,
|
||||
0x02,0x20,0x01,0x28,0x0d,0x52,0x09,0x6e,0x75,0x6d,
|
||||
0x49,0x6e,0x70,0x75,0x74,0x73,0x12,0x1f,0x0a,0x0b,
|
||||
0x6e,0x75,0x6d,0x5f,0x6f,0x75,0x74,0x70,0x75,0x74,
|
||||
0x73,0x18,0x03,0x20,0x01,0x28,0x0d,0x52,0x0a,0x6e,
|
||||
0x75,0x6d,0x4f,0x75,0x74,0x70,0x75,0x74,0x73,0x12,
|
||||
0x27,0x0a,0x01,0x61,0x18,0x04,0x20,0x01,0x28,0x0b,
|
||||
0x32,0x19,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f,
|
||||
0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75,
|
||||
0x66,0x4d,0x61,0x74,0x72,0x69,0x78,0x52,0x01,0x61,
|
||||
0x12,0x27,0x0a,0x01,0x62,0x18,0x05,0x20,0x01,0x28,
|
||||
0x0b,0x32,0x19,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,
|
||||
0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,
|
||||
0x75,0x66,0x4d,0x61,0x74,0x72,0x69,0x78,0x52,0x01,
|
||||
0x61,0x12,0x27,0x0a,0x01,0x62,0x18,0x05,0x20,0x01,
|
||||
0x62,0x12,0x27,0x0a,0x01,0x63,0x18,0x06,0x20,0x01,
|
||||
0x28,0x0b,0x32,0x19,0x2e,0x77,0x70,0x69,0x2e,0x70,
|
||||
0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,
|
||||
0x62,0x75,0x66,0x4d,0x61,0x74,0x72,0x69,0x78,0x52,
|
||||
0x01,0x62,0x12,0x27,0x0a,0x01,0x63,0x18,0x06,0x20,
|
||||
0x01,0x63,0x12,0x27,0x0a,0x01,0x64,0x18,0x07,0x20,
|
||||
0x01,0x28,0x0b,0x32,0x19,0x2e,0x77,0x70,0x69,0x2e,
|
||||
0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,
|
||||
0x6f,0x62,0x75,0x66,0x4d,0x61,0x74,0x72,0x69,0x78,
|
||||
0x52,0x01,0x63,0x12,0x27,0x0a,0x01,0x64,0x18,0x07,
|
||||
0x20,0x01,0x28,0x0b,0x32,0x19,0x2e,0x77,0x70,0x69,
|
||||
0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,
|
||||
0x74,0x6f,0x62,0x75,0x66,0x4d,0x61,0x74,0x72,0x69,
|
||||
0x78,0x52,0x01,0x64,0x42,0x17,0x0a,0x15,0x6f,0x72,
|
||||
0x67,0x2e,0x77,0x70,0x69,0x6c,0x69,0x62,0x2e,0x6d,
|
||||
0x61,0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,
|
||||
0xd5,0x03,0x0a,0x06,0x12,0x04,0x00,0x00,0x10,0x01,
|
||||
0x0a,0x08,0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12,
|
||||
0x0a,0x08,0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12,
|
||||
0x0a,0x09,0x0a,0x02,0x03,0x00,0x12,0x03,0x04,0x00,
|
||||
0x17,0x0a,0x08,0x0a,0x01,0x08,0x12,0x03,0x06,0x00,
|
||||
0x2e,0x0a,0x09,0x0a,0x02,0x08,0x01,0x12,0x03,0x06,
|
||||
0x00,0x2e,0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,
|
||||
0x08,0x00,0x10,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,
|
||||
0x01,0x12,0x03,0x08,0x08,0x1c,0x0a,0x0b,0x0a,0x04,
|
||||
0x04,0x00,0x02,0x00,0x12,0x03,0x09,0x02,0x18,0x0a,
|
||||
0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03,
|
||||
0x09,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,
|
||||
0x00,0x01,0x12,0x03,0x09,0x09,0x13,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x00,0x02,0x00,0x03,0x12,0x03,0x09,0x16,
|
||||
0x17,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12,
|
||||
0x03,0x0a,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x00,
|
||||
0x02,0x01,0x05,0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c,
|
||||
0x0a,0x05,0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x0a,
|
||||
0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,
|
||||
0x03,0x12,0x03,0x0a,0x16,0x17,0x0a,0x0b,0x0a,0x04,
|
||||
0x04,0x00,0x02,0x02,0x12,0x03,0x0b,0x02,0x19,0x0a,
|
||||
0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03,
|
||||
0x0b,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,
|
||||
0x02,0x01,0x12,0x03,0x0b,0x09,0x14,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x00,0x02,0x02,0x03,0x12,0x03,0x0b,0x17,
|
||||
0x18,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x03,0x12,
|
||||
0x03,0x0c,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x00,
|
||||
0x02,0x03,0x06,0x12,0x03,0x0c,0x02,0x10,0x0a,0x0c,
|
||||
0x0a,0x05,0x04,0x00,0x02,0x03,0x01,0x12,0x03,0x0c,
|
||||
0x11,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x03,
|
||||
0x03,0x12,0x03,0x0c,0x15,0x16,0x0a,0x0b,0x0a,0x04,
|
||||
0x04,0x00,0x02,0x04,0x12,0x03,0x0d,0x02,0x17,0x0a,
|
||||
0x0c,0x0a,0x05,0x04,0x00,0x02,0x04,0x06,0x12,0x03,
|
||||
0x0d,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,
|
||||
0x04,0x01,0x12,0x03,0x0d,0x11,0x12,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x00,0x02,0x04,0x03,0x12,0x03,0x0d,0x15,
|
||||
0x16,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x05,0x12,
|
||||
0x03,0x0e,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x00,
|
||||
0x02,0x05,0x06,0x12,0x03,0x0e,0x02,0x10,0x0a,0x0c,
|
||||
0x0a,0x05,0x04,0x00,0x02,0x05,0x01,0x12,0x03,0x0e,
|
||||
0x11,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x05,
|
||||
0x03,0x12,0x03,0x0e,0x15,0x16,0x0a,0x0b,0x0a,0x04,
|
||||
0x04,0x00,0x02,0x06,0x12,0x03,0x0f,0x02,0x17,0x0a,
|
||||
0x0c,0x0a,0x05,0x04,0x00,0x02,0x06,0x06,0x12,0x03,
|
||||
0x0f,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,
|
||||
0x06,0x01,0x12,0x03,0x0f,0x11,0x12,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x00,0x02,0x06,0x03,0x12,0x03,0x0f,0x15,
|
||||
0x16,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f,0x33,
|
||||
0x52,0x01,0x64,0x42,0x17,0x0a,0x15,0x6f,0x72,0x67,
|
||||
0x2e,0x77,0x70,0x69,0x6c,0x69,0x62,0x2e,0x6d,0x61,
|
||||
0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0x80,
|
||||
0x06,0x0a,0x06,0x12,0x04,0x00,0x00,0x18,0x01,0x0a,
|
||||
0x08,0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a,
|
||||
0x08,0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a,
|
||||
0x09,0x0a,0x02,0x03,0x00,0x12,0x03,0x04,0x00,0x17,
|
||||
0x0a,0x08,0x0a,0x01,0x08,0x12,0x03,0x06,0x00,0x2e,
|
||||
0x0a,0x09,0x0a,0x02,0x08,0x01,0x12,0x03,0x06,0x00,
|
||||
0x2e,0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x08,
|
||||
0x00,0x0e,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,
|
||||
0x12,0x03,0x08,0x08,0x17,0x0a,0x0b,0x0a,0x04,0x04,
|
||||
0x00,0x02,0x00,0x12,0x03,0x09,0x02,0x1d,0x0a,0x0c,
|
||||
0x0a,0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x09,
|
||||
0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,
|
||||
0x01,0x12,0x03,0x09,0x09,0x18,0x0a,0x0c,0x0a,0x05,
|
||||
0x04,0x00,0x02,0x00,0x03,0x12,0x03,0x09,0x1b,0x1c,
|
||||
0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03,
|
||||
0x0a,0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,
|
||||
0x01,0x05,0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x0a,0x09,
|
||||
0x15,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03,
|
||||
0x12,0x03,0x0a,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,
|
||||
0x00,0x02,0x02,0x12,0x03,0x0b,0x02,0x1b,0x0a,0x0c,
|
||||
0x0a,0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x0b,
|
||||
0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,
|
||||
0x01,0x12,0x03,0x0b,0x09,0x16,0x0a,0x0c,0x0a,0x05,
|
||||
0x04,0x00,0x02,0x02,0x03,0x12,0x03,0x0b,0x19,0x1a,
|
||||
0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x03,0x12,0x03,
|
||||
0x0c,0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,
|
||||
0x03,0x05,0x12,0x03,0x0c,0x02,0x08,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x00,0x02,0x03,0x01,0x12,0x03,0x0c,0x09,
|
||||
0x15,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x03,0x03,
|
||||
0x12,0x03,0x0c,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,
|
||||
0x00,0x02,0x04,0x12,0x03,0x0d,0x02,0x18,0x0a,0x0c,
|
||||
0x0a,0x05,0x04,0x00,0x02,0x04,0x05,0x12,0x03,0x0d,
|
||||
0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x04,
|
||||
0x01,0x12,0x03,0x0d,0x09,0x13,0x0a,0x0c,0x0a,0x05,
|
||||
0x04,0x00,0x02,0x04,0x03,0x12,0x03,0x0d,0x16,0x17,
|
||||
0x0a,0x0a,0x0a,0x02,0x04,0x01,0x12,0x04,0x10,0x00,
|
||||
0x18,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x01,0x01,0x12,
|
||||
0x03,0x10,0x08,0x1c,0x0a,0x0b,0x0a,0x04,0x04,0x01,
|
||||
0x02,0x00,0x12,0x03,0x11,0x02,0x18,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x01,0x02,0x00,0x05,0x12,0x03,0x11,0x02,
|
||||
0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x01,
|
||||
0x12,0x03,0x11,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,
|
||||
0x01,0x02,0x00,0x03,0x12,0x03,0x11,0x16,0x17,0x0a,
|
||||
0x0b,0x0a,0x04,0x04,0x01,0x02,0x01,0x12,0x03,0x12,
|
||||
0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,
|
||||
0x05,0x12,0x03,0x12,0x02,0x08,0x0a,0x0c,0x0a,0x05,
|
||||
0x04,0x01,0x02,0x01,0x01,0x12,0x03,0x12,0x09,0x13,
|
||||
0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x03,0x12,
|
||||
0x03,0x12,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x01,
|
||||
0x02,0x02,0x12,0x03,0x13,0x02,0x19,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x01,0x02,0x02,0x05,0x12,0x03,0x13,0x02,
|
||||
0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x02,0x01,
|
||||
0x12,0x03,0x13,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04,
|
||||
0x01,0x02,0x02,0x03,0x12,0x03,0x13,0x17,0x18,0x0a,
|
||||
0x0b,0x0a,0x04,0x04,0x01,0x02,0x03,0x12,0x03,0x14,
|
||||
0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x03,
|
||||
0x06,0x12,0x03,0x14,0x02,0x10,0x0a,0x0c,0x0a,0x05,
|
||||
0x04,0x01,0x02,0x03,0x01,0x12,0x03,0x14,0x11,0x12,
|
||||
0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x03,0x03,0x12,
|
||||
0x03,0x14,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x01,
|
||||
0x02,0x04,0x12,0x03,0x15,0x02,0x17,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x01,0x02,0x04,0x06,0x12,0x03,0x15,0x02,
|
||||
0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x04,0x01,
|
||||
0x12,0x03,0x15,0x11,0x12,0x0a,0x0c,0x0a,0x05,0x04,
|
||||
0x01,0x02,0x04,0x03,0x12,0x03,0x15,0x15,0x16,0x0a,
|
||||
0x0b,0x0a,0x04,0x04,0x01,0x02,0x05,0x12,0x03,0x16,
|
||||
0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x05,
|
||||
0x06,0x12,0x03,0x16,0x02,0x10,0x0a,0x0c,0x0a,0x05,
|
||||
0x04,0x01,0x02,0x05,0x01,0x12,0x03,0x16,0x11,0x12,
|
||||
0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x05,0x03,0x12,
|
||||
0x03,0x16,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x01,
|
||||
0x02,0x06,0x12,0x03,0x17,0x02,0x17,0x0a,0x0c,0x0a,
|
||||
0x05,0x04,0x01,0x02,0x06,0x06,0x12,0x03,0x17,0x02,
|
||||
0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x06,0x01,
|
||||
0x12,0x03,0x17,0x11,0x12,0x0a,0x0c,0x0a,0x05,0x04,
|
||||
0x01,0x02,0x06,0x03,0x12,0x03,0x17,0x15,0x16,0x62,
|
||||
0x06,0x70,0x72,0x6f,0x74,0x6f,0x33,
|
||||
};
|
||||
static const char file_name[] = "system.proto";
|
||||
static const char wpi_proto_ProtobufDCMotor_name[] = "wpi.proto.ProtobufDCMotor";
|
||||
std::string_view wpi_proto_ProtobufDCMotor::msg_name(void) noexcept { return wpi_proto_ProtobufDCMotor_name; }
|
||||
pb_filedesc_t wpi_proto_ProtobufDCMotor::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; }
|
||||
PB_BIND(wpi_proto_ProtobufDCMotor, wpi_proto_ProtobufDCMotor, AUTO)
|
||||
|
||||
|
||||
static const char wpi_proto_ProtobufLinearSystem_name[] = "wpi.proto.ProtobufLinearSystem";
|
||||
std::string_view wpi_proto_ProtobufLinearSystem::msg_name(void) noexcept { return wpi_proto_ProtobufLinearSystem_name; }
|
||||
pb_filedesc_t wpi_proto_ProtobufLinearSystem::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; }
|
||||
@@ -104,3 +160,11 @@ PB_BIND(wpi_proto_ProtobufLinearSystem, wpi_proto_ProtobufLinearSystem, AUTO)
|
||||
|
||||
|
||||
|
||||
#ifndef PB_CONVERT_DOUBLE_FLOAT
|
||||
/* On some platforms (such as AVR), double is really float.
|
||||
* To be able to encode/decode double on these platforms, you need.
|
||||
* to define PB_CONVERT_DOUBLE_FLOAT in pb.h or compiler command line.
|
||||
*/
|
||||
PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES)
|
||||
#endif
|
||||
|
||||
|
||||
@@ -16,6 +16,18 @@
|
||||
#endif
|
||||
|
||||
/* Struct definitions */
|
||||
typedef struct _wpi_proto_ProtobufDCMotor {
|
||||
static const pb_msgdesc_t* msg_descriptor(void) noexcept;
|
||||
static std::string_view msg_name(void) noexcept;
|
||||
static pb_filedesc_t file_descriptor(void) noexcept;
|
||||
|
||||
double nominal_voltage;
|
||||
double stall_torque;
|
||||
double stall_current;
|
||||
double free_current;
|
||||
double free_speed;
|
||||
} wpi_proto_ProtobufDCMotor;
|
||||
|
||||
typedef struct _wpi_proto_ProtobufLinearSystem {
|
||||
static const pb_msgdesc_t* msg_descriptor(void) noexcept;
|
||||
static std::string_view msg_name(void) noexcept;
|
||||
@@ -32,10 +44,17 @@ typedef struct _wpi_proto_ProtobufLinearSystem {
|
||||
|
||||
|
||||
/* Initializer values for message structs */
|
||||
#define wpi_proto_ProtobufDCMotor_init_default {0, 0, 0, 0, 0}
|
||||
#define wpi_proto_ProtobufLinearSystem_init_default {0, 0, 0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}}
|
||||
#define wpi_proto_ProtobufDCMotor_init_zero {0, 0, 0, 0, 0}
|
||||
#define wpi_proto_ProtobufLinearSystem_init_zero {0, 0, 0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}}
|
||||
|
||||
/* Field tags (for use in manual encoding/decoding) */
|
||||
#define wpi_proto_ProtobufDCMotor_nominal_voltage_tag 1
|
||||
#define wpi_proto_ProtobufDCMotor_stall_torque_tag 2
|
||||
#define wpi_proto_ProtobufDCMotor_stall_current_tag 3
|
||||
#define wpi_proto_ProtobufDCMotor_free_current_tag 4
|
||||
#define wpi_proto_ProtobufDCMotor_free_speed_tag 5
|
||||
#define wpi_proto_ProtobufLinearSystem_num_states_tag 1
|
||||
#define wpi_proto_ProtobufLinearSystem_num_inputs_tag 2
|
||||
#define wpi_proto_ProtobufLinearSystem_num_outputs_tag 3
|
||||
@@ -45,6 +64,15 @@ typedef struct _wpi_proto_ProtobufLinearSystem {
|
||||
#define wpi_proto_ProtobufLinearSystem_d_tag 7
|
||||
|
||||
/* Struct field encoding specification for nanopb */
|
||||
#define wpi_proto_ProtobufDCMotor_FIELDLIST(X, a) \
|
||||
X(a, STATIC, SINGULAR, DOUBLE, nominal_voltage, 1) \
|
||||
X(a, STATIC, SINGULAR, DOUBLE, stall_torque, 2) \
|
||||
X(a, STATIC, SINGULAR, DOUBLE, stall_current, 3) \
|
||||
X(a, STATIC, SINGULAR, DOUBLE, free_current, 4) \
|
||||
X(a, STATIC, SINGULAR, DOUBLE, free_speed, 5)
|
||||
#define wpi_proto_ProtobufDCMotor_CALLBACK NULL
|
||||
#define wpi_proto_ProtobufDCMotor_DEFAULT NULL
|
||||
|
||||
#define wpi_proto_ProtobufLinearSystem_FIELDLIST(X, a_) \
|
||||
X(a_, STATIC, SINGULAR, UINT32, num_states, 1) \
|
||||
X(a_, STATIC, SINGULAR, UINT32, num_inputs, 2) \
|
||||
@@ -62,6 +90,8 @@ X(a_, CALLBACK, OPTIONAL, MESSAGE, d, 7)
|
||||
|
||||
/* Maximum encoded size of messages (where known) */
|
||||
/* wpi_proto_ProtobufLinearSystem_size depends on runtime parameters */
|
||||
#define WPI_PROTO_SYSTEM_NPB_H_MAX_SIZE wpi_proto_ProtobufDCMotor_size
|
||||
#define wpi_proto_ProtobufDCMotor_size 45
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
@@ -16,7 +16,7 @@ import org.wpilib.math.util.Nat;
|
||||
* Filters the provided voltages to limit a differential drive's linear and angular acceleration.
|
||||
*
|
||||
* <p>The differential drive model can be created via the functions in {@link
|
||||
* org.wpilib.math.system.plant.LinearSystemId}.
|
||||
* org.wpilib.math.system.Models}.
|
||||
*/
|
||||
public class DifferentialDriveAccelerationLimiter {
|
||||
private final LinearSystem<N2, N2, N2> m_system;
|
||||
|
||||
@@ -9,7 +9,7 @@ import org.wpilib.math.controller.struct.DifferentialDriveFeedforwardStruct;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.util.protobuf.ProtobufSerializable;
|
||||
import org.wpilib.util.struct.StructSerializable;
|
||||
|
||||
@@ -41,7 +41,7 @@ public class DifferentialDriveFeedforward implements ProtobufSerializable, Struc
|
||||
*/
|
||||
public DifferentialDriveFeedforward(
|
||||
double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
|
||||
// See LinearSystemId.identifyDrivetrainSystem(double, double, double, double, double)
|
||||
// See Models.differentialDriveFromSysId(double, double, double, double, double)
|
||||
this(kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
|
||||
}
|
||||
|
||||
@@ -55,7 +55,7 @@ public class DifferentialDriveFeedforward implements ProtobufSerializable, Struc
|
||||
*/
|
||||
public DifferentialDriveFeedforward(
|
||||
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
|
||||
m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
|
||||
m_plant = Models.differentialDriveFromSysId(kVLinear, kALinear, kVAngular, kAAngular);
|
||||
m_kVLinear = kVLinear;
|
||||
m_kALinear = kALinear;
|
||||
m_kVAngular = kVAngular;
|
||||
|
||||
@@ -2,10 +2,10 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.math.system.plant;
|
||||
package org.wpilib.math.system;
|
||||
|
||||
import org.wpilib.math.system.plant.proto.DCMotorProto;
|
||||
import org.wpilib.math.system.plant.struct.DCMotorStruct;
|
||||
import org.wpilib.math.system.proto.DCMotorProto;
|
||||
import org.wpilib.math.system.struct.DCMotorStruct;
|
||||
import org.wpilib.math.util.Units;
|
||||
import org.wpilib.util.protobuf.ProtobufSerializable;
|
||||
import org.wpilib.util.struct.StructSerializable;
|
||||
370
wpimath/src/main/java/org/wpilib/math/system/Models.java
Normal file
370
wpimath/src/main/java/org/wpilib/math/system/Models.java
Normal file
@@ -0,0 +1,370 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.math.system;
|
||||
|
||||
import org.wpilib.math.linalg.MatBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.util.Nat;
|
||||
|
||||
/** Linear system factories. */
|
||||
public final class Models {
|
||||
private Models() {
|
||||
// Utility class
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a flywheel state-space model from physical constants.
|
||||
*
|
||||
* <p>The states are [angular velocity], the inputs are [voltage], and the outputs are [angular
|
||||
* velocity].
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the flywheel.
|
||||
* @param J The moment of inertia J of the flywheel.
|
||||
* @param gearing Gear ratio from motor to flywheel (greater than 1 is a reduction).
|
||||
* @return Flywheel state-space model.
|
||||
* @throws IllegalArgumentException if J <= 0 or gearing <= 0.
|
||||
*/
|
||||
public static LinearSystem<N1, N1, N1> flywheelFromPhysicalConstants(
|
||||
DCMotor motor, double J, double gearing) {
|
||||
if (J <= 0.0) {
|
||||
throw new IllegalArgumentException("J must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0.0) {
|
||||
throw new IllegalArgumentException("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
var A =
|
||||
MatBuilder.fill(
|
||||
Nat.N1(), Nat.N1(), -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J));
|
||||
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), gearing * motor.Kt / (motor.R * J));
|
||||
var C = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0);
|
||||
var D = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0);
|
||||
|
||||
return new LinearSystem<>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a flywheel state-space model from SysId constants kᵥ (V/(rad/s)) and kₐ (V/(rad/s²))
|
||||
* from the feedforward model u = kᵥv + kₐa.
|
||||
*
|
||||
* <p>The states are [velocity], the inputs are [voltage], and the outputs are [velocity].
|
||||
*
|
||||
* @param kV The velocity gain, in V/(rad/s).
|
||||
* @param kA The acceleration gain, in V/(rad/s²).
|
||||
* @return Flywheel state-space model.
|
||||
* @throws IllegalArgumentException if kV < 0 or kA <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
public static LinearSystem<N1, N1, N1> flywheelFromSysId(double kV, double kA) {
|
||||
if (kV < 0.0) {
|
||||
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
|
||||
}
|
||||
if (kA <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka must be greater than zero.");
|
||||
}
|
||||
|
||||
var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kV / kA);
|
||||
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / kA);
|
||||
var C = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0);
|
||||
var D = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0);
|
||||
|
||||
return new LinearSystem<>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an elevator state-space model from physical constants.
|
||||
*
|
||||
* <p>The states are [position, velocity], the inputs are [voltage], and the outputs are
|
||||
* [position, velocity].
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the carriage.
|
||||
* @param mass The mass of the elevator carriage, in kilograms.
|
||||
* @param radius The radius of the elevator's driving drum, in meters.
|
||||
* @param gearing Gear ratio from motor to carriage (greater than 1 is a reduction).
|
||||
* @return Elevator state-space model.
|
||||
* @throws IllegalArgumentException if mass <= 0, radius <= 0, or gearing <= 0.
|
||||
*/
|
||||
public static LinearSystem<N2, N1, N2> elevatorFromPhysicalConstants(
|
||||
DCMotor motor, double mass, double radius, double gearing) {
|
||||
if (mass <= 0.0) {
|
||||
throw new IllegalArgumentException("mass must be greater than zero.");
|
||||
}
|
||||
if (radius <= 0.0) {
|
||||
throw new IllegalArgumentException("radius must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0.0) {
|
||||
throw new IllegalArgumentException("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
var A =
|
||||
MatBuilder.fill(
|
||||
Nat.N2(),
|
||||
Nat.N2(),
|
||||
0.0,
|
||||
1.0,
|
||||
0.0,
|
||||
-Math.pow(gearing, 2) * motor.Kt / (motor.R * Math.pow(radius, 2) * mass * motor.Kv));
|
||||
var B =
|
||||
MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, gearing * motor.Kt / (motor.R * radius * mass));
|
||||
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
|
||||
var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0);
|
||||
|
||||
return new LinearSystem<>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an elevator state-space model from SysId constants kᵥ (V/(m/s)) and kₐ (V/(m/s²)) from
|
||||
* the feedforward model u = kᵥv + kₐa.
|
||||
*
|
||||
* <p>The states are [position, velocity], the inputs are [voltage], and the outputs are
|
||||
* [position, velocity].
|
||||
*
|
||||
* @param kV The velocity gain, in V/(m/s).
|
||||
* @param kA The acceleration gain, in V/(m/s²).
|
||||
* @return Elevator state-space model.
|
||||
* @throws IllegalArgumentException if kV < 0 or kA <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
public static LinearSystem<N2, N1, N2> elevatorFromSysId(double kV, double kA) {
|
||||
if (kV < 0.0) {
|
||||
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
|
||||
}
|
||||
if (kA <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka must be greater than zero.");
|
||||
}
|
||||
|
||||
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA);
|
||||
var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 1.0 / kA);
|
||||
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
|
||||
var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0);
|
||||
|
||||
return new LinearSystem<>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a single-jointed arm state-space model from physical constants.
|
||||
*
|
||||
* <p>The states are [angle, angular velocity], the inputs are [voltage], and the outputs are
|
||||
* [angle, angular velocity].
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the arm.
|
||||
* @param J The moment of inertia J of the arm.
|
||||
* @param gearing Gear ratio from motor to arm (greater than 1 is a reduction).
|
||||
* @return Single-jointed arm state-space model.
|
||||
* @throws IllegalArgumentException if J <= 0 or gearing <= 0.
|
||||
*/
|
||||
public static LinearSystem<N2, N1, N2> singleJointedArmFromPhysicalConstants(
|
||||
DCMotor motor, double J, double gearing) {
|
||||
if (J <= 0.0) {
|
||||
throw new IllegalArgumentException("J must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0.0) {
|
||||
throw new IllegalArgumentException("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
var A =
|
||||
MatBuilder.fill(
|
||||
Nat.N2(),
|
||||
Nat.N2(),
|
||||
0.0,
|
||||
1.0,
|
||||
0.0,
|
||||
-Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J));
|
||||
var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, gearing * motor.Kt / (motor.R * J));
|
||||
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
|
||||
var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0);
|
||||
|
||||
return new LinearSystem<>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a single-jointed arm state-space model from SysId constants kᵥ (V/(rad/s)) and kₐ
|
||||
* (V/(rad/s²)) from the feedforward model u = kᵥv + kₐa.
|
||||
*
|
||||
* <p>The states are [position, velocity], the inputs are [voltage], and the outputs are
|
||||
* [position, velocity].
|
||||
*
|
||||
* @param kV The velocity gain, in volts/(unit/sec).
|
||||
* @param kA The acceleration gain, in volts/(unit/sec²).
|
||||
* @return Single-jointed arm state-space model.
|
||||
* @throws IllegalArgumentException if kV < 0 or kA <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
public static LinearSystem<N2, N1, N2> singleJointedArmFromSysId(double kV, double kA) {
|
||||
if (kV < 0.0) {
|
||||
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
|
||||
}
|
||||
if (kA <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka must be greater than zero.");
|
||||
}
|
||||
|
||||
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA);
|
||||
var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 1.0 / kA);
|
||||
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
|
||||
var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0);
|
||||
|
||||
return new LinearSystem<>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a differential drive state-space model from physical constants.
|
||||
*
|
||||
* <p>The states are [left velocity, right velocity], the inputs are [left voltage, right
|
||||
* voltage], and the outputs are [left velocity, right velocity].
|
||||
*
|
||||
* @param motor The motor (or gearbox) driving the drivetrain.
|
||||
* @param mass The mass of the robot in kilograms.
|
||||
* @param r The radius of the wheels in meters.
|
||||
* @param rb The radius of the base (half of the trackwidth), in meters.
|
||||
* @param J The moment of inertia of the robot.
|
||||
* @param gearing Gear ratio from motor to wheel (greater than 1 is a reduction).
|
||||
* @return Differential drive state-space model.
|
||||
* @throws IllegalArgumentException if mass <= 0, r <= 0, rb <= 0, J <= 0, or gearing
|
||||
* <= 0.
|
||||
*/
|
||||
public static LinearSystem<N2, N2, N2> differentialDriveFromPhysicalConstants(
|
||||
DCMotor motor, double mass, double r, double rb, double J, double gearing) {
|
||||
if (mass <= 0.0) {
|
||||
throw new IllegalArgumentException("mass must be greater than zero.");
|
||||
}
|
||||
if (r <= 0.0) {
|
||||
throw new IllegalArgumentException("r must be greater than zero.");
|
||||
}
|
||||
if (rb <= 0.0) {
|
||||
throw new IllegalArgumentException("rb must be greater than zero.");
|
||||
}
|
||||
if (J <= 0.0) {
|
||||
throw new IllegalArgumentException("J must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0.0) {
|
||||
throw new IllegalArgumentException("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
double C1 = -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * Math.pow(r, 2));
|
||||
double C2 = gearing * motor.Kt / (motor.R * r);
|
||||
|
||||
var A =
|
||||
MatBuilder.fill(
|
||||
Nat.N2(),
|
||||
Nat.N2(),
|
||||
(1 / mass + Math.pow(rb, 2) / J) * C1,
|
||||
(1 / mass - Math.pow(rb, 2) / J) * C1,
|
||||
(1 / mass - Math.pow(rb, 2) / J) * C1,
|
||||
(1 / mass + Math.pow(rb, 2) / J) * C1);
|
||||
var B =
|
||||
MatBuilder.fill(
|
||||
Nat.N2(),
|
||||
Nat.N2(),
|
||||
(1 / mass + Math.pow(rb, 2) / J) * C2,
|
||||
(1 / mass - Math.pow(rb, 2) / J) * C2,
|
||||
(1 / mass - Math.pow(rb, 2) / J) * C2,
|
||||
(1 / mass + Math.pow(rb, 2) / J) * C2);
|
||||
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
|
||||
var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0);
|
||||
|
||||
return new LinearSystem<>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a differential drive state-space model from SysId constants kᵥ and kₐ in both linear
|
||||
* {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s), (V/(rad/s²))} cases.
|
||||
*
|
||||
* <p>The states are [left velocity, right velocity], the inputs are [left voltage, right
|
||||
* voltage], and the outputs are [left velocity, right velocity].
|
||||
*
|
||||
* @param kVLinear The linear velocity gain in volts per (meters per second).
|
||||
* @param kALinear The linear acceleration gain in volts per (meters per second squared).
|
||||
* @param kVAngular The angular velocity gain in volts per (meters per second).
|
||||
* @param kAAngular The angular acceleration gain in volts per (meters per second squared).
|
||||
* @return Differential drive state-space model.
|
||||
* @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, or
|
||||
* kAAngular <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
public static LinearSystem<N2, N2, N2> differentialDriveFromSysId(
|
||||
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
|
||||
if (kVLinear <= 0.0) {
|
||||
throw new IllegalArgumentException("Kv,linear must be greater than zero.");
|
||||
}
|
||||
if (kALinear <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka,linear must be greater than zero.");
|
||||
}
|
||||
if (kVAngular <= 0.0) {
|
||||
throw new IllegalArgumentException("Kv,angular must be greater than zero.");
|
||||
}
|
||||
if (kAAngular <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka,angular must be greater than zero.");
|
||||
}
|
||||
|
||||
double A1 = -0.5 * (kVLinear / kALinear + kVAngular / kAAngular);
|
||||
double A2 = -0.5 * (kVLinear / kALinear - kVAngular / kAAngular);
|
||||
double B1 = 0.5 / kALinear + 0.5 / kAAngular;
|
||||
double B2 = 0.5 / kALinear - 0.5 / kAAngular;
|
||||
|
||||
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1);
|
||||
var B = MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1);
|
||||
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
|
||||
var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0);
|
||||
|
||||
return new LinearSystem<>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a differential drive state-space model from SysId constants kᵥ and kₐ in both linear
|
||||
* {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s), (V/(rad/s²))} cases.
|
||||
*
|
||||
* <p>The states are [left velocity, right velocity], the inputs are [left voltage, right
|
||||
* voltage], and the outputs are [left velocity, right velocity].
|
||||
*
|
||||
* @param kVLinear The linear velocity gain in volts per (meters per second).
|
||||
* @param kALinear The linear acceleration gain in volts per (meters per second squared).
|
||||
* @param kVAngular The angular velocity gain in volts per (radians per second).
|
||||
* @param kAAngular The angular acceleration gain in volts per (radians per second squared).
|
||||
* @param trackwidth The distance between the differential drive's left and right wheels, in
|
||||
* meters.
|
||||
* @return Differential drive state-space model.
|
||||
* @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
|
||||
* kAAngular <= 0, or trackwidth <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
public static LinearSystem<N2, N2, N2> differentialDriveFromSysId(
|
||||
double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
|
||||
if (kVLinear <= 0.0) {
|
||||
throw new IllegalArgumentException("Kv,linear must be greater than zero.");
|
||||
}
|
||||
if (kALinear <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka,linear must be greater than zero.");
|
||||
}
|
||||
if (kVAngular <= 0.0) {
|
||||
throw new IllegalArgumentException("Kv,angular must be greater than zero.");
|
||||
}
|
||||
if (kAAngular <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka,angular must be greater than zero.");
|
||||
}
|
||||
if (trackwidth <= 0.0) {
|
||||
throw new IllegalArgumentException("r must be greater than zero.");
|
||||
}
|
||||
|
||||
// We want to find a factor to include in Kv,angular that will convert
|
||||
// `u = Kv,angular omega` to `u = Kv,angular v`.
|
||||
//
|
||||
// v = omega r
|
||||
// omega = v/r
|
||||
// omega = 1/r v
|
||||
// omega = 1/(trackwidth/2) v
|
||||
// omega = 2/trackwidth v
|
||||
//
|
||||
// So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
|
||||
// to V/(m/s).
|
||||
return differentialDriveFromSysId(
|
||||
kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
|
||||
}
|
||||
}
|
||||
@@ -1,390 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.math.system.plant;
|
||||
|
||||
import org.wpilib.math.linalg.MatBuilder;
|
||||
import org.wpilib.math.linalg.Matrix;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.util.Nat;
|
||||
|
||||
/** Linear system ID utility functions. */
|
||||
public final class LinearSystemId {
|
||||
private LinearSystemId() {
|
||||
// Utility class
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of an elevator system. The states of the system are [position,
|
||||
* velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the carriage.
|
||||
* @param mass The mass of the elevator carriage, in kilograms.
|
||||
* @param radius The radius of the elevator's driving drum, in meters.
|
||||
* @param gearing The reduction between motor and drum, as a ratio of output to input.
|
||||
* @return A LinearSystem representing the given characterized constants.
|
||||
* @throws IllegalArgumentException if mass <= 0, radius <= 0, or gearing <= 0.
|
||||
*/
|
||||
public static LinearSystem<N2, N1, N2> createElevatorSystem(
|
||||
DCMotor motor, double mass, double radius, double gearing) {
|
||||
if (mass <= 0.0) {
|
||||
throw new IllegalArgumentException("mass must be greater than zero.");
|
||||
}
|
||||
if (radius <= 0.0) {
|
||||
throw new IllegalArgumentException("radius must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0) {
|
||||
throw new IllegalArgumentException("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
return new LinearSystem<>(
|
||||
MatBuilder.fill(
|
||||
Nat.N2(),
|
||||
Nat.N2(),
|
||||
0,
|
||||
1,
|
||||
0,
|
||||
-Math.pow(gearing, 2) * motor.Kt / (motor.R * radius * radius * mass * motor.Kv)),
|
||||
VecBuilder.fill(0, gearing * motor.Kt / (motor.R * radius * mass)),
|
||||
Matrix.eye(Nat.N2()),
|
||||
new Matrix<>(Nat.N2(), Nat.N1()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of a flywheel system. The states of the system are [angular
|
||||
* velocity], inputs are [voltage], and outputs are [angular velocity].
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the flywheel.
|
||||
* @param J The moment of inertia J of the flywheel in kg-m².
|
||||
* @param gearing The reduction between motor and drum, as a ratio of output to input.
|
||||
* @return A LinearSystem representing the given characterized constants.
|
||||
* @throws IllegalArgumentException if J <= 0 or gearing <= 0.
|
||||
*/
|
||||
public static LinearSystem<N1, N1, N1> createFlywheelSystem(
|
||||
DCMotor motor, double J, double gearing) {
|
||||
if (J <= 0.0) {
|
||||
throw new IllegalArgumentException("J must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0.0) {
|
||||
throw new IllegalArgumentException("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
return new LinearSystem<>(
|
||||
VecBuilder.fill(-gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)),
|
||||
VecBuilder.fill(gearing * motor.Kt / (motor.R * J)),
|
||||
Matrix.eye(Nat.N1()),
|
||||
new Matrix<>(Nat.N1(), Nat.N1()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of a DC motor system. The states of the system are [angular
|
||||
* position, angular velocity]ᵀ, inputs are [voltage], and outputs are [angular position, angular
|
||||
* velocity]ᵀ.
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to system.
|
||||
* @param J The moment of inertia J of the DC motor in kg-m².
|
||||
* @param gearing The reduction between motor and drum, as a ratio of output to input.
|
||||
* @return A LinearSystem representing the given characterized constants.
|
||||
* @throws IllegalArgumentException if J <= 0 or gearing <= 0.
|
||||
*/
|
||||
public static LinearSystem<N2, N1, N2> createDCMotorSystem(
|
||||
DCMotor motor, double J, double gearing) {
|
||||
if (J <= 0.0) {
|
||||
throw new IllegalArgumentException("J must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0.0) {
|
||||
throw new IllegalArgumentException("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
return new LinearSystem<>(
|
||||
MatBuilder.fill(
|
||||
Nat.N2(), Nat.N2(), 0, 1, 0, -gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)),
|
||||
VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)),
|
||||
Matrix.eye(Nat.N2()),
|
||||
new Matrix<>(Nat.N2(), Nat.N1()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of a DC motor system from its kV (volts/(unit/sec)) and kA
|
||||
* (volts/(unit/sec²)). These constants can be found using SysId. the states of the system are
|
||||
* [position, velocity]ᵀ, inputs are [voltage], and outputs are [position].
|
||||
*
|
||||
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
|
||||
* {@link org.wpilib.math.util.Units} class for converting between unit types.
|
||||
*
|
||||
* <p>The parameters provided by the user are from this feedforward model:
|
||||
*
|
||||
* <p>u = K_v v + K_a a
|
||||
*
|
||||
* @param kV The velocity gain, in volts/(unit/sec)
|
||||
* @param kA The acceleration gain, in volts/(unit/sec²)
|
||||
* @return A LinearSystem representing the given characterized constants.
|
||||
* @throws IllegalArgumentException if kV < 0 or kA <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
public static LinearSystem<N2, N1, N2> createDCMotorSystem(double kV, double kA) {
|
||||
if (kV < 0.0) {
|
||||
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
|
||||
}
|
||||
if (kA <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka must be greater than zero.");
|
||||
}
|
||||
|
||||
return new LinearSystem<>(
|
||||
MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kV / kA),
|
||||
VecBuilder.fill(0, 1 / kA),
|
||||
Matrix.eye(Nat.N2()),
|
||||
new Matrix<>(Nat.N2(), Nat.N1()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of a differential drive drivetrain. In this model, the states are
|
||||
* [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are
|
||||
* [left velocity, right velocity]ᵀ.
|
||||
*
|
||||
* @param motor The motor (or gearbox) driving the drivetrain.
|
||||
* @param mass The mass of the robot in kilograms.
|
||||
* @param r The radius of the wheels in meters.
|
||||
* @param rb The radius of the base (half the trackwidth) in meters.
|
||||
* @param J The moment of inertia of the robot in kg-m².
|
||||
* @param gearing The gearing reduction as output over input.
|
||||
* @return A LinearSystem representing a differential drivetrain.
|
||||
* @throws IllegalArgumentException if m <= 0, r <= 0, rb <= 0, J <= 0, or gearing
|
||||
* <= 0.
|
||||
*/
|
||||
public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
|
||||
DCMotor motor, double mass, double r, double rb, double J, double gearing) {
|
||||
if (mass <= 0.0) {
|
||||
throw new IllegalArgumentException("mass must be greater than zero.");
|
||||
}
|
||||
if (r <= 0.0) {
|
||||
throw new IllegalArgumentException("r must be greater than zero.");
|
||||
}
|
||||
if (rb <= 0.0) {
|
||||
throw new IllegalArgumentException("rb must be greater than zero.");
|
||||
}
|
||||
if (J <= 0.0) {
|
||||
throw new IllegalArgumentException("J must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0.0) {
|
||||
throw new IllegalArgumentException("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
var C1 = -(gearing * gearing) * motor.Kt / (motor.Kv * motor.R * r * r);
|
||||
var C2 = gearing * motor.Kt / (motor.R * r);
|
||||
|
||||
final double C3 = 1 / mass + rb * rb / J;
|
||||
final double C4 = 1 / mass - rb * rb / J;
|
||||
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1);
|
||||
var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2);
|
||||
var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0);
|
||||
var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0);
|
||||
|
||||
return new LinearSystem<>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of a single jointed arm system. The states of the system are [angle,
|
||||
* angular velocity]ᵀ, inputs are [voltage], and outputs are [angle, angular velocity]ᵀ.
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the arm.
|
||||
* @param J The moment of inertia J of the arm in kg-m².
|
||||
* @param gearing The gearing between the motor and arm, in output over input. Most of the time
|
||||
* this will be greater than 1.
|
||||
* @return A LinearSystem representing the given characterized constants.
|
||||
*/
|
||||
public static LinearSystem<N2, N1, N2> createSingleJointedArmSystem(
|
||||
DCMotor motor, double J, double gearing) {
|
||||
if (J <= 0.0) {
|
||||
throw new IllegalArgumentException("J must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0.0) {
|
||||
throw new IllegalArgumentException("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
return new LinearSystem<>(
|
||||
MatBuilder.fill(
|
||||
Nat.N2(),
|
||||
Nat.N2(),
|
||||
0,
|
||||
1,
|
||||
0,
|
||||
-Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)),
|
||||
VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)),
|
||||
Matrix.eye(Nat.N2()),
|
||||
new Matrix<>(Nat.N2(), Nat.N1()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model for a 1 DOF velocity system from its kV (volts/(unit/sec)) and kA
|
||||
* (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
|
||||
* [velocity], inputs are [voltage], and outputs are [velocity].
|
||||
*
|
||||
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
|
||||
* {@link org.wpilib.math.util.Units} class for converting between unit types.
|
||||
*
|
||||
* <p>The parameters provided by the user are from this feedforward model:
|
||||
*
|
||||
* <p>u = K_v v + K_a a
|
||||
*
|
||||
* @param kV The velocity gain, in volts/(unit/sec)
|
||||
* @param kA The acceleration gain, in volts/(unit/sec²)
|
||||
* @return A LinearSystem representing the given characterized constants.
|
||||
* @throws IllegalArgumentException if kV < 0 or kA <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
|
||||
if (kV < 0.0) {
|
||||
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
|
||||
}
|
||||
if (kA <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka must be greater than zero.");
|
||||
}
|
||||
|
||||
return new LinearSystem<>(
|
||||
VecBuilder.fill(-kV / kA),
|
||||
VecBuilder.fill(1.0 / kA),
|
||||
VecBuilder.fill(1.0),
|
||||
VecBuilder.fill(0.0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA
|
||||
* (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are
|
||||
* [position, velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ.
|
||||
*
|
||||
* <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
|
||||
* {@link org.wpilib.math.util.Units} class for converting between unit types.
|
||||
*
|
||||
* <p>The parameters provided by the user are from this feedforward model:
|
||||
*
|
||||
* <p>u = K_v v + K_a a
|
||||
*
|
||||
* @param kV The velocity gain, in volts/(unit/sec)
|
||||
* @param kA The acceleration gain, in volts/(unit/sec²)
|
||||
* @return A LinearSystem representing the given characterized constants.
|
||||
* @throws IllegalArgumentException if kV < 0 or kA <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
public static LinearSystem<N2, N1, N2> identifyPositionSystem(double kV, double kA) {
|
||||
if (kV < 0.0) {
|
||||
throw new IllegalArgumentException("Kv must be greater than or equal to zero.");
|
||||
}
|
||||
if (kA <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka must be greater than zero.");
|
||||
}
|
||||
|
||||
return new LinearSystem<>(
|
||||
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA),
|
||||
VecBuilder.fill(0.0, 1.0 / kA),
|
||||
Matrix.eye(Nat.N2()),
|
||||
new Matrix<>(Nat.N2(), Nat.N1()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear
|
||||
* {(volts/(meter/sec), (volts/(meter/sec²))} and angular {(volts/(radian/sec)),
|
||||
* (volts/(radian/sec²))} cases. These constants can be found using SysId.
|
||||
*
|
||||
* <p>States: [[left velocity], [right velocity]]<br>
|
||||
* Inputs: [[left voltage], [right voltage]]<br>
|
||||
* Outputs: [[left velocity], [right velocity]]
|
||||
*
|
||||
* @param kVLinear The linear velocity gain in volts per (meters per second).
|
||||
* @param kALinear The linear acceleration gain in volts per (meters per second squared).
|
||||
* @param kVAngular The angular velocity gain in volts per (meters per second).
|
||||
* @param kAAngular The angular acceleration gain in volts per (meters per second squared).
|
||||
* @return A LinearSystem representing the given characterized constants.
|
||||
* @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, or
|
||||
* kAAngular <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
|
||||
double kVLinear, double kALinear, double kVAngular, double kAAngular) {
|
||||
if (kVLinear <= 0.0) {
|
||||
throw new IllegalArgumentException("Kv,linear must be greater than zero.");
|
||||
}
|
||||
if (kALinear <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka,linear must be greater than zero.");
|
||||
}
|
||||
if (kVAngular <= 0.0) {
|
||||
throw new IllegalArgumentException("Kv,angular must be greater than zero.");
|
||||
}
|
||||
if (kAAngular <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka,angular must be greater than zero.");
|
||||
}
|
||||
|
||||
final double A1 = 0.5 * -(kVLinear / kALinear + kVAngular / kAAngular);
|
||||
final double A2 = 0.5 * -(kVLinear / kALinear - kVAngular / kAAngular);
|
||||
final double B1 = 0.5 * (1.0 / kALinear + 1.0 / kAAngular);
|
||||
final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular);
|
||||
|
||||
return new LinearSystem<>(
|
||||
MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1),
|
||||
MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1),
|
||||
MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1),
|
||||
MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0));
|
||||
}
|
||||
|
||||
/**
|
||||
* Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear
|
||||
* {(volts/(meter/sec)), (volts/(meter/sec²))} and angular {(volts/(radian/sec)),
|
||||
* (volts/(radian/sec²))} cases. This can be found using SysId.
|
||||
*
|
||||
* <p>States: [[left velocity], [right velocity]]<br>
|
||||
* Inputs: [[left voltage], [right voltage]]<br>
|
||||
* Outputs: [[left velocity], [right velocity]]
|
||||
*
|
||||
* @param kVLinear The linear velocity gain in volts per (meters per second).
|
||||
* @param kALinear The linear acceleration gain in volts per (meters per second squared).
|
||||
* @param kVAngular The angular velocity gain in volts per (radians per second).
|
||||
* @param kAAngular The angular acceleration gain in volts per (radians per second squared).
|
||||
* @param trackwidth The distance between the differential drive's left and right wheels, in
|
||||
* meters.
|
||||
* @return A LinearSystem representing the given characterized constants.
|
||||
* @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
|
||||
* kAAngular <= 0, or trackwidth <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
|
||||
double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
|
||||
if (kVLinear <= 0.0) {
|
||||
throw new IllegalArgumentException("Kv,linear must be greater than zero.");
|
||||
}
|
||||
if (kALinear <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka,linear must be greater than zero.");
|
||||
}
|
||||
if (kVAngular <= 0.0) {
|
||||
throw new IllegalArgumentException("Kv,angular must be greater than zero.");
|
||||
}
|
||||
if (kAAngular <= 0.0) {
|
||||
throw new IllegalArgumentException("Ka,angular must be greater than zero.");
|
||||
}
|
||||
if (trackwidth <= 0.0) {
|
||||
throw new IllegalArgumentException("trackwidth must be greater than zero.");
|
||||
}
|
||||
|
||||
// We want to find a factor to include in Kv,angular that will convert
|
||||
// `u = Kv,angular omega` to `u = Kv,angular v`.
|
||||
//
|
||||
// v = omega r
|
||||
// omega = v/r
|
||||
// omega = 1/r v
|
||||
// omega = 1/(trackwidth/2) v
|
||||
// omega = 2/trackwidth v
|
||||
//
|
||||
// So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
|
||||
// to V/(m/s).
|
||||
return identifyDrivetrainSystem(
|
||||
kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
|
||||
}
|
||||
}
|
||||
@@ -2,10 +2,10 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.math.system.plant.proto;
|
||||
package org.wpilib.math.system.proto;
|
||||
|
||||
import org.wpilib.math.proto.Plant.ProtobufDCMotor;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.proto.System.ProtobufDCMotor;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.util.protobuf.Protobuf;
|
||||
import us.hebi.quickbuf.Descriptors.Descriptor;
|
||||
|
||||
@@ -2,10 +2,10 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.math.system.plant.struct;
|
||||
package org.wpilib.math.system.struct;
|
||||
|
||||
import java.nio.ByteBuffer;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.util.struct.Struct;
|
||||
|
||||
public class DCMotorStruct implements Struct<DCMotor> {
|
||||
@@ -2,11 +2,11 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "wpi/math/system/plant/proto/DCMotorProto.hpp"
|
||||
#include "wpi/math/system/proto/DCMotorProto.hpp"
|
||||
|
||||
#include <optional>
|
||||
|
||||
#include "wpimath/protobuf/plant.npb.h"
|
||||
#include "wpimath/protobuf/system.npb.h"
|
||||
|
||||
std::optional<wpi::math::DCMotor>
|
||||
wpi::util::Protobuf<wpi::math::DCMotor>::Unpack(InputStream& stream) {
|
||||
@@ -2,7 +2,7 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "wpi/math/system/plant/struct/DCMotorStruct.hpp"
|
||||
#include "wpi/math/system/struct/DCMotorStruct.hpp"
|
||||
|
||||
namespace {
|
||||
constexpr size_t kNominalVoltageOff = 0;
|
||||
@@ -23,8 +23,7 @@ namespace wpi::math {
|
||||
* Filters the provided voltages to limit a differential drive's linear and
|
||||
* angular acceleration.
|
||||
*
|
||||
* The differential drive model can be created via the functions in
|
||||
* LinearSystemId.
|
||||
* The differential drive model can be created via the functions in Models.
|
||||
*/
|
||||
class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
|
||||
public:
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
|
||||
#include "wpi/math/controller/DifferentialDriveWheelVoltages.hpp"
|
||||
#include "wpi/math/system/LinearSystem.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/units/acceleration.hpp"
|
||||
#include "wpi/units/angular_acceleration.hpp"
|
||||
#include "wpi/units/angular_velocity.hpp"
|
||||
@@ -42,7 +42,7 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
|
||||
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_rad_per_s) kVAngular,
|
||||
decltype(1_V / 1_rad_per_s_sq) kAAngular, wpi::units::meter_t trackwidth)
|
||||
// See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps),
|
||||
// See Models::DifferentialDriveFromSysId(decltype(1_V / 1_mps),
|
||||
// decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V /
|
||||
// 1_rad_per_s_sq))
|
||||
: DifferentialDriveFeedforward{kVLinear, kALinear,
|
||||
@@ -64,7 +64,7 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
|
||||
decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_mps) kVAngular,
|
||||
decltype(1_V / 1_mps_sq) kAAngular)
|
||||
: m_plant{wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
|
||||
: m_plant{wpi::math::Models::DifferentialDriveFromSysId(
|
||||
kVLinear, kALinear, kVAngular, kAAngular)},
|
||||
m_kVLinear{kVLinear},
|
||||
m_kALinear{kALinear},
|
||||
|
||||
@@ -6,7 +6,6 @@
|
||||
|
||||
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"
|
||||
#include "wpi/math/linalg/EigenCore.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/util/MathShared.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
@@ -70,47 +69,6 @@ class ElevatorFeedforward {
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and setpoints assuming continuous
|
||||
* control.
|
||||
*
|
||||
* @param velocity The velocity setpoint.
|
||||
* @param acceleration The acceleration setpoint.
|
||||
* @return The computed feedforward, in volts.
|
||||
* @deprecated Use the current/next velocity overload instead.
|
||||
*/
|
||||
[[deprecated("Use the current/next velocity overload instead.")]]
|
||||
constexpr wpi::units::volt_t Calculate(
|
||||
wpi::units::unit_t<Velocity> velocity,
|
||||
wpi::units::unit_t<Acceleration> acceleration) const {
|
||||
return kS * wpi::util::sgn(velocity) + kG + kV * velocity +
|
||||
kA * acceleration;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and setpoints assuming continuous
|
||||
* control.
|
||||
*
|
||||
* @param currentVelocity The current velocity setpoint.
|
||||
* @param nextVelocity The next velocity setpoint.
|
||||
* @param dt Time between velocity setpoints in seconds.
|
||||
* @return The computed feedforward, in volts.
|
||||
*/
|
||||
[[deprecated("Use the current/next velocity overload instead.")]]
|
||||
wpi::units::volt_t Calculate(wpi::units::unit_t<Velocity> currentVelocity,
|
||||
wpi::units::unit_t<Velocity> nextVelocity,
|
||||
wpi::units::second_t dt) const {
|
||||
// See wpimath/algorithms.md#Elevator_feedforward for derivation
|
||||
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
|
||||
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
|
||||
|
||||
Vectord<1> r{{currentVelocity.value()}};
|
||||
Vectord<1> nextR{{nextVelocity.value()}};
|
||||
|
||||
return kG + kS * wpi::util::sgn(currentVelocity.value()) +
|
||||
wpi::units::volt_t{feedforward.Calculate(r, nextR)(0)};
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the feedforward from the gains and setpoint assuming discrete
|
||||
* control. Use this method when the setpoint does not change.
|
||||
|
||||
@@ -289,5 +289,5 @@ class WPILIB_DLLEXPORT DCMotor {
|
||||
|
||||
} // namespace wpi::math
|
||||
|
||||
#include "wpi/math/system/plant/proto/DCMotorProto.hpp"
|
||||
#include "wpi/math/system/plant/struct/DCMotorStruct.hpp"
|
||||
#include "wpi/math/system/proto/DCMotorProto.hpp"
|
||||
#include "wpi/math/system/struct/DCMotorStruct.hpp"
|
||||
@@ -4,13 +4,12 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <concepts>
|
||||
#include <stdexcept>
|
||||
|
||||
#include <gcem.hpp>
|
||||
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/LinearSystem.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/units/acceleration.hpp"
|
||||
#include "wpi/units/angular_acceleration.hpp"
|
||||
#include "wpi/units/angular_velocity.hpp"
|
||||
@@ -22,9 +21,9 @@
|
||||
|
||||
namespace wpi::math {
|
||||
/**
|
||||
* Linear system ID utility functions.
|
||||
* Linear system factories.
|
||||
*/
|
||||
class WPILIB_DLLEXPORT LinearSystemId {
|
||||
class WPILIB_DLLEXPORT Models {
|
||||
public:
|
||||
template <typename Distance>
|
||||
using Velocity_t = wpi::units::unit_t<wpi::units::compound_unit<
|
||||
@@ -37,17 +36,80 @@ class WPILIB_DLLEXPORT LinearSystemId {
|
||||
wpi::units::inverse<wpi::units::seconds>>>;
|
||||
|
||||
/**
|
||||
* Create a state-space model of the elevator system. The states of the system
|
||||
* are [position, velocity]ᵀ, inputs are [voltage], and outputs are [position,
|
||||
* velocity]ᵀ.
|
||||
* Creates a flywheel state-space model from physical constants.
|
||||
*
|
||||
* The states are [angular velocity], the inputs are [voltage], and the
|
||||
* outputs are [angular velocity].
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the flywheel.
|
||||
* @param J The moment of inertia J of the flywheel.
|
||||
* @param gearing Gear ratio from motor to flywheel (greater than 1 is a
|
||||
* reduction).
|
||||
* @throws std::domain_error if J <= 0 or gearing <= 0.
|
||||
*/
|
||||
static constexpr LinearSystem<1, 1, 1> FlywheelFromPhysicalConstants(
|
||||
DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) {
|
||||
if (J <= 0_kg_sq_m) {
|
||||
throw std::domain_error("J must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0.0) {
|
||||
throw std::domain_error("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
Matrixd<1, 1> A{
|
||||
{(-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
|
||||
.value()}};
|
||||
Matrixd<1, 1> B{{(gearing * motor.Kt / (motor.R * J)).value()}};
|
||||
Matrixd<1, 1> C{{1.0}};
|
||||
Matrixd<1, 1> D{{0.0}};
|
||||
|
||||
return LinearSystem<1, 1, 1>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a flywheel state-space model from SysId constants kᵥ (V/(rad/s))
|
||||
* and kₐ (V/(rad/s²)) from the feedforward model u = kᵥv + kₐa.
|
||||
*
|
||||
* The states are [velocity], the inputs are [voltage], and the outputs are
|
||||
* [velocity].
|
||||
*
|
||||
* @param kV The velocity gain, in V/(rad/s).
|
||||
* @param kA The acceleration gain, in V/(rad/s²).
|
||||
* @throws std::domain_error if kV < 0 or kA <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
static constexpr LinearSystem<1, 1, 1> FlywheelFromSysId(
|
||||
decltype(1_V / 1_rad_per_s) kV, decltype(1_V / 1_rad_per_s_sq) kA) {
|
||||
if (kV < decltype(kV){0}) {
|
||||
throw std::domain_error("Kv must be greater than or equal to zero.");
|
||||
}
|
||||
if (kA <= decltype(kA){0}) {
|
||||
throw std::domain_error("Ka must be greater than zero.");
|
||||
}
|
||||
|
||||
Matrixd<1, 1> A{{-kV.value() / kA.value()}};
|
||||
Matrixd<1, 1> B{{1.0 / kA.value()}};
|
||||
Matrixd<1, 1> C{{1.0}};
|
||||
Matrixd<1, 1> D{{0.0}};
|
||||
|
||||
return LinearSystem<1, 1, 1>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates an elevator state-space model from physical constants.
|
||||
*
|
||||
* The states are [position, velocity], the inputs are [voltage], and the
|
||||
* outputs are [position, velocity].
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the carriage.
|
||||
* @param mass The mass of the elevator carriage, in kilograms.
|
||||
* @param radius The radius of the elevator's driving drum, in meters.
|
||||
* @param gearing Gear ratio from motor to carriage.
|
||||
* @param gearing Gear ratio from motor to carriage (greater than 1 is a
|
||||
* reduction).
|
||||
* @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0.
|
||||
*/
|
||||
static constexpr LinearSystem<2, 1, 2> ElevatorSystem(
|
||||
static constexpr LinearSystem<2, 1, 2> ElevatorFromPhysicalConstants(
|
||||
DCMotor motor, wpi::units::kilogram_t mass, wpi::units::meter_t radius,
|
||||
double gearing) {
|
||||
if (mass <= 0_kg) {
|
||||
@@ -74,105 +136,21 @@ class WPILIB_DLLEXPORT LinearSystemId {
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of a single-jointed arm system.The states of the
|
||||
* system are [angle, angular velocity]ᵀ, inputs are [voltage], and outputs
|
||||
* are [angle, angular velocity]ᵀ.
|
||||
* Creates an elevator state-space model from SysId constants kᵥ (V/(m/s)) and
|
||||
* kₐ (V/(m/s²)) from the feedforward model u = kᵥv + kₐa.
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the arm.
|
||||
* @param J The moment of inertia J of the arm.
|
||||
* @param gearing Gear ratio from motor to arm.
|
||||
* @throws std::domain_error if J <= 0 or gearing <= 0.
|
||||
*/
|
||||
static constexpr LinearSystem<2, 1, 2> SingleJointedArmSystem(
|
||||
DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) {
|
||||
if (J <= 0_kg_sq_m) {
|
||||
throw std::domain_error("J must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0.0) {
|
||||
throw std::domain_error("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
Matrixd<2, 2> A{
|
||||
{0.0, 1.0},
|
||||
{0.0, (-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
|
||||
.value()}};
|
||||
Matrixd<2, 1> B{{0.0}, {(gearing * motor.Kt / (motor.R * J)).value()}};
|
||||
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
|
||||
Matrixd<2, 1> D{{0.0}, {0.0}};
|
||||
|
||||
return LinearSystem<2, 1, 2>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model for a 1 DOF velocity system from its kV
|
||||
* (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
|
||||
* found using SysId. The states of the system are [velocity], inputs are
|
||||
* [voltage], and outputs are [velocity].
|
||||
* The states are [position, velocity], the inputs are [voltage], and the
|
||||
* outputs are [position, velocity].
|
||||
*
|
||||
* You MUST use an SI unit (i.e. meters or radians) for the Distance template
|
||||
* argument. You may still use non-SI units (such as feet or inches) for the
|
||||
* actual method arguments; they will automatically be converted to SI
|
||||
* internally.
|
||||
*
|
||||
* The parameters provided by the user are from this feedforward model:
|
||||
*
|
||||
* u = K_v v + K_a a
|
||||
*
|
||||
* @param kV The velocity gain, in volts/(unit/sec).
|
||||
* @param kA The acceleration gain, in volts/(unit/sec²).
|
||||
* @throws std::domain_error if kV < 0 or kA <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
template <typename Distance>
|
||||
requires std::same_as<wpi::units::meter, Distance> ||
|
||||
std::same_as<wpi::units::radian, Distance>
|
||||
static constexpr LinearSystem<1, 1, 1> IdentifyVelocitySystem(
|
||||
decltype(1_V / Velocity_t<Distance>(1)) kV,
|
||||
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
|
||||
if (kV < decltype(kV){0}) {
|
||||
throw std::domain_error("Kv must be greater than or equal to zero.");
|
||||
}
|
||||
if (kA <= decltype(kA){0}) {
|
||||
throw std::domain_error("Ka must be greater than zero.");
|
||||
}
|
||||
|
||||
Matrixd<1, 1> A{{-kV.value() / kA.value()}};
|
||||
Matrixd<1, 1> B{{1.0 / kA.value()}};
|
||||
Matrixd<1, 1> C{{1.0}};
|
||||
Matrixd<1, 1> D{{0.0}};
|
||||
|
||||
return LinearSystem<1, 1, 1>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model for a 1 DOF position system from its kV
|
||||
* (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
|
||||
* found using SysId. the states of the system are [position, velocity]ᵀ,
|
||||
* inputs are [voltage], and outputs are [position, velocity]ᵀ.
|
||||
*
|
||||
* You MUST use an SI unit (i.e. meters or radians) for the Distance template
|
||||
* argument. You may still use non-SI units (such as feet or inches) for the
|
||||
* actual method arguments; they will automatically be converted to SI
|
||||
* internally.
|
||||
*
|
||||
* The parameters provided by the user are from this feedforward model:
|
||||
*
|
||||
* u = K_v v + K_a a
|
||||
*
|
||||
* @param kV The velocity gain, in volts/(unit/sec).
|
||||
* @param kA The acceleration gain, in volts/(unit/sec²).
|
||||
* @param kV The velocity gain, in V/(m/s).
|
||||
* @param kA The acceleration gain, in V/(m/s²).
|
||||
*
|
||||
* @throws std::domain_error if kV < 0 or kA <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
template <typename Distance>
|
||||
requires std::same_as<wpi::units::meter, Distance> ||
|
||||
std::same_as<wpi::units::radian, Distance>
|
||||
static constexpr LinearSystem<2, 1, 2> IdentifyPositionSystem(
|
||||
decltype(1_V / Velocity_t<Distance>(1)) kV,
|
||||
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
|
||||
static constexpr LinearSystem<2, 1, 2> ElevatorFromSysId(
|
||||
decltype(1_V / 1_mps) kV, decltype(1_V / 1_mps_sq) kA) {
|
||||
if (kV < decltype(kV){0}) {
|
||||
throw std::domain_error("Kv must be greater than or equal to zero.");
|
||||
}
|
||||
@@ -189,167 +167,18 @@ class WPILIB_DLLEXPORT LinearSystemId {
|
||||
}
|
||||
|
||||
/**
|
||||
* Identify a differential drive drivetrain given the drivetrain's kV and kA
|
||||
* in both linear {(volts/(meter/sec), (volts/(meter/sec²))} and angular
|
||||
* {(volts/(radian/sec), (volts/(radian/sec²))} cases. These constants can be
|
||||
* found using SysId.
|
||||
* Create a single-jointed arm state-space model from physical constants.
|
||||
*
|
||||
* States: [[left velocity], [right velocity]]<br>
|
||||
* Inputs: [[left voltage], [right voltage]]<br>
|
||||
* Outputs: [[left velocity], [right velocity]]
|
||||
* The states are [angle, angular velocity], the inputs are [voltage], and the
|
||||
* outputs are [angle, angular velocity].
|
||||
*
|
||||
* @param kVLinear The linear velocity gain in volts per (meters per second).
|
||||
* @param kALinear The linear acceleration gain in volts per (meters per
|
||||
* second squared).
|
||||
* @param kVAngular The angular velocity gain in volts per (meters per
|
||||
* second).
|
||||
* @param kAAngular The angular acceleration gain in volts per (meters per
|
||||
* second squared).
|
||||
* @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
|
||||
* or kAAngular <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
static constexpr LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
|
||||
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) {
|
||||
if (kVLinear <= decltype(kVLinear){0}) {
|
||||
throw std::domain_error("Kv,linear must be greater than zero.");
|
||||
}
|
||||
if (kALinear <= decltype(kALinear){0}) {
|
||||
throw std::domain_error("Ka,linear must be greater than zero.");
|
||||
}
|
||||
if (kVAngular <= decltype(kVAngular){0}) {
|
||||
throw std::domain_error("Kv,angular must be greater than zero.");
|
||||
}
|
||||
if (kAAngular <= decltype(kAAngular){0}) {
|
||||
throw std::domain_error("Ka,angular must be greater than zero.");
|
||||
}
|
||||
|
||||
double A1 = -(kVLinear.value() / kALinear.value() +
|
||||
kVAngular.value() / kAAngular.value());
|
||||
double A2 = -(kVLinear.value() / kALinear.value() -
|
||||
kVAngular.value() / kAAngular.value());
|
||||
double B1 = 1.0 / kALinear.value() + 1.0 / kAAngular.value();
|
||||
double B2 = 1.0 / kALinear.value() - 1.0 / kAAngular.value();
|
||||
|
||||
A1 /= 2.0;
|
||||
A2 /= 2.0;
|
||||
B1 /= 2.0;
|
||||
B2 /= 2.0;
|
||||
|
||||
Matrixd<2, 2> A{{A1, A2}, {A2, A1}};
|
||||
Matrixd<2, 2> B{{B1, B2}, {B2, B1}};
|
||||
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
|
||||
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
|
||||
|
||||
return LinearSystem<2, 2, 2>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Identify a differential drive drivetrain given the drivetrain's kV and kA
|
||||
* in both linear {(volts/(meter/sec)), (volts/(meter/sec²))} and angular
|
||||
* {(volts/(radian/sec)), (volts/(radian/sec²))} cases. This can be found
|
||||
* using SysId.
|
||||
*
|
||||
* States: [[left velocity], [right velocity]]<br>
|
||||
* Inputs: [[left voltage], [right voltage]]<br>
|
||||
* Outputs: [[left velocity], [right velocity]]
|
||||
*
|
||||
* @param kVLinear The linear velocity gain in volts per (meters per
|
||||
* second).
|
||||
* @param kALinear The linear acceleration gain in volts per (meters per
|
||||
* second squared).
|
||||
* @param kVAngular The angular velocity gain in volts per (radians per
|
||||
* second).
|
||||
* @param kAAngular The angular acceleration gain in volts per (radians per
|
||||
* second squared).
|
||||
* @param trackwidth The distance between the differential drive's left and
|
||||
* right wheels, in meters.
|
||||
* @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
|
||||
* kAAngular <= 0, or trackwidth <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
static constexpr LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
|
||||
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_rad_per_s) kVAngular,
|
||||
decltype(1_V / 1_rad_per_s_sq) kAAngular,
|
||||
wpi::units::meter_t trackwidth) {
|
||||
if (kVLinear <= decltype(kVLinear){0}) {
|
||||
throw std::domain_error("Kv,linear must be greater than zero.");
|
||||
}
|
||||
if (kALinear <= decltype(kALinear){0}) {
|
||||
throw std::domain_error("Ka,linear must be greater than zero.");
|
||||
}
|
||||
if (kVAngular <= decltype(kVAngular){0}) {
|
||||
throw std::domain_error("Kv,angular must be greater than zero.");
|
||||
}
|
||||
if (kAAngular <= decltype(kAAngular){0}) {
|
||||
throw std::domain_error("Ka,angular must be greater than zero.");
|
||||
}
|
||||
if (trackwidth <= 0_m) {
|
||||
throw std::domain_error("r must be greater than zero.");
|
||||
}
|
||||
|
||||
// We want to find a factor to include in Kv,angular that will convert
|
||||
// `u = Kv,angular omega` to `u = Kv,angular v`.
|
||||
//
|
||||
// v = omega r
|
||||
// omega = v/r
|
||||
// omega = 1/r v
|
||||
// omega = 1/(trackwidth/2) v
|
||||
// omega = 2/trackwidth v
|
||||
//
|
||||
// So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
|
||||
// to V/(m/s).
|
||||
return IdentifyDrivetrainSystem(kVLinear, kALinear,
|
||||
kVAngular * 2.0 / trackwidth * 1_rad,
|
||||
kAAngular * 2.0 / trackwidth * 1_rad);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of a flywheel system, the states of the system
|
||||
* are [angular velocity], inputs are [voltage], and outputs are [angular
|
||||
* velocity].
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the flywheel.
|
||||
* @param J The moment of inertia J of the flywheel.
|
||||
* @param gearing Gear ratio from motor to flywheel.
|
||||
* @param motor The motor (or gearbox) attached to the arm.
|
||||
* @param J The moment of inertia J of the arm.
|
||||
* @param gearing Gear ratio from motor to arm (greater than 1 is a
|
||||
* reduction).
|
||||
* @throws std::domain_error if J <= 0 or gearing <= 0.
|
||||
*/
|
||||
static constexpr LinearSystem<1, 1, 1> FlywheelSystem(
|
||||
DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) {
|
||||
if (J <= 0_kg_sq_m) {
|
||||
throw std::domain_error("J must be greater than zero.");
|
||||
}
|
||||
if (gearing <= 0.0) {
|
||||
throw std::domain_error("gearing must be greater than zero.");
|
||||
}
|
||||
|
||||
Matrixd<1, 1> A{
|
||||
{(-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J))
|
||||
.value()}};
|
||||
Matrixd<1, 1> B{{(gearing * motor.Kt / (motor.R * J)).value()}};
|
||||
Matrixd<1, 1> C{{1.0}};
|
||||
Matrixd<1, 1> D{{0.0}};
|
||||
|
||||
return LinearSystem<1, 1, 1>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of a DC motor system. The states of the system
|
||||
* are [angular position, angular velocity]ᵀ, inputs are [voltage], and
|
||||
* outputs are [angular position, angular velocity]ᵀ.
|
||||
*
|
||||
* @param motor The motor (or gearbox) attached to the system.
|
||||
* @param J the moment of inertia J of the DC motor.
|
||||
* @param gearing Gear ratio from motor to output.
|
||||
* @throws std::domain_error if J <= 0 or gearing <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
static constexpr LinearSystem<2, 1, 2> DCMotorSystem(
|
||||
static constexpr LinearSystem<2, 1, 2> SingleJointedArmFromPhysicalConstants(
|
||||
DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) {
|
||||
if (J <= 0_kg_sq_m) {
|
||||
throw std::domain_error("J must be greater than zero.");
|
||||
@@ -370,32 +199,21 @@ class WPILIB_DLLEXPORT LinearSystemId {
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of a DC motor system from its kV
|
||||
* (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be
|
||||
* found using SysId. the states of the system are [angular position, angular
|
||||
* velocity]ᵀ, inputs are [voltage], and outputs are [angular position,
|
||||
* angular velocity]ᵀ.
|
||||
* Creates a single-jointed arm state-space model from SysId constants kᵥ
|
||||
* (V/(rad/s)) and kₐ (V/(rad/s²)) from the feedforward model u = kᵥv + kₐa.
|
||||
*
|
||||
* You MUST use an SI unit (i.e. meters or radians) for the Distance template
|
||||
* argument. You may still use non-SI units (such as feet or inches) for the
|
||||
* actual method arguments; they will automatically be converted to SI
|
||||
* internally.
|
||||
*
|
||||
* The parameters provided by the user are from this feedforward model:
|
||||
*
|
||||
* u = K_v v + K_a a
|
||||
* The states are [position, velocity], the inputs are [voltage], and the
|
||||
* outputs are [position, velocity].
|
||||
*
|
||||
* @param kV The velocity gain, in volts/(unit/sec).
|
||||
* @param kA The acceleration gain, in volts/(unit/sec²).
|
||||
*
|
||||
* @throws std::domain_error if kV < 0 or kA <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
template <typename Distance>
|
||||
requires std::same_as<wpi::units::meter, Distance> ||
|
||||
std::same_as<wpi::units::radian, Distance>
|
||||
static constexpr LinearSystem<2, 1, 2> DCMotorSystem(
|
||||
decltype(1_V / Velocity_t<Distance>(1)) kV,
|
||||
decltype(1_V / Acceleration_t<Distance>(1)) kA) {
|
||||
static constexpr LinearSystem<2, 1, 2> SingleJointedArmFromSysId(
|
||||
decltype(1_V / 1_rad_per_s) kV, decltype(1_V / 1_rad_per_s_sq) kA) {
|
||||
if (kV < decltype(kV){0}) {
|
||||
throw std::domain_error("Kv must be greater than or equal to zero.");
|
||||
}
|
||||
@@ -404,7 +222,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
|
||||
}
|
||||
|
||||
Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
|
||||
Matrixd<2, 1> B{0.0, 1.0 / kA.value()};
|
||||
Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}};
|
||||
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
|
||||
Matrixd<2, 1> D{{0.0}, {0.0}};
|
||||
|
||||
@@ -412,21 +230,23 @@ class WPILIB_DLLEXPORT LinearSystemId {
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a state-space model of differential drive drivetrain. In this model,
|
||||
* the states are [left velocity, right velocity]ᵀ, the inputs are [left
|
||||
* Creates a differential drive state-space model from physical constants.
|
||||
*
|
||||
* The states are [left velocity, right velocity], the inputs are [left
|
||||
* voltage, right voltage], and the outputs are [left velocity, right
|
||||
* velocity]ᵀ.
|
||||
* velocity].
|
||||
*
|
||||
* @param motor The motor (or gearbox) driving the drivetrain.
|
||||
* @param mass The mass of the robot in kilograms.
|
||||
* @param r The radius of the wheels in meters.
|
||||
* @param rb The radius of the base (half of the trackwidth), in meters.
|
||||
* @param J The moment of inertia of the robot.
|
||||
* @param gearing Gear ratio from motor to wheel.
|
||||
* @param gearing Gear ratio from motor to wheel (greater than 1 is a
|
||||
* reduction).
|
||||
* @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or
|
||||
* gearing <= 0.
|
||||
*/
|
||||
static constexpr LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
|
||||
static constexpr LinearSystem<2, 2, 2> DifferentialDriveFromPhysicalConstants(
|
||||
const DCMotor& motor, wpi::units::kilogram_t mass, wpi::units::meter_t r,
|
||||
wpi::units::meter_t rb, wpi::units::kilogram_square_meter_t J,
|
||||
double gearing) {
|
||||
@@ -465,6 +285,119 @@ class WPILIB_DLLEXPORT LinearSystemId {
|
||||
|
||||
return LinearSystem<2, 2, 2>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a differential drive state-space model from SysId constants kᵥ and
|
||||
* kₐ in both linear {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s),
|
||||
* (V/(rad/s²))} cases.
|
||||
*
|
||||
* The states are [left velocity, right velocity], the inputs are [left
|
||||
* voltage, right voltage], and the outputs are [left velocity, right
|
||||
* velocity].
|
||||
*
|
||||
* @param kVLinear The linear velocity gain in volts per (meters per second).
|
||||
* @param kALinear The linear acceleration gain in volts per (meters per
|
||||
* second squared).
|
||||
* @param kVAngular The angular velocity gain in volts per (meters per
|
||||
* second).
|
||||
* @param kAAngular The angular acceleration gain in volts per (meters per
|
||||
* second squared).
|
||||
* @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
|
||||
* or kAAngular <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
static constexpr LinearSystem<2, 2, 2> DifferentialDriveFromSysId(
|
||||
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) {
|
||||
if (kVLinear <= decltype(kVLinear){0}) {
|
||||
throw std::domain_error("Kv,linear must be greater than zero.");
|
||||
}
|
||||
if (kALinear <= decltype(kALinear){0}) {
|
||||
throw std::domain_error("Ka,linear must be greater than zero.");
|
||||
}
|
||||
if (kVAngular <= decltype(kVAngular){0}) {
|
||||
throw std::domain_error("Kv,angular must be greater than zero.");
|
||||
}
|
||||
if (kAAngular <= decltype(kAAngular){0}) {
|
||||
throw std::domain_error("Ka,angular must be greater than zero.");
|
||||
}
|
||||
|
||||
double A1 = -0.5 * (kVLinear.value() / kALinear.value() +
|
||||
kVAngular.value() / kAAngular.value());
|
||||
double A2 = -0.5 * (kVLinear.value() / kALinear.value() -
|
||||
kVAngular.value() / kAAngular.value());
|
||||
double B1 = 0.5 / kALinear.value() + 0.5 / kAAngular.value();
|
||||
double B2 = 0.5 / kALinear.value() - 0.5 / kAAngular.value();
|
||||
|
||||
Matrixd<2, 2> A{{A1, A2}, {A2, A1}};
|
||||
Matrixd<2, 2> B{{B1, B2}, {B2, B1}};
|
||||
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
|
||||
Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
|
||||
|
||||
return LinearSystem<2, 2, 2>(A, B, C, D);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a differential drive state-space model from SysId constants kᵥ and
|
||||
* kₐ in both linear {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s),
|
||||
* (V/(rad/s²))} cases.
|
||||
*
|
||||
* The states are [left velocity, right velocity], the inputs are [left
|
||||
* voltage, right voltage], and the outputs are [left velocity, right
|
||||
* velocity].
|
||||
*
|
||||
* @param kVLinear The linear velocity gain in volts per (meters per
|
||||
* second).
|
||||
* @param kALinear The linear acceleration gain in volts per (meters per
|
||||
* second squared).
|
||||
* @param kVAngular The angular velocity gain in volts per (radians per
|
||||
* second).
|
||||
* @param kAAngular The angular acceleration gain in volts per (radians per
|
||||
* second squared).
|
||||
* @param trackwidth The distance between the differential drive's left and
|
||||
* right wheels, in meters.
|
||||
* @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0,
|
||||
* kAAngular <= 0, or trackwidth <= 0.
|
||||
* @see <a
|
||||
* href="https://github.com/wpilibsuite/allwpilib/tree/main/sysid">https://github.com/wpilibsuite/allwpilib/tree/main/sysid</a>
|
||||
*/
|
||||
static constexpr LinearSystem<2, 2, 2> DifferentialDriveFromSysId(
|
||||
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_rad_per_s) kVAngular,
|
||||
decltype(1_V / 1_rad_per_s_sq) kAAngular,
|
||||
wpi::units::meter_t trackwidth) {
|
||||
if (kVLinear <= decltype(kVLinear){0}) {
|
||||
throw std::domain_error("Kv,linear must be greater than zero.");
|
||||
}
|
||||
if (kALinear <= decltype(kALinear){0}) {
|
||||
throw std::domain_error("Ka,linear must be greater than zero.");
|
||||
}
|
||||
if (kVAngular <= decltype(kVAngular){0}) {
|
||||
throw std::domain_error("Kv,angular must be greater than zero.");
|
||||
}
|
||||
if (kAAngular <= decltype(kAAngular){0}) {
|
||||
throw std::domain_error("Ka,angular must be greater than zero.");
|
||||
}
|
||||
if (trackwidth <= 0_m) {
|
||||
throw std::domain_error("r must be greater than zero.");
|
||||
}
|
||||
|
||||
// We want to find a factor to include in Kv,angular that will convert
|
||||
// `u = Kv,angular omega` to `u = Kv,angular v`.
|
||||
//
|
||||
// v = omega r
|
||||
// omega = v/r
|
||||
// omega = 1/r v
|
||||
// omega = 1/(trackwidth/2) v
|
||||
// omega = 2/trackwidth v
|
||||
//
|
||||
// So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
|
||||
// to V/(m/s).
|
||||
return DifferentialDriveFromSysId(kVLinear, kALinear,
|
||||
kVAngular * 2.0 / trackwidth * 1_rad,
|
||||
kAAngular * 2.0 / trackwidth * 1_rad);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace wpi::math
|
||||
@@ -4,10 +4,10 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/util/SymbolExports.hpp"
|
||||
#include "wpi/util/protobuf/Protobuf.hpp"
|
||||
#include "wpimath/protobuf/plant.npb.h"
|
||||
#include "wpimath/protobuf/system.npb.h"
|
||||
|
||||
template <>
|
||||
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::DCMotor> {
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/util/SymbolExports.hpp"
|
||||
#include "wpi/util/struct/Struct.hpp"
|
||||
|
||||
@@ -1,13 +0,0 @@
|
||||
syntax = "proto3";
|
||||
|
||||
package wpi.proto;
|
||||
|
||||
option java_package = "org.wpilib.math.proto";
|
||||
|
||||
message ProtobufDCMotor {
|
||||
double nominal_voltage = 1;
|
||||
double stall_torque = 2;
|
||||
double stall_current = 3;
|
||||
double free_current = 4;
|
||||
double free_speed = 5;
|
||||
}
|
||||
@@ -6,6 +6,14 @@ import "wpimath.proto";
|
||||
|
||||
option java_package = "org.wpilib.math.proto";
|
||||
|
||||
message ProtobufDCMotor {
|
||||
double nominal_voltage = 1;
|
||||
double stall_torque = 2;
|
||||
double stall_current = 3;
|
||||
double free_current = 4;
|
||||
double free_speed = 5;
|
||||
}
|
||||
|
||||
message ProtobufLinearSystem {
|
||||
uint32 num_states = 1;
|
||||
uint32 num_inputs = 2;
|
||||
|
||||
@@ -1560,17 +1560,15 @@ SplineHelper = "wpi/math/spline/SplineHelper.hpp"
|
||||
SplineParameterizer = "wpi/math/spline/SplineParameterizer.hpp"
|
||||
|
||||
# wpi/math/system
|
||||
DCMotor = "wpi/math/system/DCMotor.hpp"
|
||||
# Discretization = "wpi/math/system/Discretization.hpp"
|
||||
LinearSystem = "wpi/math/system/LinearSystem.hpp"
|
||||
LinearSystemLoop = "wpi/math/system/LinearSystemLoop.hpp"
|
||||
# LinearSystemUtil = "wpi/math/system/LinearSystemUtil.hpp"
|
||||
Models = "wpi/math/system/Models.hpp"
|
||||
# NumericalIntegration = "wpi/math/system/NumericalIntegration.hpp"
|
||||
# NumericalJacobian = "wpi/math/system/NumericalJacobian.hpp"
|
||||
|
||||
# wpi/math/system/plant
|
||||
DCMotor = "wpi/math/system/plant/DCMotor.hpp"
|
||||
LinearSystemId = "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
|
||||
# wpi/math/trajectory
|
||||
ExponentialProfile = "wpi/math/trajectory/ExponentialProfile.hpp"
|
||||
Trajectory = "wpi/math/trajectory/Trajectory.hpp"
|
||||
|
||||
@@ -10,10 +10,6 @@ classes:
|
||||
wpi::units::volt_t, wpi::units::volt_t, wpi::units::unit_t<kv_unit>, wpi::units::unit_t<ka_unit>:
|
||||
Calculate:
|
||||
overloads:
|
||||
wpi::units::unit_t<Velocity>, wpi::units::unit_t<Acceleration> [const]:
|
||||
ignore: true
|
||||
wpi::units::unit_t<Velocity>, wpi::units::unit_t<Velocity>, wpi::units::second_t [const]:
|
||||
ignore: true
|
||||
wpi::units::unit_t<Velocity> [const]:
|
||||
wpi::units::unit_t<Velocity>, wpi::units::unit_t<Velocity> [const]:
|
||||
MaxAchievableVelocity:
|
||||
|
||||
@@ -1,59 +0,0 @@
|
||||
classes:
|
||||
wpi::math::LinearSystemId:
|
||||
typealias:
|
||||
- kv_meters = wpi::units::unit_t<wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<wpi::units::meters_per_second>>>
|
||||
- kv_radians = wpi::units::unit_t<wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<wpi::units::radians_per_second>>>
|
||||
- ka_meters = wpi::units::unit_t<wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<wpi::units::meters_per_second_squared>>>
|
||||
- ka_radians = wpi::units::unit_t<wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<wpi::units::radians_per_second_squared>>>
|
||||
methods:
|
||||
ElevatorSystem:
|
||||
SingleJointedArmSystem:
|
||||
IdentifyVelocitySystem:
|
||||
rename: identifyVelocitySystemMeters
|
||||
cpp_code: |
|
||||
[](kv_meters kV, ka_meters kA) {
|
||||
return wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::meter>(kV, kA);
|
||||
}
|
||||
IdentifyPositionSystem:
|
||||
rename: identifyPositionSystemMeters
|
||||
cpp_code: |
|
||||
[](kv_meters kV, ka_meters kA) {
|
||||
return wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::meter>(kV, kA);
|
||||
}
|
||||
IdentifyDrivetrainSystem:
|
||||
overloads:
|
||||
decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_mps), decltype(1_V/1_mps_sq):
|
||||
cpp_code: |
|
||||
[](kv_meters kVlinear, ka_meters kAlinear, kv_meters kVangular, ka_meters kAangular) {
|
||||
return wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVlinear, kAlinear, kVangular, kAangular);
|
||||
}
|
||||
decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_rad_per_s), decltype(1_V/1_rad_per_s_sq), wpi::units::meter_t:
|
||||
cpp_code: |
|
||||
[](kv_meters kVlinear, ka_meters kAlinear, kv_radians kVangular, ka_radians kAangular, wpi::units::meter_t trackWidth) {
|
||||
return wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVlinear, kAlinear, kVangular, kAangular, trackWidth);
|
||||
}
|
||||
FlywheelSystem:
|
||||
DCMotorSystem:
|
||||
overloads:
|
||||
DCMotor, wpi::units::kilogram_square_meter_t, double:
|
||||
decltype(1_V/Velocity_t<Distance> (1)), decltype(1_V/Acceleration_t<Distance> (1)):
|
||||
cpp_code: |
|
||||
[](kv_meters kv, ka_meters ka) {
|
||||
return wpi::math::LinearSystemId::DCMotorSystem<wpi::units::meter>(kv, ka);
|
||||
}
|
||||
DrivetrainVelocitySystem:
|
||||
|
||||
inline_code: |
|
||||
.def_static("DCMotorSystemRadians", [](kv_radians kV, ka_radians kA) {
|
||||
return wpi::math::LinearSystemId::DCMotorSystem<wpi::units::radian>(kV, kA);
|
||||
}, py::arg("kV"), py::arg("kA"), release_gil()
|
||||
)
|
||||
|
||||
.def_static("identifyVelocitySystemRadians", [](kv_radians kV, ka_radians kA) {
|
||||
return wpi::math::LinearSystemId::IdentifyVelocitySystem<wpi::units::radian>(kV, kA);
|
||||
}, py::arg("kV"), py::arg("kA"), release_gil()
|
||||
)
|
||||
.def_static("identifyPositionSystemRadians", [](kv_radians kV, ka_radians kA) {
|
||||
return wpi::math::LinearSystemId::IdentifyPositionSystem<wpi::units::radian>(kV, kA);
|
||||
}, py::arg("kV"), py::arg("kA"), release_gil()
|
||||
)
|
||||
14
wpimath/src/main/python/semiwrap/Models.yml
Normal file
14
wpimath/src/main/python/semiwrap/Models.yml
Normal file
@@ -0,0 +1,14 @@
|
||||
classes:
|
||||
wpi::math::Models:
|
||||
methods:
|
||||
FlywheelFromPhysicalConstants:
|
||||
FlywheelFromSysId:
|
||||
ElevatorFromPhysicalConstants:
|
||||
ElevatorFromSysId:
|
||||
SingleJointedArmFromPhysicalConstants:
|
||||
SingleJointedArmFromSysId:
|
||||
DifferentialDriveFromPhysicalConstants:
|
||||
DifferentialDriveFromSysId:
|
||||
overloads:
|
||||
decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_mps), decltype(1_V/1_mps_sq):
|
||||
decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_rad_per_s), decltype(1_V/1_rad_per_s_sq), wpi::units::meter_t:
|
||||
@@ -60,7 +60,6 @@ from ._wpimath import (
|
||||
LinearQuadraticRegulator_2_1,
|
||||
LinearQuadraticRegulator_2_2,
|
||||
LinearQuadraticRegulator_3_2,
|
||||
LinearSystemId,
|
||||
LinearSystemLoop_1_1_1,
|
||||
LinearSystemLoop_2_1_1,
|
||||
LinearSystemLoop_2_1_2,
|
||||
@@ -94,6 +93,7 @@ from ._wpimath import (
|
||||
MecanumDriveWheelPositions,
|
||||
MecanumDriveWheelSpeeds,
|
||||
MedianFilter,
|
||||
Models,
|
||||
PIDController,
|
||||
Pose2d,
|
||||
Pose3d,
|
||||
@@ -247,7 +247,6 @@ __all__ = [
|
||||
"LinearQuadraticRegulator_2_1",
|
||||
"LinearQuadraticRegulator_2_2",
|
||||
"LinearQuadraticRegulator_3_2",
|
||||
"LinearSystemId",
|
||||
"LinearSystemLoop_1_1_1",
|
||||
"LinearSystemLoop_2_1_1",
|
||||
"LinearSystemLoop_2_1_2",
|
||||
@@ -281,6 +280,7 @@ __all__ = [
|
||||
"MecanumDriveWheelPositions",
|
||||
"MecanumDriveWheelSpeeds",
|
||||
"MedianFilter",
|
||||
"Models",
|
||||
"PIDController",
|
||||
"Pose2d",
|
||||
"Pose3d",
|
||||
|
||||
@@ -14,7 +14,7 @@ import org.wpilib.math.linalg.Matrix;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
|
||||
class DifferentialDriveAccelerationLimiterTest {
|
||||
@Test
|
||||
@@ -24,7 +24,7 @@ class DifferentialDriveAccelerationLimiterTest {
|
||||
final double maxA = 2.0;
|
||||
final double maxAlpha = 2.0;
|
||||
|
||||
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
|
||||
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
|
||||
|
||||
var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, maxA, maxAlpha);
|
||||
|
||||
@@ -114,7 +114,7 @@ class DifferentialDriveAccelerationLimiterTest {
|
||||
final double trackwidth = 0.9;
|
||||
final double dt = 0.005;
|
||||
|
||||
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
|
||||
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
|
||||
|
||||
// Limits are so high, they don't get hit, so states of constrained and
|
||||
// unconstrained systems should match
|
||||
@@ -178,7 +178,7 @@ class DifferentialDriveAccelerationLimiterTest {
|
||||
final double maxA = 2.0;
|
||||
final double maxAlpha = 2.0;
|
||||
|
||||
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
|
||||
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
|
||||
|
||||
var accelLimiter =
|
||||
new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha);
|
||||
@@ -237,7 +237,7 @@ class DifferentialDriveAccelerationLimiterTest {
|
||||
|
||||
@Test
|
||||
void testMinAccelGreaterThanMaxAccel() {
|
||||
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
|
||||
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
|
||||
assertDoesNotThrow(() -> new DifferentialDriveAccelerationLimiter(plant, 1, -1, 1, 1e3));
|
||||
assertThrows(
|
||||
IllegalArgumentException.class,
|
||||
|
||||
@@ -12,7 +12,7 @@ import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
|
||||
class DifferentialDriveFeedforwardTest {
|
||||
private static final double kVLinear = 1.0;
|
||||
@@ -27,8 +27,7 @@ class DifferentialDriveFeedforwardTest {
|
||||
DifferentialDriveFeedforward differentialDriveFeedforward =
|
||||
new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
|
||||
LinearSystem<N2, N2, N2> plant =
|
||||
LinearSystemId.identifyDrivetrainSystem(
|
||||
kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
|
||||
Models.differentialDriveFromSysId(kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
|
||||
for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
|
||||
for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
|
||||
for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {
|
||||
@@ -58,7 +57,7 @@ class DifferentialDriveFeedforwardTest {
|
||||
DifferentialDriveFeedforward differentialDriveFeedforward =
|
||||
new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular);
|
||||
LinearSystem<N2, N2, N2> plant =
|
||||
LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
|
||||
Models.differentialDriveFromSysId(kVLinear, kALinear, kVAngular, kAAngular);
|
||||
for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
|
||||
for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
|
||||
for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {
|
||||
|
||||
@@ -12,14 +12,14 @@ import org.wpilib.math.linalg.Matrix;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
|
||||
class ImplicitModelFollowerTest {
|
||||
@Test
|
||||
void testSameModel() {
|
||||
final double dt = 0.005;
|
||||
|
||||
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
|
||||
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
|
||||
|
||||
var imf = new ImplicitModelFollower<>(plant, plant);
|
||||
|
||||
@@ -61,10 +61,10 @@ class ImplicitModelFollowerTest {
|
||||
void testSlowerRefModel() {
|
||||
final double dt = 0.005;
|
||||
|
||||
var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
|
||||
var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0);
|
||||
|
||||
// Linear acceleration is slower, but angular acceleration is the same
|
||||
var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0);
|
||||
var plantRef = Models.differentialDriveFromSysId(1.0, 2.0, 1.0, 1.0);
|
||||
|
||||
var imf = new ImplicitModelFollower<>(plant, plantRef);
|
||||
|
||||
|
||||
@@ -18,8 +18,8 @@ import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.numbers.N5;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.system.NumericalIntegration;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.trajectory.TrajectoryConfig;
|
||||
import org.wpilib.math.trajectory.TrajectoryGenerator;
|
||||
import org.wpilib.math.util.MathUtil;
|
||||
@@ -52,7 +52,7 @@ class LTVDifferentialDriveControllerTest {
|
||||
private static final double kAngularV = 1.382; // V/(m/s)
|
||||
private static final double kAngularA = 0.08495; // V/(m/s²)
|
||||
private static final LinearSystem<N2, N2, N2> plant =
|
||||
LinearSystemId.identifyDrivetrainSystem(kLinearV, kLinearA, kAngularV, kAngularA);
|
||||
Models.differentialDriveFromSysId(kLinearV, kLinearA, kAngularV, kAngularA);
|
||||
private static final double kTrackwidth = 0.9;
|
||||
|
||||
private static Matrix<N5, N1> dynamics(Matrix<N5, N1> x, Matrix<N2, N1> u) {
|
||||
|
||||
@@ -10,9 +10,9 @@ import org.junit.jupiter.api.Test;
|
||||
import org.wpilib.math.linalg.MatBuilder;
|
||||
import org.wpilib.math.linalg.Matrix;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.Discretization;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.util.Nat;
|
||||
import org.wpilib.math.util.Num;
|
||||
|
||||
@@ -25,7 +25,7 @@ class LinearQuadraticRegulatorTest {
|
||||
var r = 0.0181864;
|
||||
var G = 1.0;
|
||||
|
||||
var plant = LinearSystemId.createElevatorSystem(motors, m, r, G);
|
||||
var plant = Models.elevatorFromPhysicalConstants(motors, m, r, G);
|
||||
|
||||
var qElms = VecBuilder.fill(0.02, 0.4);
|
||||
var rElms = VecBuilder.fill(12.0);
|
||||
@@ -42,7 +42,7 @@ class LinearQuadraticRegulatorTest {
|
||||
var dt = 0.020;
|
||||
|
||||
var plant =
|
||||
LinearSystemId.createElevatorSystem(
|
||||
Models.elevatorFromPhysicalConstants(
|
||||
DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
|
||||
|
||||
var K =
|
||||
@@ -61,7 +61,7 @@ class LinearQuadraticRegulatorTest {
|
||||
var r = 0.4;
|
||||
var G = 100.0;
|
||||
|
||||
var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G);
|
||||
var plant = Models.singleJointedArmFromPhysicalConstants(motors, 1d / 3d * m * r * r, G);
|
||||
|
||||
var qElms = VecBuilder.fill(0.01745, 0.08726);
|
||||
var rElms = VecBuilder.fill(12.0);
|
||||
@@ -160,7 +160,7 @@ class LinearQuadraticRegulatorTest {
|
||||
var dt = 0.02;
|
||||
|
||||
var plant =
|
||||
LinearSystemId.createElevatorSystem(
|
||||
Models.elevatorFromPhysicalConstants(
|
||||
DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
|
||||
|
||||
var regulator =
|
||||
|
||||
@@ -14,10 +14,10 @@ import org.wpilib.math.linalg.Matrix;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.LinearSystemLoop;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.trajectory.TrapezoidProfile;
|
||||
import org.wpilib.math.util.Nat;
|
||||
|
||||
@@ -27,7 +27,7 @@ class LinearSystemLoopTest {
|
||||
private static final Random random = new Random();
|
||||
|
||||
LinearSystem<N2, N1, N2> m_plant =
|
||||
LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0);
|
||||
Models.elevatorFromPhysicalConstants(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0);
|
||||
|
||||
@SuppressWarnings("unchecked")
|
||||
KalmanFilter<N2, N1, N1> m_observer =
|
||||
@@ -95,7 +95,7 @@ class LinearSystemLoopTest {
|
||||
@Test
|
||||
void testFlywheelEnabled() {
|
||||
LinearSystem<N1, N1, N1> plant =
|
||||
LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0);
|
||||
Models.flywheelFromPhysicalConstants(DCMotor.getNEO(2), 0.00289, 1.0);
|
||||
KalmanFilter<N1, N1, N1> observer =
|
||||
new KalmanFilter<>(
|
||||
Nat.N1(), Nat.N1(), plant, VecBuilder.fill(1.0), VecBuilder.fill(kPositionStddev), kDt);
|
||||
|
||||
@@ -18,9 +18,9 @@ import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.numbers.N3;
|
||||
import org.wpilib.math.numbers.N5;
|
||||
import org.wpilib.math.random.Normal;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.NumericalIntegration;
|
||||
import org.wpilib.math.system.NumericalJacobian;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.trajectory.TrajectoryConfig;
|
||||
import org.wpilib.math.trajectory.TrajectoryGenerator;
|
||||
import org.wpilib.math.util.Nat;
|
||||
|
||||
@@ -19,9 +19,9 @@ import org.wpilib.math.numbers.N1;
|
||||
import org.wpilib.math.numbers.N2;
|
||||
import org.wpilib.math.numbers.N3;
|
||||
import org.wpilib.math.numbers.N6;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.LinearSystem;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.trajectory.TrajectoryConfig;
|
||||
import org.wpilib.math.trajectory.TrajectoryGenerator;
|
||||
import org.wpilib.math.util.Nat;
|
||||
@@ -41,7 +41,7 @@ class KalmanFilterTest {
|
||||
var m = 5.0;
|
||||
var r = 0.0181864;
|
||||
var G = 1.0;
|
||||
elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
|
||||
elevatorPlant = Models.elevatorFromPhysicalConstants(motors, m, r, G);
|
||||
}
|
||||
|
||||
// A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]ᵀ,
|
||||
|
||||
@@ -23,11 +23,11 @@ import org.wpilib.math.numbers.N3;
|
||||
import org.wpilib.math.numbers.N4;
|
||||
import org.wpilib.math.numbers.N5;
|
||||
import org.wpilib.math.random.Normal;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.Discretization;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.system.NumericalIntegration;
|
||||
import org.wpilib.math.system.NumericalJacobian;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.trajectory.TrajectoryConfig;
|
||||
import org.wpilib.math.trajectory.TrajectoryGenerator;
|
||||
import org.wpilib.math.util.Nat;
|
||||
@@ -222,7 +222,7 @@ class MerweUKFTest {
|
||||
@Test
|
||||
void testLinearUKF() {
|
||||
var dt = 0.020;
|
||||
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
|
||||
var plant = Models.flywheelFromSysId(0.02, 0.006);
|
||||
var observer =
|
||||
new MerweUKF<>(
|
||||
Nat.N1(),
|
||||
|
||||
@@ -23,11 +23,11 @@ import org.wpilib.math.numbers.N3;
|
||||
import org.wpilib.math.numbers.N4;
|
||||
import org.wpilib.math.numbers.N5;
|
||||
import org.wpilib.math.random.Normal;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
import org.wpilib.math.system.Discretization;
|
||||
import org.wpilib.math.system.Models;
|
||||
import org.wpilib.math.system.NumericalIntegration;
|
||||
import org.wpilib.math.system.NumericalJacobian;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.trajectory.TrajectoryConfig;
|
||||
import org.wpilib.math.trajectory.TrajectoryGenerator;
|
||||
import org.wpilib.math.util.Nat;
|
||||
@@ -222,7 +222,7 @@ class S3UKFTest {
|
||||
@Test
|
||||
void testLinearUKF() {
|
||||
var dt = 0.020;
|
||||
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
|
||||
var plant = Models.flywheelFromSysId(0.02, 0.006);
|
||||
var observer =
|
||||
new S3UKF<>(
|
||||
Nat.N1(),
|
||||
|
||||
@@ -9,17 +9,36 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.wpilib.math.linalg.MatBuilder;
|
||||
import org.wpilib.math.linalg.Matrix;
|
||||
import org.wpilib.math.linalg.VecBuilder;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.plant.LinearSystemId;
|
||||
import org.wpilib.math.util.Nat;
|
||||
|
||||
class LinearSystemIDTest {
|
||||
class ModelsTest {
|
||||
@Test
|
||||
void testDrivetrainVelocitySystem() {
|
||||
void testFlywheelFromPhysicalConstants() {
|
||||
var model = Models.flywheelFromPhysicalConstants(DCMotor.getNEO(2), 0.00032, 1.0);
|
||||
assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001));
|
||||
|
||||
assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001));
|
||||
|
||||
assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001));
|
||||
|
||||
assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testFlywheelFromSysId() {
|
||||
var kv = 1.0;
|
||||
var ka = 0.5;
|
||||
var model = Models.flywheelFromSysId(kv, ka);
|
||||
|
||||
assertEquals(model.getA(), VecBuilder.fill(-kv / ka));
|
||||
assertEquals(model.getB(), VecBuilder.fill(1 / ka));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDifferentialDriveFromPhysicalConstants() {
|
||||
var model =
|
||||
LinearSystemId.createDrivetrainVelocitySystem(DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6);
|
||||
Models.differentialDriveFromPhysicalConstants(DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6);
|
||||
assertTrue(
|
||||
model
|
||||
.getA()
|
||||
@@ -41,8 +60,8 @@ class LinearSystemIDTest {
|
||||
}
|
||||
|
||||
@Test
|
||||
void testElevatorSystem() {
|
||||
var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12);
|
||||
void testElevatorFromPhysicalConstants() {
|
||||
var model = Models.elevatorFromPhysicalConstants(DCMotor.getNEO(2), 5, 0.05, 12);
|
||||
assertTrue(
|
||||
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -99.05473), 0.001));
|
||||
|
||||
@@ -54,52 +73,24 @@ class LinearSystemIDTest {
|
||||
}
|
||||
|
||||
@Test
|
||||
void testFlywheelSystem() {
|
||||
var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0);
|
||||
assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001));
|
||||
|
||||
assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001));
|
||||
|
||||
assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001));
|
||||
|
||||
assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testDCMotorSystem() {
|
||||
var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0);
|
||||
assertTrue(
|
||||
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -26.87032), 0.001));
|
||||
|
||||
assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001));
|
||||
|
||||
assertTrue(model.getC().isEqual(Matrix.eye(Nat.N2()), 0.001));
|
||||
|
||||
assertTrue(model.getD().isEqual(new Matrix<>(Nat.N2(), Nat.N1()), 0.001));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testIdentifyPositionSystem() {
|
||||
// By controls engineering in frc,
|
||||
// x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
|
||||
void testElevatorFromSysId() {
|
||||
var kv = 1.0;
|
||||
var ka = 0.5;
|
||||
var model = LinearSystemId.identifyPositionSystem(kv, ka);
|
||||
var model = Models.elevatorFromSysId(kv, ka);
|
||||
|
||||
assertEquals(model.getA(), MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kv / ka));
|
||||
assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka));
|
||||
}
|
||||
|
||||
@Test
|
||||
void testIdentifyVelocitySystem() {
|
||||
// By controls engineering in frc,
|
||||
// V = kv * velocity + ka * acceleration
|
||||
// x-dot = -kv/ka * v + 1/ka \cdot V
|
||||
void testSingleJointedArmFromSysId() {
|
||||
var kv = 1.0;
|
||||
var ka = 0.5;
|
||||
var model = LinearSystemId.identifyVelocitySystem(kv, ka);
|
||||
|
||||
assertEquals(model.getA(), VecBuilder.fill(-kv / ka));
|
||||
assertEquals(model.getB(), VecBuilder.fill(1 / ka));
|
||||
var model = Models.singleJointedArmFromSysId(kv, ka);
|
||||
|
||||
assertTrue(
|
||||
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kv / ka), 0.001));
|
||||
assertTrue(model.getB().isEqual(MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 1.0 / ka), 0.001));
|
||||
}
|
||||
}
|
||||
@@ -2,13 +2,13 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.math.system.plant.proto;
|
||||
package org.wpilib.math.system.proto;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.wpilib.math.proto.Plant.ProtobufDCMotor;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.proto.System.ProtobufDCMotor;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
|
||||
class DCMotorProtoTest {
|
||||
private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3);
|
||||
@@ -2,14 +2,14 @@
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package org.wpilib.math.system.plant.struct;
|
||||
package org.wpilib.math.system.struct;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import java.nio.ByteBuffer;
|
||||
import java.nio.ByteOrder;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.wpilib.math.system.plant.DCMotor;
|
||||
import org.wpilib.math.system.DCMotor;
|
||||
|
||||
class DCMotorStructTest {
|
||||
private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3);
|
||||
@@ -11,10 +11,10 @@
|
||||
#include "wpi/math/controller/LinearQuadraticRegulator.hpp"
|
||||
#include "wpi/math/estimator/KalmanFilter.hpp"
|
||||
#include "wpi/math/linalg/EigenCore.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/LinearSystem.hpp"
|
||||
#include "wpi/math/system/LinearSystemLoop.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
|
||||
namespace wpi::math {
|
||||
@@ -36,7 +36,8 @@ class StateSpaceTest : public testing::Test {
|
||||
// Gear ratio
|
||||
constexpr double G = 40.0 / 40.0;
|
||||
|
||||
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
|
||||
return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G)
|
||||
.Slice(0);
|
||||
}();
|
||||
LinearQuadraticRegulator<2, 1> controller{plant, {0.02, 0.4}, {12.0}, kDt};
|
||||
KalmanFilter<2, 1, 1> observer{plant, {0.05, 1.0}, {0.0001}, kDt};
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/units/math.hpp"
|
||||
|
||||
namespace wpi::math {
|
||||
@@ -19,8 +19,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) {
|
||||
|
||||
using Kv_t = decltype(1_V / 1_mps);
|
||||
using Ka_t = decltype(1_V / 1_mps_sq);
|
||||
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
|
||||
DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, maxA,
|
||||
maxAlpha};
|
||||
@@ -111,8 +111,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) {
|
||||
using Kv_t = decltype(1_V / 1_mps);
|
||||
using Ka_t = decltype(1_V / 1_mps_sq);
|
||||
|
||||
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
|
||||
// Limits are so high, they don't get hit, so states of constrained and
|
||||
// unconstrained systems should match
|
||||
@@ -181,8 +181,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
|
||||
|
||||
using Kv_t = decltype(1_V / 1_mps);
|
||||
using Ka_t = decltype(1_V / 1_mps_sq);
|
||||
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
|
||||
DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, minA,
|
||||
maxA, maxAlpha};
|
||||
@@ -240,8 +240,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) {
|
||||
TEST(DifferentialDriveAccelerationLimiterTest, MinAccelGreaterThanMaxAccel) {
|
||||
using Kv_t = decltype(1_V / 1_mps);
|
||||
using Ka_t = decltype(1_V / 1_mps_sq);
|
||||
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
EXPECT_NO_THROW({
|
||||
DifferentialDriveAccelerationLimiter accelLimiter(plant, 1_m, 1_mps_sq,
|
||||
1_rad_per_s_sq);
|
||||
|
||||
@@ -4,13 +4,10 @@
|
||||
|
||||
#include "wpi/math/controller/DifferentialDriveFeedforward.hpp"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/units/acceleration.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
@@ -26,7 +23,7 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithTrackwidth) {
|
||||
wpi::math::DifferentialDriveFeedforward differentialDriveFeedforward{
|
||||
kVLinear, kALinear, kVAngular, kAAngular, trackwidth};
|
||||
wpi::math::LinearSystem<2, 2, 2> plant =
|
||||
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
|
||||
wpi::math::Models::DifferentialDriveFromSysId(
|
||||
kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
|
||||
for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
|
||||
currentLeftVelocity += 2_mps) {
|
||||
@@ -60,8 +57,8 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) {
|
||||
wpi::math::DifferentialDriveFeedforward differentialDriveFeedforward{
|
||||
kVLinear, kALinear, kVAngular, kAAngular};
|
||||
wpi::math::LinearSystem<2, 2, 2> plant =
|
||||
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear,
|
||||
kVAngular, kAAngular);
|
||||
wpi::math::Models::DifferentialDriveFromSysId(kVLinear, kALinear,
|
||||
kVAngular, kAAngular);
|
||||
for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
|
||||
currentLeftVelocity += 2_mps) {
|
||||
for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps;
|
||||
|
||||
@@ -4,15 +4,13 @@
|
||||
|
||||
#include "wpi/math/controller/ElevatorFeedforward.hpp"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"
|
||||
#include "wpi/math/linalg/EigenCore.hpp"
|
||||
#include "wpi/units/acceleration.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
#include "wpi/units/velocity.hpp"
|
||||
|
||||
static constexpr auto Ks = 0.5_V;
|
||||
static constexpr auto Kv = 1.5_V * 1_s / 1_m;
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
|
||||
namespace wpi::math {
|
||||
|
||||
@@ -15,8 +15,8 @@ TEST(ImplicitModelFollowerTest, SameModel) {
|
||||
|
||||
using Kv_t = decltype(1_V / 1_mps);
|
||||
using Ka_t = decltype(1_V / 1_mps_sq);
|
||||
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
|
||||
ImplicitModelFollower<2, 2> imf{plant, plant};
|
||||
|
||||
@@ -60,12 +60,12 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) {
|
||||
using Kv_t = decltype(1_V / 1_mps);
|
||||
using Ka_t = decltype(1_V / 1_mps_sq);
|
||||
|
||||
auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
|
||||
// Linear acceleration is slower, but angular acceleration is the same
|
||||
auto plantRef = LinearSystemId::IdentifyDrivetrainSystem(
|
||||
Kv_t{1.0}, Ka_t{2.0}, Kv_t{1.0}, Ka_t{1.0});
|
||||
auto plantRef = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{2.0},
|
||||
Kv_t{1.0}, Ka_t{1.0});
|
||||
|
||||
ImplicitModelFollower<2, 2> imf{plant, plantRef};
|
||||
|
||||
|
||||
@@ -8,8 +8,8 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/math/system/NumericalIntegration.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
|
||||
#include "wpi/math/util/MathUtil.hpp"
|
||||
#include "wpi/units/math.hpp"
|
||||
@@ -46,7 +46,7 @@ static constexpr auto kLinearV = 3.02_V / 1_mps;
|
||||
static constexpr auto kLinearA = 0.642_V / 1_mps_sq;
|
||||
static constexpr auto kAngularV = 1.382_V / 1_mps;
|
||||
static constexpr auto kAngularA = 0.08495_V / 1_mps_sq;
|
||||
static auto plant = wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
|
||||
static auto plant = wpi::math::Models::DifferentialDriveFromSysId(
|
||||
kLinearV, kLinearA, kAngularV, kAngularA);
|
||||
static constexpr auto kTrackwidth = 0.9_m;
|
||||
|
||||
|
||||
@@ -4,8 +4,6 @@
|
||||
|
||||
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/math/linalg/EigenCore.hpp"
|
||||
|
||||
@@ -4,14 +4,12 @@
|
||||
|
||||
#include "wpi/math/controller/LinearQuadraticRegulator.hpp"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/math/linalg/EigenCore.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/LinearSystem.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/system/plant/LinearSystemId.hpp"
|
||||
#include "wpi/math/system/Models.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
|
||||
namespace wpi::math {
|
||||
@@ -29,7 +27,8 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) {
|
||||
// Gear ratio
|
||||
constexpr double G = 40.0 / 40.0;
|
||||
|
||||
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
|
||||
return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G)
|
||||
.Slice(0);
|
||||
}();
|
||||
Matrixd<1, 2> K =
|
||||
LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5_ms}.K();
|
||||
@@ -51,7 +50,7 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) {
|
||||
// Gear ratio
|
||||
constexpr double G = 100.0;
|
||||
|
||||
return wpi::math::LinearSystemId::SingleJointedArmSystem(
|
||||
return wpi::math::Models::SingleJointedArmFromPhysicalConstants(
|
||||
motors, 1.0 / 3.0 * m * r * r, G)
|
||||
.Slice(0);
|
||||
}();
|
||||
@@ -77,7 +76,8 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
|
||||
// Gear ratio
|
||||
constexpr double G = 14.67;
|
||||
|
||||
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
|
||||
return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G)
|
||||
.Slice(0);
|
||||
}();
|
||||
Matrixd<1, 2> K =
|
||||
LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K();
|
||||
@@ -179,7 +179,8 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
|
||||
// Gear ratio
|
||||
constexpr double G = 14.67;
|
||||
|
||||
return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0);
|
||||
return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G)
|
||||
.Slice(0);
|
||||
}();
|
||||
LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 20_ms};
|
||||
|
||||
|
||||
@@ -4,8 +4,6 @@
|
||||
|
||||
#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"
|
||||
|
||||
@@ -16,7 +16,6 @@
|
||||
#include "wpi/math/geometry/Rotation2d.hpp"
|
||||
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
|
||||
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
|
||||
#include "wpi/math/util/StateSpaceUtil.hpp"
|
||||
#include "wpi/units/angle.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
|
||||
@@ -17,7 +17,6 @@
|
||||
#include "wpi/math/geometry/Rotation2d.hpp"
|
||||
#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp"
|
||||
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
|
||||
#include "wpi/math/util/StateSpaceUtil.hpp"
|
||||
#include "wpi/units/angle.hpp"
|
||||
#include "wpi/units/length.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
|
||||
@@ -12,8 +12,8 @@
|
||||
|
||||
#include "wpi/math/linalg/EigenCore.hpp"
|
||||
#include "wpi/math/random/Normal.hpp"
|
||||
#include "wpi/math/system/DCMotor.hpp"
|
||||
#include "wpi/math/system/NumericalJacobian.hpp"
|
||||
#include "wpi/math/system/plant/DCMotor.hpp"
|
||||
#include "wpi/math/trajectory/TrajectoryGenerator.hpp"
|
||||
#include "wpi/units/moment_of_inertia.hpp"
|
||||
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user