Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +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(),