Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

SCRIPT: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
View File

@@ -16,7 +16,8 @@
using namespace wpi::math;
wpi::units::volt_t ArmFeedforward::Calculate(
wpi::units::unit_t<Angle> currentAngle, wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Angle> currentAngle,
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity) const {
using VarMat = slp::VariableMatrix;
@@ -43,8 +44,9 @@ wpi::units::volt_t ArmFeedforward::Calculate(
// Initial guess
auto acceleration = (nextVelocity - currentVelocity) / m_dt;
u_k.set_value((kS * wpi::util::sgn(currentVelocity.value()) + kV * currentVelocity +
kA * acceleration + kG * wpi::units::math::cos(currentAngle))
u_k.set_value((kS * wpi::util::sgn(currentVelocity.value()) +
kV * currentVelocity + kA * acceleration +
kG * wpi::units::math::cos(currentAngle))
.value());
auto r_k1 = RK4<decltype(f), VarMat, VarMat>(f, r_k, u_k, m_dt);

4
wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
View File

@@ -10,8 +10,8 @@ using namespace wpi::math;
DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate(
wpi::units::meters_per_second_t leftVelocity,
wpi::units::meters_per_second_t rightVelocity, wpi::units::volt_t leftVoltage,
wpi::units::volt_t rightVoltage) {
wpi::units::meters_per_second_t rightVelocity,
wpi::units::volt_t leftVoltage, wpi::units::volt_t rightVoltage) {
Vectord<2> u{leftVoltage.value(), rightVoltage.value()};
// Find unconstrained wheel accelerations

3
wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
View File

@@ -14,7 +14,8 @@ DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate(
wpi::units::meters_per_second_t currentLeftVelocity,
wpi::units::meters_per_second_t nextLeftVelocity,
wpi::units::meters_per_second_t currentRightVelocity,
wpi::units::meters_per_second_t nextRightVelocity, wpi::units::second_t dt) {
wpi::units::meters_per_second_t nextRightVelocity,
wpi::units::second_t dt) {
wpi::math::LinearPlantInversionFeedforward<2, 2> feedforward{m_plant, dt};
Eigen::Vector2d r{currentLeftVelocity, currentRightVelocity};

6
wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
View File

@@ -26,7 +26,8 @@ DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
// [vₗ]
// [vᵣ]
wpi::units::meters_per_second_t velocity{(leftVelocity + rightVelocity) / 2.0};
wpi::units::meters_per_second_t velocity{(leftVelocity + rightVelocity) /
2.0};
// The DARE is ill-conditioned if the velocity is close to zero, so don't
// let the system stop.
@@ -42,7 +43,8 @@ DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
leftVelocity.value(), rightVelocity.value()};
m_error = r - x;
m_error(2) = wpi::math::AngleModulus(wpi::units::radian_t{m_error(2)}).value();
m_error(2) =
wpi::math::AngleModulus(wpi::units::radian_t{m_error(2)}).value();
Eigen::Matrix<double, 5, 5> A{
{0.0, 0.0, 0.0, 0.5, 0.5},

6
wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
View File

@@ -78,7 +78,7 @@ ChassisSpeeds LTVUnicycleController::Calculate(
m_poseError.Rotation().Radians().value()};
Eigen::Vector2d u = K * e;
return ChassisSpeeds{linearVelocityRef + wpi::units::meters_per_second_t{u(0)},
0_mps,
angularVelocityRef + wpi::units::radians_per_second_t{u(1)}};
return ChassisSpeeds{
linearVelocityRef + wpi::units::meters_per_second_t{u(0)}, 0_mps,
angularVelocityRef + wpi::units::radians_per_second_t{u(1)}};
}

4
wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp
View File

@@ -8,8 +8,8 @@
#include "wpimath/protobuf/controller.npb.h"
std::optional<wpi::math::ArmFeedforward> wpi::util::Protobuf<wpi::math::ArmFeedforward>::Unpack(
InputStream& stream) {
std::optional<wpi::math::ArmFeedforward>
wpi::util::Protobuf<wpi::math::ArmFeedforward>::Unpack(InputStream& stream) {
wpi_proto_ProtobufArmFeedforward msg;
if (!stream.Decode(msg)) {
return {};

7
wpimath/src/main/native/cpp/controller/proto/DifferentialDriveFeedforwardProto.cpp
View File

@@ -6,8 +6,8 @@
#include "wpimath/protobuf/controller.npb.h"
std::optional<wpi::math::DifferentialDriveFeedforward>
wpi::util::Protobuf<wpi::math::DifferentialDriveFeedforward>::Unpack(InputStream& stream) {
std::optional<wpi::math::DifferentialDriveFeedforward> wpi::util::Protobuf<
wpi::math::DifferentialDriveFeedforward>::Unpack(InputStream& stream) {
wpi_proto_ProtobufDifferentialDriveFeedforward msg;
if (!stream.Decode(msg)) {
return {};
@@ -22,7 +22,8 @@ wpi::util::Protobuf<wpi::math::DifferentialDriveFeedforward>::Unpack(InputStream
}
bool wpi::util::Protobuf<wpi::math::DifferentialDriveFeedforward>::Pack(
OutputStream& stream, const wpi::math::DifferentialDriveFeedforward& value) {
OutputStream& stream,
const wpi::math::DifferentialDriveFeedforward& value) {
wpi_proto_ProtobufDifferentialDriveFeedforward msg{
.kv_linear = value.m_kVLinear.value(),
.ka_linear = value.m_kALinear.value(),

3
wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp
View File

@@ -22,7 +22,8 @@ std::optional<wpi::math::DifferentialDriveWheelVoltages> wpi::util::Protobuf<
}
bool wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages>::Pack(
OutputStream& stream, const wpi::math::DifferentialDriveWheelVoltages& value) {
OutputStream& stream,
const wpi::math::DifferentialDriveWheelVoltages& value) {
wpi_proto_ProtobufDifferentialDriveWheelVoltages msg{
.left = value.left.value(),
.right = value.right.value(),

4
wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp
View File

@@ -8,8 +8,8 @@
#include "wpimath/protobuf/controller.npb.h"
std::optional<wpi::math::ElevatorFeedforward>
wpi::util::Protobuf<wpi::math::ElevatorFeedforward>::Unpack(InputStream& stream) {
std::optional<wpi::math::ElevatorFeedforward> wpi::util::Protobuf<
wpi::math::ElevatorFeedforward>::Unpack(InputStream& stream) {
wpi_proto_ProtobufElevatorFeedforward msg;
if (!stream.Decode(msg)) {
return {};

21
wpimath/src/main/native/cpp/controller/struct/DifferentialDriveFeedforwardStruct.cpp
View File

@@ -11,17 +11,22 @@ constexpr size_t kKvAngularOff = kKaLinearOff + 8;
constexpr size_t kKaAngularOff = kKvAngularOff + 8;
} // namespace
wpi::math::DifferentialDriveFeedforward wpi::util::Struct<
wpi::math::DifferentialDriveFeedforward>::Unpack(std::span<const uint8_t> data) {
return {
decltype(1_V / 1_mps){wpi::util::UnpackStruct<double, kKvLinearOff>(data)},
decltype(1_V / 1_mps_sq){wpi::util::UnpackStruct<double, kKaLinearOff>(data)},
decltype(1_V / 1_mps){wpi::util::UnpackStruct<double, kKvAngularOff>(data)},
decltype(1_V / 1_mps_sq){wpi::util::UnpackStruct<double, kKaAngularOff>(data)}};
wpi::math::DifferentialDriveFeedforward
wpi::util::Struct<wpi::math::DifferentialDriveFeedforward>::Unpack(
std::span<const uint8_t> data) {
return {decltype(1_V /
1_mps){wpi::util::UnpackStruct<double, kKvLinearOff>(data)},
decltype(1_V / 1_mps_sq){
wpi::util::UnpackStruct<double, kKaLinearOff>(data)},
decltype(1_V /
1_mps){wpi::util::UnpackStruct<double, kKvAngularOff>(data)},
decltype(1_V / 1_mps_sq){
wpi::util::UnpackStruct<double, kKaAngularOff>(data)}};
}
void wpi::util::Struct<wpi::math::DifferentialDriveFeedforward>::Pack(
std::span<uint8_t> data, const wpi::math::DifferentialDriveFeedforward& value) {
std::span<uint8_t> data,
const wpi::math::DifferentialDriveFeedforward& value) {
wpi::util::PackStruct<kKvLinearOff>(data, value.m_kVLinear.value());
wpi::util::PackStruct<kKaLinearOff>(data, value.m_kALinear.value());
wpi::util::PackStruct<kKvAngularOff>(data, value.m_kVAngular.value());

3
wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp
View File

@@ -13,7 +13,8 @@ constexpr size_t kKaOff = kKvOff + 8;
using StructType = wpi::util::Struct<wpi::math::ElevatorFeedforward>;
wpi::math::ElevatorFeedforward StructType::Unpack(std::span<const uint8_t> data) {
wpi::math::ElevatorFeedforward StructType::Unpack(
std::span<const uint8_t> data) {
return wpi::math::ElevatorFeedforward{
wpi::units::volt_t{wpi::util::UnpackStruct<double, kKsOff>(data)},
wpi::units::volt_t{wpi::util::UnpackStruct<double, kKgOff>(data)},

2
wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp
View File

@@ -42,7 +42,7 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const {
problem.solve();
rotPoint = wpi::math::Translation2d{wpi::units::meter_t{x.value()},
wpi::units::meter_t{y.value()}};
wpi::units::meter_t{y.value()}};
}
// Undo rotation

2
wpimath/src/main/native/cpp/geometry/Pose2d.cpp
View File

@@ -8,7 +8,7 @@
void wpi::math::to_json(wpi::util::json& json, const Pose2d& pose) {
json = wpi::util::json{{"translation", pose.Translation()},
{"rotation", pose.Rotation()}};
{"rotation", pose.Rotation()}};
}
void wpi::math::from_json(const wpi::util::json& json, Pose2d& pose) {

2
wpimath/src/main/native/cpp/geometry/Pose3d.cpp
View File

@@ -8,7 +8,7 @@
void wpi::math::to_json(wpi::util::json& json, const Pose3d& pose) {
json = wpi::util::json{{"translation", pose.Translation()},
{"rotation", pose.Rotation()}};
{"rotation", pose.Rotation()}};
}
void wpi::math::from_json(const wpi::util::json& json, Pose3d& pose) {

6
wpimath/src/main/native/cpp/geometry/Quaternion.cpp
View File

@@ -8,9 +8,9 @@
void wpi::math::to_json(wpi::util::json& json, const Quaternion& quaternion) {
json = wpi::util::json{{"W", quaternion.W()},
{"X", quaternion.X()},
{"Y", quaternion.Y()},
{"Z", quaternion.Z()}};
{"X", quaternion.X()},
{"Y", quaternion.Y()},
{"Z", quaternion.Z()}};
}
void wpi::math::from_json(const wpi::util::json& json, Quaternion& quaternion) {

10
wpimath/src/main/native/cpp/geometry/Translation2d.cpp
View File

@@ -6,12 +6,14 @@
#include "wpi/util/json.hpp"
void wpi::math::to_json(wpi::util::json& json, const Translation2d& translation) {
json =
wpi::util::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
void wpi::math::to_json(wpi::util::json& json,
const Translation2d& translation) {
json = wpi::util::json{{"x", translation.X().value()},
{"y", translation.Y().value()}};
}
void wpi::math::from_json(const wpi::util::json& json, Translation2d& translation) {
void wpi::math::from_json(const wpi::util::json& json,
Translation2d& translation) {
translation = Translation2d{wpi::units::meter_t{json.at("x").get<double>()},
wpi::units::meter_t{json.at("y").get<double>()}};
}

10
wpimath/src/main/native/cpp/geometry/Translation3d.cpp
View File

@@ -6,13 +6,15 @@
#include "wpi/util/json.hpp"
void wpi::math::to_json(wpi::util::json& json, const Translation3d& translation) {
void wpi::math::to_json(wpi::util::json& json,
const Translation3d& translation) {
json = wpi::util::json{{"x", translation.X().value()},
{"y", translation.Y().value()},
{"z", translation.Z().value()}};
{"y", translation.Y().value()},
{"z", translation.Z().value()}};
}
void wpi::math::from_json(const wpi::util::json& json, Translation3d& translation) {
void wpi::math::from_json(const wpi::util::json& json,
Translation3d& translation) {
translation = Translation3d{wpi::units::meter_t{json.at("x").get<double>()},
wpi::units::meter_t{json.at("y").get<double>()},
wpi::units::meter_t{json.at("z").get<double>()}};

8
wpimath/src/main/native/cpp/geometry/proto/Ellipse2dProto.cpp
View File

@@ -7,8 +7,8 @@
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
#include "wpimath/protobuf/geometry2d.npb.h"
std::optional<wpi::math::Ellipse2d> wpi::util::Protobuf<wpi::math::Ellipse2d>::Unpack(
InputStream& stream) {
std::optional<wpi::math::Ellipse2d>
wpi::util::Protobuf<wpi::math::Ellipse2d>::Unpack(InputStream& stream) {
wpi::util::UnpackCallback<wpi::math::Pose2d> pose;
wpi_proto_ProtobufEllipse2d msg{
.center = pose.Callback(),
@@ -32,8 +32,8 @@ std::optional<wpi::math::Ellipse2d> wpi::util::Protobuf<wpi::math::Ellipse2d>::U
};
}
bool wpi::util::Protobuf<wpi::math::Ellipse2d>::Pack(OutputStream& stream,
const wpi::math::Ellipse2d& value) {
bool wpi::util::Protobuf<wpi::math::Ellipse2d>::Pack(
OutputStream& stream, const wpi::math::Ellipse2d& value) {
wpi::util::PackCallback pose{&value.Center()};
wpi_proto_ProtobufEllipse2d msg{
.center = pose.Callback(),

4
wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp
View File

@@ -32,8 +32,8 @@ std::optional<wpi::math::Pose2d> wpi::util::Protobuf<wpi::math::Pose2d>::Unpack(
};
}
bool wpi::util::Protobuf<wpi::math::Pose2d>::Pack(OutputStream& stream,
const wpi::math::Pose2d& value) {
bool wpi::util::Protobuf<wpi::math::Pose2d>::Pack(
OutputStream& stream, const wpi::math::Pose2d& value) {
wpi::util::PackCallback tsln{&value.Translation()};
wpi::util::PackCallback rot{&value.Rotation()};
wpi_proto_ProtobufPose2d msg{

4
wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp
View File

@@ -34,8 +34,8 @@ std::optional<wpi::math::Pose3d> wpi::util::Protobuf<wpi::math::Pose3d>::Unpack(
};
}
bool wpi::util::Protobuf<wpi::math::Pose3d>::Pack(OutputStream& stream,
const wpi::math::Pose3d& value) {
bool wpi::util::Protobuf<wpi::math::Pose3d>::Pack(
OutputStream& stream, const wpi::math::Pose3d& value) {
wpi::util::PackCallback tsln{&value.Translation()};
wpi::util::PackCallback rot{&value.Rotation()};
wpi_proto_ProtobufPose3d msg{

8
wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp
View File

@@ -6,8 +6,8 @@
#include "wpimath/protobuf/geometry3d.npb.h"
std::optional<wpi::math::Quaternion> wpi::util::Protobuf<wpi::math::Quaternion>::Unpack(
InputStream& stream) {
std::optional<wpi::math::Quaternion>
wpi::util::Protobuf<wpi::math::Quaternion>::Unpack(InputStream& stream) {
wpi_proto_ProtobufQuaternion msg;
if (!stream.Decode(msg)) {
return {};
@@ -21,8 +21,8 @@ std::optional<wpi::math::Quaternion> wpi::util::Protobuf<wpi::math::Quaternion>:
};
}
bool wpi::util::Protobuf<wpi::math::Quaternion>::Pack(OutputStream& stream,
const wpi::math::Quaternion& value) {
bool wpi::util::Protobuf<wpi::math::Quaternion>::Pack(
OutputStream& stream, const wpi::math::Quaternion& value) {
wpi_proto_ProtobufQuaternion msg{
.w = value.W(),
.x = value.X(),

8
wpimath/src/main/native/cpp/geometry/proto/Rectangle2dProto.cpp
View File

@@ -7,8 +7,8 @@
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
#include "wpimath/protobuf/geometry2d.npb.h"
std::optional<wpi::math::Rectangle2d> wpi::util::Protobuf<wpi::math::Rectangle2d>::Unpack(
InputStream& stream) {
std::optional<wpi::math::Rectangle2d>
wpi::util::Protobuf<wpi::math::Rectangle2d>::Unpack(InputStream& stream) {
wpi::util::UnpackCallback<wpi::math::Pose2d> pose;
wpi_proto_ProtobufRectangle2d msg{
.center = pose.Callback(),
@@ -32,8 +32,8 @@ std::optional<wpi::math::Rectangle2d> wpi::util::Protobuf<wpi::math::Rectangle2d
};
}
bool wpi::util::Protobuf<wpi::math::Rectangle2d>::Pack(OutputStream& stream,
const wpi::math::Rectangle2d& value) {
bool wpi::util::Protobuf<wpi::math::Rectangle2d>::Pack(
OutputStream& stream, const wpi::math::Rectangle2d& value) {
wpi::util::PackCallback pose{&value.Center()};
wpi_proto_ProtobufRectangle2d msg{
.center = pose.Callback(),

8
wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp
View File

@@ -6,8 +6,8 @@
#include "wpimath/protobuf/geometry2d.npb.h"
std::optional<wpi::math::Rotation2d> wpi::util::Protobuf<wpi::math::Rotation2d>::Unpack(
InputStream& stream) {
std::optional<wpi::math::Rotation2d>
wpi::util::Protobuf<wpi::math::Rotation2d>::Unpack(InputStream& stream) {
wpi_proto_ProtobufRotation2d msg;
if (!stream.Decode(msg)) {
return {};
@@ -18,8 +18,8 @@ std::optional<wpi::math::Rotation2d> wpi::util::Protobuf<wpi::math::Rotation2d>:
};
}
bool wpi::util::Protobuf<wpi::math::Rotation2d>::Pack(OutputStream& stream,
const wpi::math::Rotation2d& value) {
bool wpi::util::Protobuf<wpi::math::Rotation2d>::Pack(
OutputStream& stream, const wpi::math::Rotation2d& value) {
wpi_proto_ProtobufRotation2d msg{
.value = value.Radians().value(),
};

8
wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp
View File

@@ -7,8 +7,8 @@
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
#include "wpimath/protobuf/geometry3d.npb.h"
std::optional<wpi::math::Rotation3d> wpi::util::Protobuf<wpi::math::Rotation3d>::Unpack(
InputStream& stream) {
std::optional<wpi::math::Rotation3d>
wpi::util::Protobuf<wpi::math::Rotation3d>::Unpack(InputStream& stream) {
wpi::util::UnpackCallback<wpi::math::Quaternion> quat;
wpi_proto_ProtobufRotation3d msg{
.q = quat.Callback(),
@@ -28,8 +28,8 @@ std::optional<wpi::math::Rotation3d> wpi::util::Protobuf<wpi::math::Rotation3d>:
};
}
bool wpi::util::Protobuf<wpi::math::Rotation3d>::Pack(OutputStream& stream,
const wpi::math::Rotation3d& value) {
bool wpi::util::Protobuf<wpi::math::Rotation3d>::Pack(
OutputStream& stream, const wpi::math::Rotation3d& value) {
wpi::util::PackCallback quat{&value.GetQuaternion()};
wpi_proto_ProtobufRotation3d msg{
.q = quat.Callback(),

8
wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp
View File

@@ -7,8 +7,8 @@
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
#include "wpimath/protobuf/geometry2d.npb.h"
std::optional<wpi::math::Transform2d> wpi::util::Protobuf<wpi::math::Transform2d>::Unpack(
InputStream& stream) {
std::optional<wpi::math::Transform2d>
wpi::util::Protobuf<wpi::math::Transform2d>::Unpack(InputStream& stream) {
wpi::util::UnpackCallback<wpi::math::Translation2d> tsln;
wpi::util::UnpackCallback<wpi::math::Rotation2d> rot;
wpi_proto_ProtobufTransform2d msg{
@@ -32,8 +32,8 @@ std::optional<wpi::math::Transform2d> wpi::util::Protobuf<wpi::math::Transform2d
};
}
bool wpi::util::Protobuf<wpi::math::Transform2d>::Pack(OutputStream& stream,
const wpi::math::Transform2d& value) {
bool wpi::util::Protobuf<wpi::math::Transform2d>::Pack(
OutputStream& stream, const wpi::math::Transform2d& value) {
wpi::util::PackCallback tsln{&value.Translation()};
wpi::util::PackCallback rot{&value.Rotation()};
wpi_proto_ProtobufTransform2d msg{

8
wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp
View File

@@ -7,8 +7,8 @@
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
#include "wpimath/protobuf/geometry3d.npb.h"
std::optional<wpi::math::Transform3d> wpi::util::Protobuf<wpi::math::Transform3d>::Unpack(
InputStream& stream) {
std::optional<wpi::math::Transform3d>
wpi::util::Protobuf<wpi::math::Transform3d>::Unpack(InputStream& stream) {
wpi::util::UnpackCallback<wpi::math::Translation3d> tsln;
wpi::util::UnpackCallback<wpi::math::Rotation3d> rot;
wpi_proto_ProtobufTransform3d msg{
@@ -32,8 +32,8 @@ std::optional<wpi::math::Transform3d> wpi::util::Protobuf<wpi::math::Transform3d
};
}
bool wpi::util::Protobuf<wpi::math::Transform3d>::Pack(OutputStream& stream,
const wpi::math::Transform3d& value) {
bool wpi::util::Protobuf<wpi::math::Transform3d>::Pack(
OutputStream& stream, const wpi::math::Transform3d& value) {
wpi::util::PackCallback tsln{&value.Translation()};
wpi::util::PackCallback rot{&value.Rotation()};
wpi_proto_ProtobufTransform3d msg{

8
wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp
View File

@@ -6,8 +6,8 @@
#include "wpimath/protobuf/geometry2d.npb.h"
std::optional<wpi::math::Translation2d> wpi::util::Protobuf<wpi::math::Translation2d>::Unpack(
InputStream& stream) {
std::optional<wpi::math::Translation2d>
wpi::util::Protobuf<wpi::math::Translation2d>::Unpack(InputStream& stream) {
wpi_proto_ProtobufTranslation2d msg;
if (!stream.Decode(msg)) {
return {};
@@ -19,8 +19,8 @@ std::optional<wpi::math::Translation2d> wpi::util::Protobuf<wpi::math::Translati
};
}
bool wpi::util::Protobuf<wpi::math::Translation2d>::Pack(OutputStream& stream,
const wpi::math::Translation2d& value) {
bool wpi::util::Protobuf<wpi::math::Translation2d>::Pack(
OutputStream& stream, const wpi::math::Translation2d& value) {
wpi_proto_ProtobufTranslation2d msg{
.x = value.X().value(),
.y = value.Y().value(),

8
wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp
View File

@@ -6,8 +6,8 @@
#include "wpimath/protobuf/geometry3d.npb.h"
std::optional<wpi::math::Translation3d> wpi::util::Protobuf<wpi::math::Translation3d>::Unpack(
InputStream& stream) {
std::optional<wpi::math::Translation3d>
wpi::util::Protobuf<wpi::math::Translation3d>::Unpack(InputStream& stream) {
wpi_proto_ProtobufTranslation3d msg;
if (!stream.Decode(msg)) {
return {};
@@ -20,8 +20,8 @@ std::optional<wpi::math::Translation3d> wpi::util::Protobuf<wpi::math::Translati
};
}
bool wpi::util::Protobuf<wpi::math::Translation3d>::Pack(OutputStream& stream,
const wpi::math::Translation3d& value) {
bool wpi::util::Protobuf<wpi::math::Translation3d>::Pack(
OutputStream& stream, const wpi::math::Translation3d& value) {
wpi_proto_ProtobufTranslation3d msg{
.x = value.X().value(),
.y = value.Y().value(),

8
wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp
View File

@@ -6,8 +6,8 @@
#include "wpimath/protobuf/geometry2d.npb.h"
std::optional<wpi::math::Twist2d> wpi::util::Protobuf<wpi::math::Twist2d>::Unpack(
InputStream& stream) {
std::optional<wpi::math::Twist2d>
wpi::util::Protobuf<wpi::math::Twist2d>::Unpack(InputStream& stream) {
wpi_proto_ProtobufTwist2d msg;
if (!stream.Decode(msg)) {
return {};
@@ -20,8 +20,8 @@ std::optional<wpi::math::Twist2d> wpi::util::Protobuf<wpi::math::Twist2d>::Unpac
};
}
bool wpi::util::Protobuf<wpi::math::Twist2d>::Pack(OutputStream& stream,
const wpi::math::Twist2d& value) {
bool wpi::util::Protobuf<wpi::math::Twist2d>::Pack(
OutputStream& stream, const wpi::math::Twist2d& value) {
wpi_proto_ProtobufTwist2d msg{
.dx = value.dx.value(),
.dy = value.dy.value(),

13
wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp
View File

@@ -6,21 +6,22 @@
#include "wpimath/protobuf/geometry3d.npb.h"
std::optional<wpi::math::Twist3d> wpi::util::Protobuf<wpi::math::Twist3d>::Unpack(
InputStream& stream) {
std::optional<wpi::math::Twist3d>
wpi::util::Protobuf<wpi::math::Twist3d>::Unpack(InputStream& stream) {
wpi_proto_ProtobufTwist3d msg;
if (!stream.Decode(msg)) {
return {};
}
return wpi::math::Twist3d{
wpi::units::meter_t{msg.dx}, wpi::units::meter_t{msg.dy}, wpi::units::meter_t{msg.dz},
wpi::units::radian_t{msg.rx}, wpi::units::radian_t{msg.ry}, wpi::units::radian_t{msg.rz},
wpi::units::meter_t{msg.dx}, wpi::units::meter_t{msg.dy},
wpi::units::meter_t{msg.dz}, wpi::units::radian_t{msg.rx},
wpi::units::radian_t{msg.ry}, wpi::units::radian_t{msg.rz},
};
}
bool wpi::util::Protobuf<wpi::math::Twist3d>::Pack(OutputStream& stream,
const wpi::math::Twist3d& value) {
bool wpi::util::Protobuf<wpi::math::Twist3d>::Pack(
OutputStream& stream, const wpi::math::Twist3d& value) {
wpi_proto_ProtobufTwist3d msg{
.dx = value.dx.value(),
.dy = value.dy.value(),

6
wpimath/src/main/native/cpp/geometry/struct/Ellipse2dStruct.cpp
View File

@@ -6,7 +6,8 @@
namespace {
constexpr size_t kCenterOff = 0;
constexpr size_t kXSemiAxisOff = kCenterOff + wpi::util::GetStructSize<wpi::math::Pose2d>();
constexpr size_t kXSemiAxisOff =
kCenterOff + wpi::util::GetStructSize<wpi::math::Pose2d>();
constexpr size_t kYSemiAxisOff = kXSemiAxisOff + 8;
} // namespace
@@ -20,7 +21,8 @@ wpi::math::Ellipse2d StructType::Unpack(std::span<const uint8_t> data) {
};
}
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Ellipse2d& value) {
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::Ellipse2d& value) {
wpi::util::PackStruct<kCenterOff>(data, value.Center());
wpi::util::PackStruct<kXSemiAxisOff>(data, value.XSemiAxis().value());
wpi::util::PackStruct<kYSemiAxisOff>(data, value.YSemiAxis().value());

3
wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp
View File

@@ -22,7 +22,8 @@ wpi::math::Quaternion StructType::Unpack(std::span<const uint8_t> data) {
};
}
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Quaternion& value) {
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::Quaternion& value) {
wpi::util::PackStruct<kWOff>(data, value.W());
wpi::util::PackStruct<kXOff>(data, value.X());
wpi::util::PackStruct<kYOff>(data, value.Y());

6
wpimath/src/main/native/cpp/geometry/struct/Rectangle2dStruct.cpp
View File

@@ -6,7 +6,8 @@
namespace {
constexpr size_t kCenterOff = 0;
constexpr size_t kXWidthOff = kCenterOff + wpi::util::GetStructSize<wpi::math::Pose2d>();
constexpr size_t kXWidthOff =
kCenterOff + wpi::util::GetStructSize<wpi::math::Pose2d>();
constexpr size_t kYWidthOff = kXWidthOff + 8;
} // namespace
@@ -20,7 +21,8 @@ wpi::math::Rectangle2d StructType::Unpack(std::span<const uint8_t> data) {
};
}
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Rectangle2d& value) {
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::Rectangle2d& value) {
wpi::util::PackStruct<kCenterOff>(data, value.Center());
wpi::util::PackStruct<kXWidthOff>(data, value.XWidth().value());
wpi::util::PackStruct<kYWidthOff>(data, value.YWidth().value());

3
wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp
View File

@@ -16,6 +16,7 @@ wpi::math::Rotation2d StructType::Unpack(std::span<const uint8_t> data) {
};
}
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Rotation2d& value) {
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::Rotation2d& value) {
wpi::util::PackStruct<kValueOff>(data, value.Radians().value());
}

3
wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp
View File

@@ -16,6 +16,7 @@ wpi::math::Rotation3d StructType::Unpack(std::span<const uint8_t> data) {
};
}
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Rotation3d& value) {
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::Rotation3d& value) {
wpi::util::PackStruct<kQOff>(data, value.GetQuaternion());
}

3
wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp
View File

@@ -19,7 +19,8 @@ wpi::math::Transform2d StructType::Unpack(std::span<const uint8_t> data) {
};
}
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Transform2d& value) {
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::Transform2d& value) {
wpi::util::PackStruct<kTranslationOff>(data, value.Translation());
wpi::util::PackStruct<kRotationOff>(data, value.Rotation());
}

3
wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp
View File

@@ -19,7 +19,8 @@ wpi::math::Transform3d StructType::Unpack(std::span<const uint8_t> data) {
};
}
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Transform3d& value) {
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::Transform3d& value) {
wpi::util::PackStruct<kTranslationOff>(data, value.Translation());
wpi::util::PackStruct<kRotationOff>(data, value.Rotation());
}

3
wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp
View File

@@ -20,7 +20,8 @@ wpi::math::Twist2d StructType::Unpack(std::span<const uint8_t> data) {
};
}
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Twist2d& value) {
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::Twist2d& value) {
wpi::util::PackStruct<kDxOff>(data, value.dx.value());
wpi::util::PackStruct<kDyOff>(data, value.dy.value());
wpi::util::PackStruct<kDthetaOff>(data, value.dtheta.value());

3
wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp
View File

@@ -26,7 +26,8 @@ wpi::math::Twist3d StructType::Unpack(std::span<const uint8_t> data) {
};
}
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Twist3d& value) {
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::Twist3d& value) {
wpi::util::PackStruct<kDxOff>(data, value.dx.value());
wpi::util::PackStruct<kDyOff>(data, value.dy.value());
wpi::util::PackStruct<kDzOff>(data, value.dz.value());

9
wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp
View File

@@ -23,10 +23,11 @@ Java_org_wpilib_math_jni_ArmFeedforwardJNI_calculate
jdouble currentAngle, jdouble currentVelocity, jdouble nextVelocity,
jdouble dt)
{
return wpi::math::ArmFeedforward{wpi::units::volt_t{ks}, wpi::units::volt_t{kg},
wpi::units::unit_t<wpi::math::ArmFeedforward::kv_unit>{kv},
wpi::units::unit_t<wpi::math::ArmFeedforward::ka_unit>{ka},
wpi::units::second_t{dt}}
return wpi::math::ArmFeedforward{
wpi::units::volt_t{ks}, wpi::units::volt_t{kg},
wpi::units::unit_t<wpi::math::ArmFeedforward::kv_unit>{kv},
wpi::units::unit_t<wpi::math::ArmFeedforward::ka_unit>{ka},
wpi::units::second_t{dt}}
.Calculate(wpi::units::radian_t{currentAngle},
wpi::units::radians_per_second_t{currentVelocity},
wpi::units::radians_per_second_t{nextVelocity})

18
wpimath/src/main/native/cpp/jni/DAREJNI.cpp
View File

@@ -45,9 +45,9 @@ Java_org_wpilib_math_jni_DAREJNI_dareNoPrecondABQR
Eigen::RowMajor>>
Rmat{nativeR.data(), inputs, inputs};
auto result =
wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, false)
.value();
auto result = wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(
Amat, Bmat, Qmat, Rmat, false)
.value();
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
}
@@ -84,8 +84,8 @@ Java_org_wpilib_math_jni_DAREJNI_dareNoPrecondABQRN
Eigen::RowMajor>>
Nmat{nativeN.data(), states, inputs};
auto result = wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat,
Rmat, Nmat, false)
auto result = wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(
Amat, Bmat, Qmat, Rmat, Nmat, false)
.value();
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
@@ -119,8 +119,8 @@ Java_org_wpilib_math_jni_DAREJNI_dareABQR
Eigen::RowMajor>>
Rmat{nativeR.data(), inputs, inputs};
if (auto result =
wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat)) {
if (auto result = wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(
Amat, Bmat, Qmat, Rmat)) {
env->SetDoubleArrayRegion(S, 0, states * states, result.value().data());
// K = (BᵀSB + R)⁻¹BᵀSA
} else if (result.error() == wpi::math::DAREError::QNotSymmetric ||
@@ -172,8 +172,8 @@ Java_org_wpilib_math_jni_DAREJNI_dareABQRN
Eigen::RowMajor>>
Nmat{nativeN.data(), states, inputs};
if (auto result = wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat,
Rmat, Nmat)) {
if (auto result = wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(
Amat, Bmat, Qmat, Rmat, Nmat)) {
env->SetDoubleArrayRegion(S, 0, states * states, result.value().data());
} else if (result.error() == wpi::math::DAREError::QNotSymmetric ||
result.error() == wpi::math::DAREError::QNotPositiveSemidefinite) {

5
wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp
View File

@@ -26,8 +26,9 @@ Java_org_wpilib_math_jni_Ellipse2dJNI_nearest
{
auto point =
wpi::math::Ellipse2d{
wpi::math::Pose2d{wpi::units::meter_t{centerX}, wpi::units::meter_t{centerY},
wpi::units::radian_t{centerHeading}},
wpi::math::Pose2d{wpi::units::meter_t{centerX},
wpi::units::meter_t{centerY},
wpi::units::radian_t{centerHeading}},
wpi::units::meter_t{xSemiAxis}, wpi::units::meter_t{ySemiAxis}}
.Nearest({wpi::units::meter_t{pointX}, wpi::units::meter_t{pointY}});

3
wpimath/src/main/native/cpp/jni/Transform3dJNI.cpp
View File

@@ -28,7 +28,8 @@ Java_org_wpilib_math_jni_Transform3dJNI_log
jdouble relQx, jdouble relQy, jdouble relQz)
{
wpi::math::Transform3d transform3d{
wpi::units::meter_t{relX}, wpi::units::meter_t{relY}, wpi::units::meter_t{relZ},
wpi::units::meter_t{relX}, wpi::units::meter_t{relY},
wpi::units::meter_t{relZ},
wpi::math::Rotation3d{wpi::math::Quaternion{relQw, relQx, relQy, relQz}}};
wpi::math::Twist3d result = transform3d.Log();

7
wpimath/src/main/native/cpp/jni/Twist3dJNI.cpp
View File

@@ -25,9 +25,10 @@ Java_org_wpilib_math_jni_Twist3dJNI_exp
(JNIEnv* env, jclass, jdouble twistDx, jdouble twistDy, jdouble twistDz,
jdouble twistRx, jdouble twistRy, jdouble twistRz)
{
wpi::math::Twist3d twist{wpi::units::meter_t{twistDx}, wpi::units::meter_t{twistDy},
wpi::units::meter_t{twistDz}, wpi::units::radian_t{twistRx},
wpi::units::radian_t{twistRy}, wpi::units::radian_t{twistRz}};
wpi::math::Twist3d twist{
wpi::units::meter_t{twistDx}, wpi::units::meter_t{twistDy},
wpi::units::meter_t{twistDz}, wpi::units::radian_t{twistRx},
wpi::units::radian_t{twistRy}, wpi::units::radian_t{twistRz}};
wpi::math::Transform3d result = twist.Exp();

6
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -60,7 +60,8 @@ Twist2d MecanumDriveKinematics::ToTwist2d(
Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
return {wpi::units::meter_t{twistVector(0)}, wpi::units::meter_t{twistVector(1)},
return {wpi::units::meter_t{twistVector(0)},
wpi::units::meter_t{twistVector(1)},
wpi::units::radian_t{twistVector(2)}};
}
@@ -72,7 +73,8 @@ Twist2d MecanumDriveKinematics::ToTwist2d(
Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
return {wpi::units::meter_t{twistVector(0)}, wpi::units::meter_t{twistVector(1)},
return {wpi::units::meter_t{twistVector(0)},
wpi::units::meter_t{twistVector(1)},
wpi::units::radian_t{twistVector(2)}};
}

8
wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp
View File

@@ -6,8 +6,8 @@
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::ChassisSpeeds> wpi::util::Protobuf<wpi::math::ChassisSpeeds>::Unpack(
InputStream& stream) {
std::optional<wpi::math::ChassisSpeeds>
wpi::util::Protobuf<wpi::math::ChassisSpeeds>::Unpack(InputStream& stream) {
wpi_proto_ProtobufChassisSpeeds msg;
if (!stream.Decode(msg)) {
return {};
@@ -20,8 +20,8 @@ std::optional<wpi::math::ChassisSpeeds> wpi::util::Protobuf<wpi::math::ChassisSp
};
}
bool wpi::util::Protobuf<wpi::math::ChassisSpeeds>::Pack(OutputStream& stream,
const wpi::math::ChassisSpeeds& value) {
bool wpi::util::Protobuf<wpi::math::ChassisSpeeds>::Pack(
OutputStream& stream, const wpi::math::ChassisSpeeds& value) {
wpi_proto_ProtobufChassisSpeeds msg{
.vx = value.vx.value(),
.vy = value.vy.value(),

4
wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp
View File

@@ -6,8 +6,8 @@
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::DifferentialDriveKinematics>
wpi::util::Protobuf<wpi::math::DifferentialDriveKinematics>::Unpack(InputStream& stream) {
std::optional<wpi::math::DifferentialDriveKinematics> wpi::util::Protobuf<
wpi::math::DifferentialDriveKinematics>::Unpack(InputStream& stream) {
wpi_proto_ProtobufDifferentialDriveKinematics msg;
if (!stream.Decode(msg)) {
return {};

3
wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelPositionsProto.cpp
View File

@@ -20,7 +20,8 @@ std::optional<wpi::math::DifferentialDriveWheelPositions> wpi::util::Protobuf<
}
bool wpi::util::Protobuf<wpi::math::DifferentialDriveWheelPositions>::Pack(
OutputStream& stream, const wpi::math::DifferentialDriveWheelPositions& value) {
OutputStream& stream,
const wpi::math::DifferentialDriveWheelPositions& value) {
wpi_proto_ProtobufDifferentialDriveWheelPositions msg{
.left = value.left.value(),
.right = value.right.value(),

7
wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp
View File

@@ -6,8 +6,8 @@
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::DifferentialDriveWheelSpeeds>
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelSpeeds>::Unpack(InputStream& stream) {
std::optional<wpi::math::DifferentialDriveWheelSpeeds> wpi::util::Protobuf<
wpi::math::DifferentialDriveWheelSpeeds>::Unpack(InputStream& stream) {
wpi_proto_ProtobufDifferentialDriveWheelSpeeds msg;
if (!stream.Decode(msg)) {
return {};
@@ -20,7 +20,8 @@ wpi::util::Protobuf<wpi::math::DifferentialDriveWheelSpeeds>::Unpack(InputStream
}
bool wpi::util::Protobuf<wpi::math::DifferentialDriveWheelSpeeds>::Pack(
OutputStream& stream, const wpi::math::DifferentialDriveWheelSpeeds& value) {
OutputStream& stream,
const wpi::math::DifferentialDriveWheelSpeeds& value) {
wpi_proto_ProtobufDifferentialDriveWheelSpeeds msg{
.left = value.left.value(),
.right = value.right.value(),

4
wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp
View File

@@ -7,8 +7,8 @@
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::MecanumDriveKinematics>
wpi::util::Protobuf<wpi::math::MecanumDriveKinematics>::Unpack(InputStream& stream) {
std::optional<wpi::math::MecanumDriveKinematics> wpi::util::Protobuf<
wpi::math::MecanumDriveKinematics>::Unpack(InputStream& stream) {
wpi::util::UnpackCallback<wpi::math::Translation2d> frontLeft;
wpi::util::UnpackCallback<wpi::math::Translation2d> frontRight;
wpi::util::UnpackCallback<wpi::math::Translation2d> rearLeft;

4
wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp
View File

@@ -6,8 +6,8 @@
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::MecanumDriveWheelPositions>
wpi::util::Protobuf<wpi::math::MecanumDriveWheelPositions>::Unpack(InputStream& stream) {
std::optional<wpi::math::MecanumDriveWheelPositions> wpi::util::Protobuf<
wpi::math::MecanumDriveWheelPositions>::Unpack(InputStream& stream) {
wpi_proto_ProtobufMecanumDriveWheelPositions msg;
if (!stream.Decode(msg)) {
return {};

4
wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp
View File

@@ -6,8 +6,8 @@
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::MecanumDriveWheelSpeeds>
wpi::util::Protobuf<wpi::math::MecanumDriveWheelSpeeds>::Unpack(InputStream& stream) {
std::optional<wpi::math::MecanumDriveWheelSpeeds> wpi::util::Protobuf<
wpi::math::MecanumDriveWheelSpeeds>::Unpack(InputStream& stream) {
wpi_proto_ProtobufMecanumDriveWheelSpeeds msg;
if (!stream.Decode(msg)) {
return {};

4
wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp
View File

@@ -7,8 +7,8 @@
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
#include "wpimath/protobuf/kinematics.npb.h"
std::optional<wpi::math::SwerveModulePosition>
wpi::util::Protobuf<wpi::math::SwerveModulePosition>::Unpack(InputStream& stream) {
std::optional<wpi::math::SwerveModulePosition> wpi::util::Protobuf<
wpi::math::SwerveModulePosition>::Unpack(InputStream& stream) {
wpi::util::UnpackCallback<wpi::math::Rotation2d> angle;
wpi_proto_ProtobufSwerveModulePosition msg{
.distance = 0,

9
wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp
View File

@@ -14,9 +14,12 @@ using StructType = wpi::util::Struct<wpi::math::ChassisSpeeds>;
wpi::math::ChassisSpeeds StructType::Unpack(std::span<const uint8_t> data) {
return wpi::math::ChassisSpeeds{
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kVxOff>(data)},
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kVyOff>(data)},
wpi::units::radians_per_second_t{wpi::util::UnpackStruct<double, kOmegaOff>(data)},
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kVxOff>(data)},
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kVyOff>(data)},
wpi::units::radians_per_second_t{
wpi::util::UnpackStruct<double, kOmegaOff>(data)},
};
}

3
wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp
View File

@@ -13,7 +13,8 @@ using StructType = wpi::util::Struct<wpi::math::DifferentialDriveKinematics>;
wpi::math::DifferentialDriveKinematics StructType::Unpack(
std::span<const uint8_t> data) {
return wpi::math::DifferentialDriveKinematics{
wpi::units::meter_t{wpi::util::UnpackStruct<double, kTrackwidthOff>(data)},
wpi::units::meter_t{
wpi::util::UnpackStruct<double, kTrackwidthOff>(data)},
};
}

3
wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelPositionsStruct.cpp
View File

@@ -9,7 +9,8 @@ constexpr size_t kLeftOff = 0;
constexpr size_t kRightOff = kLeftOff + 8;
} // namespace
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelPositions>;
using StructType =
wpi::util::Struct<wpi::math::DifferentialDriveWheelPositions>;
wpi::math::DifferentialDriveWheelPositions StructType::Unpack(
std::span<const uint8_t> data) {

6
wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp
View File

@@ -14,8 +14,10 @@ using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeeds>;
wpi::math::DifferentialDriveWheelSpeeds StructType::Unpack(
std::span<const uint8_t> data) {
return wpi::math::DifferentialDriveWheelSpeeds{
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kLeftOff>(data)},
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kRightOff>(data)},
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kLeftOff>(data)},
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kRightOff>(data)},
};
}

3
wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp
View File

@@ -16,7 +16,8 @@ constexpr size_t kRearRightOff =
using StructType = wpi::util::Struct<wpi::math::MecanumDriveKinematics>;
wpi::math::MecanumDriveKinematics StructType::Unpack(std::span<const uint8_t> data) {
wpi::math::MecanumDriveKinematics StructType::Unpack(
std::span<const uint8_t> data) {
return wpi::math::MecanumDriveKinematics{
wpi::util::UnpackStruct<wpi::math::Translation2d, kFrontLeftOff>(data),
wpi::util::UnpackStruct<wpi::math::Translation2d, kFrontRightOff>(data),

3
wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp
View File

@@ -17,7 +17,8 @@ wpi::math::MecanumDriveWheelPositions StructType::Unpack(
std::span<const uint8_t> data) {
return wpi::math::MecanumDriveWheelPositions{
wpi::units::meter_t{wpi::util::UnpackStruct<double, kFrontLeftOff>(data)},
wpi::units::meter_t{wpi::util::UnpackStruct<double, kFrontRightOff>(data)},
wpi::units::meter_t{
wpi::util::UnpackStruct<double, kFrontRightOff>(data)},
wpi::units::meter_t{wpi::util::UnpackStruct<double, kRearLeftOff>(data)},
wpi::units::meter_t{wpi::util::UnpackStruct<double, kRearRightOff>(data)},
};

6
wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp
View File

@@ -13,13 +13,15 @@ constexpr size_t kRearRightOff = kRearLeftOff + 8;
using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelSpeeds>;
wpi::math::MecanumDriveWheelSpeeds StructType::Unpack(std::span<const uint8_t> data) {
wpi::math::MecanumDriveWheelSpeeds StructType::Unpack(
std::span<const uint8_t> data) {
return wpi::math::MecanumDriveWheelSpeeds{
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kFrontLeftOff>(data)},
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kFrontRightOff>(data)},
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kRearLeftOff>(data)},
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kRearLeftOff>(data)},
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kRearRightOff>(data)},
};

3
wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp
View File

@@ -11,7 +11,8 @@ constexpr size_t kAngleOff = kDistanceOff + 8;
using StructType = wpi::util::Struct<wpi::math::SwerveModulePosition>;
wpi::math::SwerveModulePosition StructType::Unpack(std::span<const uint8_t> data) {
wpi::math::SwerveModulePosition StructType::Unpack(
std::span<const uint8_t> data) {
return wpi::math::SwerveModulePosition{
wpi::units::meter_t{wpi::util::UnpackStruct<double, kDistanceOff>(data)},
wpi::util::UnpackStruct<wpi::math::Rotation2d, kAngleOff>(data),

3
wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp
View File

@@ -13,7 +13,8 @@ using StructType = wpi::util::Struct<wpi::math::SwerveModuleState>;
wpi::math::SwerveModuleState StructType::Unpack(std::span<const uint8_t> data) {
return wpi::math::SwerveModuleState{
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kSpeedOff>(data)},
wpi::units::meters_per_second_t{
wpi::util::UnpackStruct<double, kSpeedOff>(data)},
wpi::util::UnpackStruct<wpi::math::Rotation2d, kAngleOff>(data),
};
}

4
wpimath/src/main/native/cpp/spline/proto/CubicHermiteSplineProto.cpp
View File

@@ -7,8 +7,8 @@
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
#include "wpimath/protobuf/spline.npb.h"
std::optional<wpi::math::CubicHermiteSpline>
wpi::util::Protobuf<wpi::math::CubicHermiteSpline>::Unpack(InputStream& stream) {
std::optional<wpi::math::CubicHermiteSpline> wpi::util::Protobuf<
wpi::math::CubicHermiteSpline>::Unpack(InputStream& stream) {
wpi::util::WpiArrayUnpackCallback<double, 2> xInitial;
wpi::util::WpiArrayUnpackCallback<double, 2> xFinal;
wpi::util::WpiArrayUnpackCallback<double, 2> yInitial;

4
wpimath/src/main/native/cpp/spline/proto/QuinticHermiteSplineProto.cpp
View File

@@ -7,8 +7,8 @@
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
#include "wpimath/protobuf/spline.npb.h"
std::optional<wpi::math::QuinticHermiteSpline>
wpi::util::Protobuf<wpi::math::QuinticHermiteSpline>::Unpack(InputStream& stream) {
std::optional<wpi::math::QuinticHermiteSpline> wpi::util::Protobuf<
wpi::math::QuinticHermiteSpline>::Unpack(InputStream& stream) {
wpi::util::WpiArrayUnpackCallback<double, 3> xInitial;
wpi::util::WpiArrayUnpackCallback<double, 3> xFinal;
wpi::util::WpiArrayUnpackCallback<double, 3> yInitial;

18
wpimath/src/main/native/cpp/spline/struct/CubicHermiteSplineStruct.cpp
View File

@@ -11,8 +11,8 @@ constexpr size_t kYInitialOff = kXFinalOff + 2 * 8;
constexpr size_t kYFinalOff = kYInitialOff + 2 * 8;
} // namespace
wpi::math::CubicHermiteSpline wpi::util::Struct<wpi::math::CubicHermiteSpline>::Unpack(
std::span<const uint8_t> data) {
wpi::math::CubicHermiteSpline wpi::util::Struct<
wpi::math::CubicHermiteSpline>::Unpack(std::span<const uint8_t> data) {
return wpi::math::CubicHermiteSpline{
wpi::util::UnpackStructArray<double, kXInitialOff, 2>(data),
wpi::util::UnpackStructArray<double, kXFinalOff, 2>(data),
@@ -22,10 +22,12 @@ wpi::math::CubicHermiteSpline wpi::util::Struct<wpi::math::CubicHermiteSpline>::
void wpi::util::Struct<wpi::math::CubicHermiteSpline>::Pack(
std::span<uint8_t> data, const wpi::math::CubicHermiteSpline& value) {
wpi::util::PackStructArray<kXInitialOff, 2>(data,
value.GetInitialControlVector().x);
wpi::util::PackStructArray<kXFinalOff, 2>(data, value.GetFinalControlVector().x);
wpi::util::PackStructArray<kYInitialOff, 2>(data,
value.GetInitialControlVector().y);
wpi::util::PackStructArray<kYFinalOff, 2>(data, value.GetFinalControlVector().y);
wpi::util::PackStructArray<kXInitialOff, 2>(
data, value.GetInitialControlVector().x);
wpi::util::PackStructArray<kXFinalOff, 2>(data,
value.GetFinalControlVector().x);
wpi::util::PackStructArray<kYInitialOff, 2>(
data, value.GetInitialControlVector().y);
wpi::util::PackStructArray<kYFinalOff, 2>(data,
value.GetFinalControlVector().y);
}

18
wpimath/src/main/native/cpp/spline/struct/QuinticHermiteSplineStruct.cpp
View File

@@ -11,8 +11,8 @@ constexpr size_t kYInitialOff = kXFinalOff + 3 * 8;
constexpr size_t kYFinalOff = kYInitialOff + 3 * 8;
} // namespace
wpi::math::QuinticHermiteSpline wpi::util::Struct<wpi::math::QuinticHermiteSpline>::Unpack(
std::span<const uint8_t> data) {
wpi::math::QuinticHermiteSpline wpi::util::Struct<
wpi::math::QuinticHermiteSpline>::Unpack(std::span<const uint8_t> data) {
return wpi::math::QuinticHermiteSpline{
wpi::util::UnpackStructArray<double, kXInitialOff, 3>(data),
wpi::util::UnpackStructArray<double, kXFinalOff, 3>(data),
@@ -22,10 +22,12 @@ wpi::math::QuinticHermiteSpline wpi::util::Struct<wpi::math::QuinticHermiteSplin
void wpi::util::Struct<wpi::math::QuinticHermiteSpline>::Pack(
std::span<uint8_t> data, const wpi::math::QuinticHermiteSpline& value) {
wpi::util::PackStructArray<kXInitialOff, 3>(data,
value.GetInitialControlVector().x);
wpi::util::PackStructArray<kXFinalOff, 3>(data, value.GetFinalControlVector().x);
wpi::util::PackStructArray<kYInitialOff, 3>(data,
value.GetInitialControlVector().y);
wpi::util::PackStructArray<kYFinalOff, 3>(data, value.GetFinalControlVector().y);
wpi::util::PackStructArray<kXInitialOff, 3>(
data, value.GetInitialControlVector().x);
wpi::util::PackStructArray<kXFinalOff, 3>(data,
value.GetFinalControlVector().x);
wpi::util::PackStructArray<kYInitialOff, 3>(
data, value.GetInitialControlVector().y);
wpi::util::PackStructArray<kYFinalOff, 3>(data,
value.GetFinalControlVector().y);
}

8
wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp
View File

@@ -8,8 +8,8 @@
#include "wpimath/protobuf/plant.npb.h"
std::optional<wpi::math::DCMotor> wpi::util::Protobuf<wpi::math::DCMotor>::Unpack(
InputStream& stream) {
std::optional<wpi::math::DCMotor>
wpi::util::Protobuf<wpi::math::DCMotor>::Unpack(InputStream& stream) {
wpi_proto_ProtobufDCMotor msg;
if (!stream.Decode(msg)) {
return {};
@@ -24,8 +24,8 @@ std::optional<wpi::math::DCMotor> wpi::util::Protobuf<wpi::math::DCMotor>::Unpac
};
}
bool wpi::util::Protobuf<wpi::math::DCMotor>::Pack(OutputStream& stream,
const wpi::math::DCMotor& value) {
bool wpi::util::Protobuf<wpi::math::DCMotor>::Pack(
OutputStream& stream, const wpi::math::DCMotor& value) {
wpi_proto_ProtobufDCMotor msg{
.nominal_voltage = value.nominalVoltage.value(),
.stall_torque = value.stallTorque.value(),

15
wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp
View File

@@ -16,16 +16,21 @@ using StructType = wpi::util::Struct<wpi::math::DCMotor>;
wpi::math::DCMotor StructType::Unpack(std::span<const uint8_t> data) {
return wpi::math::DCMotor{
wpi::units::volt_t{wpi::util::UnpackStruct<double, kNominalVoltageOff>(data)},
wpi::units::newton_meter_t{wpi::util::UnpackStruct<double, kStallTorqueOff>(data)},
wpi::units::ampere_t{wpi::util::UnpackStruct<double, kStallCurrentOff>(data)},
wpi::units::ampere_t{wpi::util::UnpackStruct<double, kFreeCurrentOff>(data)},
wpi::units::volt_t{
wpi::util::UnpackStruct<double, kNominalVoltageOff>(data)},
wpi::units::newton_meter_t{
wpi::util::UnpackStruct<double, kStallTorqueOff>(data)},
wpi::units::ampere_t{
wpi::util::UnpackStruct<double, kStallCurrentOff>(data)},
wpi::units::ampere_t{
wpi::util::UnpackStruct<double, kFreeCurrentOff>(data)},
wpi::units::radians_per_second_t{
wpi::util::UnpackStruct<double, kFreeSpeedOff>(data)},
};
}
void StructType::Pack(std::span<uint8_t> data, const wpi::math::DCMotor& value) {
void StructType::Pack(std::span<uint8_t> data,
const wpi::math::DCMotor& value) {
wpi::util::PackStruct<kNominalVoltageOff>(data, value.nominalVoltage.value());
wpi::util::PackStruct<kStallTorqueOff>(data, value.stallTorque.value());
wpi::util::PackStruct<kStallCurrentOff>(data, value.stallCurrent.value());

15
wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
View File

@@ -10,18 +10,19 @@ using namespace wpi::math;
void wpi::math::to_json(wpi::util::json& json, const Trajectory::State& state) {
json = wpi::util::json{{"time", state.t.value()},
{"velocity", state.velocity.value()},
{"acceleration", state.acceleration.value()},
{"pose", state.pose},
{"curvature", state.curvature.value()}};
{"velocity", state.velocity.value()},
{"acceleration", state.acceleration.value()},
{"pose", state.pose},
{"curvature", state.curvature.value()}};
}
void wpi::math::from_json(const wpi::util::json& json, Trajectory::State& state) {
void wpi::math::from_json(const wpi::util::json& json,
Trajectory::State& state) {
state.pose = json.at("pose").get<Pose2d>();
state.t = wpi::units::second_t{json.at("time").get<double>()};
state.velocity =
wpi::units::meters_per_second_t{json.at("velocity").get<double>()};
state.acceleration =
wpi::units::meters_per_second_squared_t{json.at("acceleration").get<double>()};
state.acceleration = wpi::units::meters_per_second_squared_t{
json.at("acceleration").get<double>()};
state.curvature = wpi::units::curvature_t{json.at("curvature").get<double>()};
}

8
wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
View File

@@ -67,9 +67,9 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
// acceleration limit. v_f = √(v_i² + 2ad).
constrainedState.maxVelocity = wpi::units::math::min(
maxVelocity,
wpi::units::math::sqrt(predecessor.maxVelocity * predecessor.maxVelocity +
predecessor.maxAcceleration * ds * 2.0));
maxVelocity, wpi::units::math::sqrt(
predecessor.maxVelocity * predecessor.maxVelocity +
predecessor.maxAcceleration * ds * 2.0));
constrainedState.minAcceleration = -maxAcceleration;
constrainedState.maxAcceleration = maxAcceleration;
@@ -131,7 +131,7 @@ Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
// v_f = √(v_i² + 2ad), where v_i = successor.
wpi::units::meters_per_second_t newMaxVelocity =
wpi::units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
successor.minAcceleration * ds * 2.0);
successor.minAcceleration * ds * 2.0);
// No more limits to impose! This state can be finalized.
if (newMaxVelocity >= constrainedState.maxVelocity) {

11
wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp
View File

@@ -9,9 +9,10 @@
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
#include "wpimath/protobuf/trajectory.npb.h"
std::optional<wpi::math::Trajectory> wpi::util::Protobuf<wpi::math::Trajectory>::Unpack(
InputStream& stream) {
wpi::util::StdVectorUnpackCallback<wpi::math::Trajectory::State, SIZE_MAX> states;
std::optional<wpi::math::Trajectory>
wpi::util::Protobuf<wpi::math::Trajectory>::Unpack(InputStream& stream) {
wpi::util::StdVectorUnpackCallback<wpi::math::Trajectory::State, SIZE_MAX>
states;
wpi_proto_ProtobufTrajectory msg{
.states = states.Callback(),
};
@@ -22,8 +23,8 @@ std::optional<wpi::math::Trajectory> wpi::util::Protobuf<wpi::math::Trajectory>:
return wpi::math::Trajectory{states.Vec()};
}
bool wpi::util::Protobuf<wpi::math::Trajectory>::Pack(OutputStream& stream,
const wpi::math::Trajectory& value) {
bool wpi::util::Protobuf<wpi::math::Trajectory>::Pack(
OutputStream& stream, const wpi::math::Trajectory& value) {
wpi::util::PackCallback<wpi::math::Trajectory::State> states{value.States()};
wpi_proto_ProtobufTrajectory msg{
.states = states.Callback(),

27
wpimath/src/main/native/include/wpi/math/controller/ArmFeedforward.hpp
View File

@@ -23,13 +23,13 @@ class WPILIB_DLLEXPORT ArmFeedforward {
public:
using Angle = wpi::units::radians;
using Velocity = wpi::units::radians_per_second;
using Acceleration = wpi::units::compound_unit<wpi::units::radians_per_second,
wpi::units::inverse<wpi::units::second>>;
using kv_unit =
wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<wpi::units::radians_per_second>>;
using ka_unit =
wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Acceleration>>;
using Acceleration =
wpi::units::compound_unit<wpi::units::radians_per_second,
wpi::units::inverse<wpi::units::second>>;
using kv_unit = wpi::units::compound_unit<
wpi::units::volts, wpi::units::inverse<wpi::units::radians_per_second>>;
using ka_unit = wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<Acceleration>>;
/**
* Creates a new ArmFeedforward with the specified gains.
@@ -44,7 +44,8 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @throws IllegalArgumentException for period &le; zero.
*/
constexpr ArmFeedforward(
wpi::units::volt_t kS, wpi::units::volt_t kG, wpi::units::unit_t<kv_unit> kV,
wpi::units::volt_t kS, wpi::units::volt_t kG,
wpi::units::unit_t<kv_unit> kV,
wpi::units::unit_t<ka_unit> kA = wpi::units::unit_t<ka_unit>(0),
wpi::units::second_t dt = 20_ms)
: kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) {
@@ -104,9 +105,9 @@ class WPILIB_DLLEXPORT ArmFeedforward {
*/
[[deprecated("Use the current/next velocity overload instead.")]]
wpi::units::volt_t Calculate(wpi::units::unit_t<Angle> currentAngle,
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity,
wpi::units::second_t dt) const {
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity,
wpi::units::second_t dt) const {
return Calculate(currentAngle, currentVelocity, nextVelocity);
}
@@ -141,8 +142,8 @@ class WPILIB_DLLEXPORT ArmFeedforward {
* @return The computed feedforward in volts.
*/
wpi::units::volt_t Calculate(wpi::units::unit_t<Angle> currentAngle,
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity) const;
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity) const;
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:

4
wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveAccelerationLimiter.hpp
View File

@@ -84,8 +84,8 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
*/
DifferentialDriveWheelVoltages Calculate(
wpi::units::meters_per_second_t leftVelocity,
wpi::units::meters_per_second_t rightVelocity, wpi::units::volt_t leftVoltage,
wpi::units::volt_t rightVoltage);
wpi::units::meters_per_second_t rightVelocity,
wpi::units::volt_t leftVoltage, wpi::units::volt_t rightVoltage);
private:
LinearSystem<2, 2, 2> m_system;

3
wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveFeedforward.hpp
View File

@@ -89,7 +89,8 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
wpi::units::meters_per_second_t currentLeftVelocity,
wpi::units::meters_per_second_t nextLeftVelocity,
wpi::units::meters_per_second_t currentRightVelocity,
wpi::units::meters_per_second_t nextRightVelocity, wpi::units::second_t dt);
wpi::units::meters_per_second_t nextRightVelocity,
wpi::units::second_t dt);
decltype(1_V / 1_mps) m_kVLinear;
decltype(1_V / 1_mps_sq) m_kALinear;

32
wpimath/src/main/native/include/wpi/math/controller/ElevatorFeedforward.hpp
View File

@@ -22,12 +22,15 @@ class ElevatorFeedforward {
public:
using Distance = wpi::units::meters;
using Velocity =
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Distance,
wpi::units::inverse<wpi::units::seconds>>;
using Acceleration =
wpi::units::compound_unit<Velocity, wpi::units::inverse<wpi::units::seconds>>;
using kv_unit = wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Velocity>>;
using ka_unit =
wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Acceleration>>;
wpi::units::compound_unit<Velocity,
wpi::units::inverse<wpi::units::seconds>>;
using kv_unit = wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<Velocity>>;
using ka_unit = wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<Acceleration>>;
/**
* Creates a new ElevatorFeedforward with the specified gains.
@@ -42,7 +45,8 @@ class ElevatorFeedforward {
* @throws IllegalArgumentException for period &le; zero.
*/
constexpr ElevatorFeedforward(
wpi::units::volt_t kS, wpi::units::volt_t kG, wpi::units::unit_t<kv_unit> kV,
wpi::units::volt_t kS, wpi::units::volt_t kG,
wpi::units::unit_t<kv_unit> kV,
wpi::units::unit_t<ka_unit> kA = wpi::units::unit_t<ka_unit>(0),
wpi::units::second_t dt = 20_ms)
: kS(kS), kG(kG), kV(kV), kA(kA), m_dt(dt) {
@@ -79,7 +83,8 @@ class ElevatorFeedforward {
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;
return kS * wpi::util::sgn(velocity) + kG + kV * velocity +
kA * acceleration;
}
/**
@@ -93,8 +98,8 @@ class ElevatorFeedforward {
*/
[[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 {
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};
@@ -161,7 +166,8 @@ class ElevatorFeedforward {
* @return The maximum possible velocity at the given acceleration.
*/
constexpr wpi::units::unit_t<Velocity> MaxAchievableVelocity(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Acceleration> acceleration) {
wpi::units::volt_t maxVoltage,
wpi::units::unit_t<Acceleration> acceleration) {
// Assume max velocity is positive
return (maxVoltage - kS - kG - kA * acceleration) / kV;
}
@@ -178,7 +184,8 @@ class ElevatorFeedforward {
* @return The minimum possible velocity at the given acceleration.
*/
constexpr wpi::units::unit_t<Velocity> MinAchievableVelocity(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Acceleration> acceleration) {
wpi::units::volt_t maxVoltage,
wpi::units::unit_t<Acceleration> acceleration) {
// Assume min velocity is negative, ks flips sign
return (-maxVoltage + kS - kG - kA * acceleration) / kV;
}
@@ -196,7 +203,8 @@ class ElevatorFeedforward {
*/
constexpr wpi::units::unit_t<Acceleration> MaxAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Velocity> velocity) {
return (maxVoltage - kS * wpi::util::sgn(velocity) - kG - kV * velocity) / kA;
return (maxVoltage - kS * wpi::util::sgn(velocity) - kG - kV * velocity) /
kA;
}
/**

3
wpimath/src/main/native/include/wpi/math/controller/LTVUnicycleController.hpp
View File

@@ -54,7 +54,8 @@ class WPILIB_DLLEXPORT LTVUnicycleController {
* @param dt Discretization timestep.
*/
LTVUnicycleController(const wpi::util::array<double, 3>& Qelems,
const wpi::util::array<double, 2>& Relems, wpi::units::second_t dt)
const wpi::util::array<double, 2>& Relems,
wpi::units::second_t dt)
: m_Q{wpi::math::MakeCostMatrix(Qelems)},
m_R{wpi::math::MakeCostMatrix(Relems)},
m_dt{dt} {}

3
wpimath/src/main/native/include/wpi/math/controller/LinearPlantInversionFeedforward.hpp
View File

@@ -44,7 +44,8 @@ class LinearPlantInversionFeedforward {
*/
template <int Outputs>
LinearPlantInversionFeedforward(
const LinearSystem<States, Inputs, Outputs>& plant, wpi::units::second_t dt)
const LinearSystem<States, Inputs, Outputs>& plant,
wpi::units::second_t dt)
: LinearPlantInversionFeedforward(plant.A(), plant.B(), dt) {}
/**

3
wpimath/src/main/native/include/wpi/math/controller/LinearQuadraticRegulator.hpp
View File

@@ -291,7 +291,8 @@ class LinearQuadraticRegulator {
*/
template <int Outputs>
void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
wpi::units::second_t dt, wpi::units::second_t inputDelay) {
wpi::units::second_t dt,
wpi::units::second_t inputDelay) {
Matrixd<States, States> discA;
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);

9
wpimath/src/main/native/include/wpi/math/controller/ProfiledPIDController.hpp
View File

@@ -34,10 +34,12 @@ class ProfiledPIDController
public:
using Distance_t = wpi::units::unit_t<Distance>;
using Velocity =
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Distance,
wpi::units::inverse<wpi::units::seconds>>;
using Velocity_t = wpi::units::unit_t<Velocity>;
using Acceleration =
wpi::units::compound_unit<Velocity, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Velocity,
wpi::units::inverse<wpi::units::seconds>>;
using Acceleration_t = wpi::units::unit_t<Acceleration>;
using State = typename TrapezoidProfile<Distance>::State;
using Constraints = typename TrapezoidProfile<Distance>::Constraints;
@@ -64,7 +66,8 @@ class ProfiledPIDController
int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
wpi::math::MathSharedStore::ReportUsage("ProfiledPIDController",
std::to_string(instances));
wpi::util::SendableRegistry::Add(this, "ProfiledPIDController", instances);
wpi::util::SendableRegistry::Add(this, "ProfiledPIDController",
instances);
}
}

22
wpimath/src/main/native/include/wpi/math/controller/SimpleMotorFeedforward.hpp
View File

@@ -24,12 +24,15 @@ template <class Distance>
class SimpleMotorFeedforward {
public:
using Velocity =
wpi::units::compound_unit<Distance, wpi::units::inverse<wpi::units::seconds>>;
wpi::units::compound_unit<Distance,
wpi::units::inverse<wpi::units::seconds>>;
using Acceleration =
wpi::units::compound_unit<Velocity, wpi::units::inverse<wpi::units::seconds>>;
using kv_unit = wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Velocity>>;
using ka_unit =
wpi::units::compound_unit<wpi::units::volts, wpi::units::inverse<Acceleration>>;
wpi::units::compound_unit<Velocity,
wpi::units::inverse<wpi::units::seconds>>;
using kv_unit = wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<Velocity>>;
using ka_unit = wpi::units::compound_unit<wpi::units::volts,
wpi::units::inverse<Acceleration>>;
/**
* Creates a new SimpleMotorFeedforward with the specified gains.
@@ -75,7 +78,8 @@ class SimpleMotorFeedforward {
* @param velocity The velocity setpoint.
* @return The computed feedforward, in volts.
*/
constexpr wpi::units::volt_t Calculate(wpi::units::unit_t<Velocity> velocity) const {
constexpr wpi::units::volt_t Calculate(
wpi::units::unit_t<Velocity> velocity) const {
return Calculate(velocity, velocity);
}
@@ -158,7 +162,8 @@ class SimpleMotorFeedforward {
* @return The maximum possible acceleration at the given velocity.
*/
constexpr wpi::units::unit_t<Acceleration> MaxAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Velocity> velocity) const {
wpi::units::volt_t maxVoltage,
wpi::units::unit_t<Velocity> velocity) const {
return (maxVoltage - kS * wpi::util::sgn(velocity) - kV * velocity) / kA;
}
@@ -174,7 +179,8 @@ class SimpleMotorFeedforward {
* @return The minimum possible acceleration at the given velocity.
*/
constexpr wpi::units::unit_t<Acceleration> MinAchievableAcceleration(
wpi::units::volt_t maxVoltage, wpi::units::unit_t<Velocity> velocity) const {
wpi::units::volt_t maxVoltage,
wpi::units::unit_t<Velocity> velocity) const {
return MaxAchievableAcceleration(-maxVoltage, velocity);
}

3
wpimath/src/main/native/include/wpi/math/controller/proto/ArmFeedforwardProto.hpp
View File

@@ -16,5 +16,6 @@ struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::ArmFeedforward> {
using InputStream = wpi::util::ProtoInputStream<wpi::math::ArmFeedforward>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::ArmFeedforward>;
static std::optional<wpi::math::ArmFeedforward> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::ArmFeedforward& value);
static bool Pack(OutputStream& stream,
const wpi::math::ArmFeedforward& value);
};

6
wpimath/src/main/native/include/wpi/math/controller/proto/DifferentialDriveFeedforwardProto.hpp
View File

@@ -11,9 +11,11 @@
#include "wpimath/protobuf/controller.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::DifferentialDriveFeedforward> {
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::DifferentialDriveFeedforward> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveFeedforward;
using InputStream = wpi::util::ProtoInputStream<wpi::math::DifferentialDriveFeedforward>;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveFeedforward>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::DifferentialDriveFeedforward>;
static std::optional<wpi::math::DifferentialDriveFeedforward> Unpack(

3
wpimath/src/main/native/include/wpi/math/controller/proto/DifferentialDriveWheelVoltagesProto.hpp
View File

@@ -11,7 +11,8 @@
#include "wpimath/protobuf/controller.npb.h"
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages> {
struct WPILIB_DLLEXPORT
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages> {
using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelVoltages;
using InputStream =
wpi::util::ProtoInputStream<wpi::math::DifferentialDriveWheelVoltages>;

12
wpimath/src/main/native/include/wpi/math/controller/proto/ElevatorFeedforwardProto.hpp
View File

@@ -13,8 +13,12 @@
template <>
struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::ElevatorFeedforward> {
using MessageStruct = wpi_proto_ProtobufElevatorFeedforward;
using InputStream = wpi::util::ProtoInputStream<wpi::math::ElevatorFeedforward>;
using OutputStream = wpi::util::ProtoOutputStream<wpi::math::ElevatorFeedforward>;
static std::optional<wpi::math::ElevatorFeedforward> Unpack(InputStream& stream);
static bool Pack(OutputStream& stream, const wpi::math::ElevatorFeedforward& value);
using InputStream =
wpi::util::ProtoInputStream<wpi::math::ElevatorFeedforward>;
using OutputStream =
wpi::util::ProtoOutputStream<wpi::math::ElevatorFeedforward>;
static std::optional<wpi::math::ElevatorFeedforward> Unpack(
InputStream& stream);
static bool Pack(OutputStream& stream,
const wpi::math::ElevatorFeedforward& value);
};

16
wpimath/src/main/native/include/wpi/math/controller/proto/SimpleMotorFeedforwardProto.hpp
View File

@@ -26,7 +26,8 @@ struct wpi::util::Protobuf<wpi::math::SimpleMotorFeedforward<Distance>> {
static std::optional<wpi::math::SimpleMotorFeedforward<Distance>> Unpack(
InputStream& stream) {
using BaseUnit =
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
wpi::units::unit<std::ratio<1>,
wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
wpi_proto_ProtobufSimpleMotorFeedforward msg;
if (!stream.Decode(msg)) {
@@ -44,14 +45,17 @@ struct wpi::util::Protobuf<wpi::math::SimpleMotorFeedforward<Distance>> {
static bool Pack(OutputStream& stream,
const wpi::math::SimpleMotorFeedforward<Distance>& value) {
using BaseUnit =
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
wpi::units::unit<std::ratio<1>,
wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
wpi_proto_ProtobufSimpleMotorFeedforward msg{
.ks = value.GetKs().value(),
.kv = wpi::units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
.value(),
.ka = wpi::units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
.value(),
.kv =
wpi::units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
.value(),
.ka =
wpi::units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
.value(),
.dt = wpi::units::second_t{value.GetDt()}.value(),
};
return stream.Encode(msg);

3
wpimath/src/main/native/include/wpi/math/controller/struct/ArmFeedforwardStruct.hpp
View File

@@ -17,7 +17,8 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::ArmFeedforward> {
}
static wpi::math::ArmFeedforward Unpack(std::span<const uint8_t> data);
static void Pack(std::span<uint8_t> data, const wpi::math::ArmFeedforward& value);
static void Pack(std::span<uint8_t> data,
const wpi::math::ArmFeedforward& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::ArmFeedforward>);

6
wpimath/src/main/native/include/wpi/math/controller/struct/DifferentialDriveFeedforwardStruct.hpp
View File

@@ -9,7 +9,8 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveFeedforward> {
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::DifferentialDriveFeedforward> {
static constexpr std::string_view GetTypeName() {
return "DifferentialDriveFeedforward";
}
@@ -25,4 +26,5 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveFeedforwar
const wpi::math::DifferentialDriveFeedforward& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::DifferentialDriveFeedforward>);
static_assert(
wpi::util::StructSerializable<wpi::math::DifferentialDriveFeedforward>);

6
wpimath/src/main/native/include/wpi/math/controller/struct/DifferentialDriveWheelVoltagesStruct.hpp
View File

@@ -9,7 +9,8 @@
#include "wpi/util/struct/Struct.hpp"
template <>
struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveWheelVoltages> {
struct WPILIB_DLLEXPORT
wpi::util::Struct<wpi::math::DifferentialDriveWheelVoltages> {
static constexpr std::string_view GetTypeName() {
return "DifferentialDriveWheelVoltages";
}
@@ -24,4 +25,5 @@ struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::DifferentialDriveWheelVolta
const wpi::math::DifferentialDriveWheelVoltages& value);
};
static_assert(wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelVoltages>);
static_assert(
wpi::util::StructSerializable<wpi::math::DifferentialDriveWheelVoltages>);

44
wpimath/src/main/native/include/wpi/math/controller/struct/SimpleMotorFeedforwardStruct.hpp
View File

@@ -26,24 +26,27 @@ struct wpi::util::Struct<wpi::math::SimpleMotorFeedforward<Distance>> {
static wpi::math::SimpleMotorFeedforward<Distance> Unpack(
std::span<const uint8_t> data) {
using BaseUnit =
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
wpi::units::unit<std::ratio<1>,
wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
constexpr size_t kKsOff = 0;
constexpr size_t kKvOff = kKsOff + 8;
constexpr size_t kKaOff = kKvOff + 8;
constexpr size_t kDtOff = kKaOff + 8;
return {wpi::units::volt_t{wpi::util::UnpackStruct<double, kKsOff>(data)},
wpi::units::unit_t<typename BaseFeedforward::kv_unit>{
wpi::util::UnpackStruct<double, kKvOff>(data)},
wpi::units::unit_t<typename BaseFeedforward::ka_unit>{
wpi::util::UnpackStruct<double, kKaOff>(data)},
wpi::units::second_t{wpi::util::UnpackStruct<double, kDtOff>(data)}};
return {
wpi::units::volt_t{wpi::util::UnpackStruct<double, kKsOff>(data)},
wpi::units::unit_t<typename BaseFeedforward::kv_unit>{
wpi::util::UnpackStruct<double, kKvOff>(data)},
wpi::units::unit_t<typename BaseFeedforward::ka_unit>{
wpi::util::UnpackStruct<double, kKaOff>(data)},
wpi::units::second_t{wpi::util::UnpackStruct<double, kDtOff>(data)}};
}
static void Pack(std::span<uint8_t> data,
const wpi::math::SimpleMotorFeedforward<Distance>& value) {
using BaseUnit =
wpi::units::unit<std::ratio<1>, wpi::units::traits::base_unit_of<Distance>>;
wpi::units::unit<std::ratio<1>,
wpi::units::traits::base_unit_of<Distance>>;
using BaseFeedforward = wpi::math::SimpleMotorFeedforward<BaseUnit>;
constexpr size_t kKsOff = 0;
constexpr size_t kKvOff = kKsOff + 8;
@@ -51,18 +54,21 @@ struct wpi::util::Struct<wpi::math::SimpleMotorFeedforward<Distance>> {
constexpr size_t kDtOff = kKaOff + 8;
wpi::util::PackStruct<kKsOff>(data, value.GetKs().value());
wpi::util::PackStruct<kKvOff>(
data, wpi::units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
.value());
data,
wpi::units::unit_t<typename BaseFeedforward::kv_unit>{value.GetKv()}
.value());
wpi::util::PackStruct<kKaOff>(
data, wpi::units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
.value());
wpi::util::PackStruct<kDtOff>(data, wpi::units::second_t{value.GetDt()}.value());
data,
wpi::units::unit_t<typename BaseFeedforward::ka_unit>{value.GetKa()}
.value());
wpi::util::PackStruct<kDtOff>(data,
wpi::units::second_t{value.GetDt()}.value());
}
};
static_assert(
wpi::util::StructSerializable<wpi::math::SimpleMotorFeedforward<wpi::units::meters>>);
static_assert(
wpi::util::StructSerializable<wpi::math::SimpleMotorFeedforward<wpi::units::feet>>);
static_assert(
wpi::util::StructSerializable<wpi::math::SimpleMotorFeedforward<wpi::units::radians>>);
static_assert(wpi::util::StructSerializable<
wpi::math::SimpleMotorFeedforward<wpi::units::meters>>);
static_assert(wpi::util::StructSerializable<
wpi::math::SimpleMotorFeedforward<wpi::units::feet>>);
static_assert(wpi::util::StructSerializable<
wpi::math::SimpleMotorFeedforward<wpi::units::radians>>);

6
wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator.hpp
View File

@@ -75,7 +75,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
DifferentialDrivePoseEstimator(
DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
wpi::units::meter_t leftDistance, wpi::units::meter_t rightDistance,
const Pose2d& initialPose, const wpi::util::array<double, 3>& stateStdDevs,
const Pose2d& initialPose,
const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs);
/**
@@ -86,7 +87,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
* @param rightDistance The distance traveled by the right encoder.
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation2d& gyroAngle, wpi::units::meter_t leftDistance,
void ResetPosition(const Rotation2d& gyroAngle,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance, const Pose2d& pose) {
PoseEstimator::ResetPosition(gyroAngle, {leftDistance, rightDistance},
pose);

6
wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp
View File

@@ -80,7 +80,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
DifferentialDrivePoseEstimator3d(
DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle,
wpi::units::meter_t leftDistance, wpi::units::meter_t rightDistance,
const Pose3d& initialPose, const wpi::util::array<double, 4>& stateStdDevs,
const Pose3d& initialPose,
const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs);
/**
@@ -91,7 +92,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
* @param rightDistance The distance traveled by the right encoder.
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation3d& gyroAngle, wpi::units::meter_t leftDistance,
void ResetPosition(const Rotation3d& gyroAngle,
wpi::units::meter_t leftDistance,
wpi::units::meter_t rightDistance, const Pose3d& pose) {
PoseEstimator3d::ResetPosition(gyroAngle, {leftDistance, rightDistance},
pose);

14
wpimath/src/main/native/include/wpi/math/estimator/KalmanFilterLatencyCompensator.hpp
View File

@@ -62,7 +62,8 @@ class KalmanFilterLatencyCompensator {
* @param timestamp The timestamp of the state.
*/
void AddObserverState(const KalmanFilterType& observer, Vectord<Inputs> u,
Vectord<Outputs> localY, wpi::units::second_t timestamp) {
Vectord<Outputs> localY,
wpi::units::second_t timestamp) {
// Add the new state into the vector.
m_pastObserverSnapshots.emplace_back(timestamp,
ObserverSnapshot{observer, u, localY});
@@ -86,7 +87,8 @@ class KalmanFilterLatencyCompensator {
*/
template <int Rows>
void ApplyPastGlobalMeasurement(
KalmanFilterType* observer, wpi::units::second_t nominalDt, Vectord<Rows> y,
KalmanFilterType* observer, wpi::units::second_t nominalDt,
Vectord<Rows> y,
std::function<void(const Vectord<Inputs>& u, const Vectord<Rows>& y)>
globalMeasurementCorrect,
wpi::units::second_t timestamp) {
@@ -130,10 +132,10 @@ class KalmanFilterLatencyCompensator {
int prevIdx = nextIdx - 1;
// Find the snapshot closest in time to global measurement
wpi::units::second_t prevTimeDiff =
wpi::units::math::abs(timestamp - m_pastObserverSnapshots[prevIdx].first);
wpi::units::second_t nextTimeDiff =
wpi::units::math::abs(timestamp - m_pastObserverSnapshots[nextIdx].first);
wpi::units::second_t prevTimeDiff = wpi::units::math::abs(
timestamp - m_pastObserverSnapshots[prevIdx].first);
wpi::units::second_t nextTimeDiff = wpi::units::math::abs(
timestamp - m_pastObserverSnapshots[nextIdx].first);
indexOfClosestEntry = prevTimeDiff < nextTimeDiff ? prevIdx : nextIdx;
}

3
wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp
View File

@@ -74,7 +74,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
MecanumDrivePoseEstimator(
MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& initialPose, const wpi::util::array<double, 3>& stateStdDevs,
const Pose2d& initialPose,
const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs);
private:

3
wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp
View File

@@ -79,7 +79,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d
MecanumDrivePoseEstimator3d(
MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose3d& initialPose, const wpi::util::array<double, 4>& stateStdDevs,
const Pose3d& initialPose,
const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs);
private:

14
wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp
View File

@@ -279,14 +279,16 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
// measurements compared to our current pose.
wpi::math::Vectord<6> k_times_twist =
m_visionK * wpi::math::Vectord<6>{twist.dx.value(), twist.dy.value(),
twist.dz.value(), twist.rx.value(),
twist.ry.value(), twist.rz.value()};
twist.dz.value(), twist.rx.value(),
twist.ry.value(), twist.rz.value()};
// Step 6: Convert back to Twist3d.
Twist3d scaledTwist{
wpi::units::meter_t{k_times_twist(0)}, wpi::units::meter_t{k_times_twist(1)},
wpi::units::meter_t{k_times_twist(2)}, wpi::units::radian_t{k_times_twist(3)},
wpi::units::radian_t{k_times_twist(4)}, wpi::units::radian_t{k_times_twist(5)}};
Twist3d scaledTwist{wpi::units::meter_t{k_times_twist(0)},
wpi::units::meter_t{k_times_twist(1)},
wpi::units::meter_t{k_times_twist(2)},
wpi::units::radian_t{k_times_twist(3)},
wpi::units::radian_t{k_times_twist(4)},
wpi::units::radian_t{k_times_twist(5)}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample.value() + scaledTwist.Exp(),

3
wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp
View File

@@ -79,7 +79,8 @@ class SwerveDrivePoseEstimator
SwerveDriveKinematics<NumModules>& kinematics,
const Rotation2d& gyroAngle,
const wpi::util::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose, const wpi::util::array<double, 3>& stateStdDevs,
const Pose2d& initialPose,
const wpi::util::array<double, 3>& stateStdDevs,
const wpi::util::array<double, 3>& visionMeasurementStdDevs)
: SwerveDrivePoseEstimator::PoseEstimator(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),

8
wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp
View File

@@ -34,8 +34,9 @@ namespace wpi::math {
*/
template <size_t NumModules>
class SwerveDrivePoseEstimator3d
: public PoseEstimator3d<wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModulePosition, NumModules>> {
: public PoseEstimator3d<
wpi::util::array<SwerveModuleState, NumModules>,
wpi::util::array<SwerveModulePosition, NumModules>> {
public:
/**
* Constructs a SwerveDrivePoseEstimator3d with default standard deviations
@@ -85,7 +86,8 @@ class SwerveDrivePoseEstimator3d
SwerveDriveKinematics<NumModules>& kinematics,
const Rotation3d& gyroAngle,
const wpi::util::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose3d& initialPose, const wpi::util::array<double, 4>& stateStdDevs,
const Pose3d& initialPose,
const wpi::util::array<double, 4>& stateStdDevs,
const wpi::util::array<double, 4>& visionMeasurementStdDevs)
: SwerveDrivePoseEstimator3d::PoseEstimator3d(
kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),

4
wpimath/src/main/native/include/wpi/math/filter/Debouncer.hpp
View File

@@ -63,7 +63,9 @@ class WPILIB_DLLEXPORT Debouncer {
* @return The number of seconds the value must change from baseline
* for the filtered value to change.
*/
constexpr wpi::units::second_t GetDebounceTime() const { return m_debounceTime; }
constexpr wpi::units::second_t GetDebounceTime() const {
return m_debounceTime;
}
/**
* Set the debounce type.

3
wpimath/src/main/native/include/wpi/math/filter/LinearFilter.hpp
View File

@@ -191,7 +191,8 @@ class LinearFilter {
*/
template <int Derivative, int Samples>
static LinearFilter<T> FiniteDifference(
const wpi::util::array<int, Samples>& stencil, wpi::units::second_t period) {
const wpi::util::array<int, Samples>& stencil,
wpi::units::second_t period) {
// See
// https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
//

10
wpimath/src/main/native/include/wpi/math/filter/SlewRateLimiter.hpp
View File

@@ -24,7 +24,8 @@ template <class Unit>
class SlewRateLimiter {
public:
using Unit_t = wpi::units::unit_t<Unit>;
using Rate = wpi::units::compound_unit<Unit, wpi::units::inverse<wpi::units::seconds>>;
using Rate =
wpi::units::compound_unit<Unit, wpi::units::inverse<wpi::units::seconds>>;
using Rate_t = wpi::units::unit_t<Rate>;
/**
@@ -44,8 +45,8 @@ class SlewRateLimiter {
: m_positiveRateLimit{positiveRateLimit},
m_negativeRateLimit{negativeRateLimit},
m_prevVal{initialValue},
m_prevTime{
wpi::units::microsecond_t{wpi::math::MathSharedStore::GetTimestamp()}} {}
m_prevTime{wpi::units::microsecond_t{
wpi::math::MathSharedStore::GetTimestamp()}} {}
/**
* Creates a new SlewRateLimiter with the given positive rate limit and
@@ -64,7 +65,8 @@ class SlewRateLimiter {
* rate.
*/
Unit_t Calculate(Unit_t input) {
wpi::units::second_t currentTime = wpi::math::MathSharedStore::GetTimestamp();
wpi::units::second_t currentTime =
wpi::math::MathSharedStore::GetTimestamp();
wpi::units::second_t elapsedTime = currentTime - m_prevTime;
m_prevVal +=
std::clamp(input - m_prevVal, m_negativeRateLimit * elapsedTime,

Some files were not shown because too many files have changed in this diff Show More