Squashed 'third_party/allwpilib/' changes from f1a82828fe..ce550705d7
ce550705d7 [ntcore] Fix client "received unknown id -1" (#6186)
3989617bde [ntcore] NetworkTable::GetStruct: Add I template param (#6183)
f1836e1321 [fieldImages] Fix 2024 field json (#6179)
d05f179a9a [build] Fix running apriltagsvision Java example (#6173)
b1b03bed85 [wpilib] Update MotorControllerGroup deprecation message (#6171)
fa63fbf446 LICENSE.md: Bump year to 2024 (#6169)
4809f3d0fc [apriltag] Add 2024 AprilTag locations (#6168)
dd90965362 [wpiutil] Fix RawFrame.setInfo() NPE (#6167)
8659372d08 [fieldImages] Add 2024 field image (#6166)
a2e4d0b15d [docs] Fix docs for SysID routine (#6164)
0a46a3a618 [wpilib] Make ADXL345 default I2C address public (#6163)
7c26bc70ab [sysid] Load DataLog files directly for analysis (#6103)
f94e3d81b9 [docs] Fix SysId routine JavaDoc warnings (#6159)
6bed82a18e [wpilibc] Clean up C++ SysId routine (#6160)
4595f84719 [wpilib] Report LiveWindow-enabled-in-test (#6158)
707cb06105 [wpilib] Add SysIdRoutine logging utility and command factory (#6033)
3e40b9e5da [wpilib] Correct SmartDashboard usage reporting (#6157)
106518c3f8 [docs] Fix wpilibj JavaDoc warnings (#6154)
19cb2a8eb4 [wpilibj] Make class variables private to match C++ (#6153)
13f4460e00 [docs] Add missing docs to enum fields (NFC) (#6150)
4210f5635d [docs] Fix warnings about undocumented default constructors (#6151)
0f060afb55 [ntcore] Disable WebSocket fragmentation (#6149)
f29a7d2e50 [docs] Add missing JavaDocs (#6146)
6e58db398d [commands] Make Java fields private (#6148)
4ac0720385 [build] Clean up CMake files (#6141)
44db3e0ac0 [sysid] Make constexpr variables outside class scope inline (#6145)
73c7d87db7 [glass] NTStringChooser: Properly set retained (#6144)
25636b712f [build] Remove unnecessary native dependencies in wpilibjExamples (#6143)
01fb98baaa [docs] Add Missing JNI docs from C++ (NFC) (#6139)
5c424248c4 [wpilibj] Remove unused AnalogTriggerException (#6142)
c486972c55 [wpimath] Make ExponentialProfile.State mutable (#6138)
783acb9b72 [wpilibj] Store long preferences as integers (#6136)
99ab836894 [wpiutil] Add missing JavaDocs (NFC) (#6132)
ad0859a8c9 [docs] Add missing JavaDocs (#6125)
5579219716 [docs] Exclude quickbuf files and proto/struct packages from doclint (#6128)
98f06911c7 [sysid] Use eigenvector component instead of eigenvalue for fit quality check (#6131)
e1d49b975c [wpimath] Add LinearFilter reset() overload to initialize input and output buffers (#6133)
8a0bf2b7a4 [hal] Add CANAPITypes to java (#6121)
91d8837c11 [wpilib] Make protected fields in accelerometers/gyros private (#6134)
e7c9f27683 [wpilib] Add functional interface equivalents to MotorController (#6053)
8aca706217 [glass] Add type information to SmartDashboard menu (#6117)
7d3e4ddba9 [docs] Add warning about using user button to docs (NFC) (#6129)
ec3cb3dcba [build] Disable clang-tidy warning about test case names (#6127)
495585b25d [examples] Update april tag family to 36h11 (#6126)
f9aabc5ab2 [wpilib] Throw early when EventLoop is modified while running (#6115)
c16946c0ec [hal] Add CANJNI docs (NFC) (#6120)
b7f4eb2811 [doc] Update maven artifacts for units and apriltags (NFC) (#6123)
f419a62b38 [doc] Update maintainers.md (NFC) (#6124)
938bf45fd9 [wpiutil] Remove type param from ProtobufSerializable and StructSerializable (#6122)
c34debe012 [docs] Link to external OpenCV docs (#6119)
07183765de [hal] Fix formatting of HAL_ENUM enums (NFC) (#6114)
af46034b7f [wpilib] Document only first party controllers are guaranteed to have correct mapping (#6112)
636ef58d94 [hal] Properly error check readCANStreamSession (#6108)
cc631d2a69 [build] Fix generated source set location in the HAL (#6113)
09f76b32c2 [wpimath] Compile with UTF-8 encoding (#6111)
47c5fd8620 [sysid] Check data quality before OLS (#6110)
24a76be694 [hal] Add method to detect if the CAN Stream has overflowed (#6105)
9333951736 [hal] Allocate CANStreamMessage in JNI if null (#6107)
6a2d3c30a6 [wpiutil] Struct: Add info template parameter pack (#6086)
e07de37e64 [commands] Mark ParallelDeadlineGroup.setDeadline() final (#6102)
141241d2d6 [wpilib] Fix usage reporting for static classes (#6090)
f2c2bab7dc [sysid] Fix adjusted R² calculation (#6101)
5659038443 [wpiutil,cscore,apriltag] Fix RawFrame (#6098)
8aeee03626 [commands] Improve error message when composing commands twice in same composition (#6091)
55508706ff [wpiutil,cscore] Move VideoMode.PixelFormat to wpiutil (#6097)
ab78b930e9 [wpilib] ADIS16470: Add access to all 3 axes (#6074)
795d4be9fd [wpilib] Fix precision issue in Color round-and-clamp (#6100)
7aa9ad44b8 [commands] Deprecate C++ TransferOwnership() (#6095)
92c81d0791 [ci] Update pregenerate workflow to actions/checkout@v4 (#6094)
1ce617be07 [ci] Update artifact actions to v4 (#6092)
2441b57156 [wpilib] Add PWMSparkFlex MotorController (#6089)
21d1972d7a [wpiutil] DataLog: Ensure file is written on shutdown (#6087)
c29e8c66cf [wpiutil] DataLog: Fix UB in AppendImpl (#6088)
ab309e34ef [glass] Fix order of loading window settings (#6056)
22a322c9f3 [wpimath] Report error on negative PID gains (#6055)
1dba26c937 [wpilib] Add method to get breaker fault at a specific channel in PowerDistribution[Sticky]Faults (#5521)
ef1cb3f41e [commands] Fix compose-while-scheduled issue and test all compositions (#5581)
aeb1a4aa33 [wpiutil] Add serializable marker interfaces (#6060)
c1178d5add [wpilib] Add StadiaController and command wrapper (#6083)
4e4a468d4d [wpimath] Make feedforward classes throw exceptions for negative Kv or Ka (#6084)
d1793f077d [build] cmake: Add NO_WERROR option to disable -Werror (#6071)
43fb6e9f87 [glass] Add Profiled PID controller support & IZone Support (#5959)
bcef6c5398 [apriltag] Fix Java generation functions (#6063)
4059e0cd9f [hal,wpilib] Add function to control "Radio" LED (#6073)
0b2cfb3abc [dlt] Change datalogtool default folder to logs folder (#6079)
df5e439b0c [wpilib] PS4Controller: enable usage reporting (#6081)
0ff7478968 [cscore] Fix RawFrame class not being loaded in JNI (#6077)
6f23d32fe1 [wpilib] AddressableLED: Update warning about single driver (NFC) (#6069)
35a1c52788 [build] Upgrade quickbuf to 1.3.3 (#6072)
e4e2bafdb1 [sysid] Document timestamp units (#6065)
3d201c71f7 [ntcore] Fix overlapping subscriber handling (#6067)
f02984159f [glass] Check for null entries when updating struct/proto (#6059)
a004c9e05f [commands] SubsystemBase: allow setting name in constructor (#6052)
0b4c6a1546 [wpimath] Add more docs to SimulatedAnnealing (NFC) (#6054)
ab15dae887 [wpilib] ArcadeDrive: Fix max output handling (#6051)
9599c1f56f [hal] Add usage reporting ids from 2024v2 image (#6041)
f87c64af8a [wpimath] MecanumDriveWheelSpeeds: Fix desaturate() (#6040)
8798700cec [wpilibcExamples] Add inline specifier to constexpr constants (#6049)
85c9ae6eff [wpilib] Fix PS5 Controller mappings (#6050)
7c8b7a97ad [wpiutil] Zero out roborio system timestamp (#6042)
d9b504bc84 [wpilib] DataLogManager: Change sim location to logs subdir (#6039)
906b810136 [build] cmake: Fix ntcore generated header install (#6038)
56e5b404d1 Update to final 2024 V2 image (#6034)
8723ee5c39 [ntcore] Add cached topic property (#5494)
192a28af47 Fix JDK 21 warnings (#6028)
d40bdd70ba [build] Upgrade to spotbugs Gradle plugin 6.0.2 (#6027)
7bfadf32e5 [wpilibj] Joystick: make remainder of get axis methods final (#6024)
a770110438 [commands] CommandCompositionError: Include stacktrace of original composition (#5984)
54a55b8b53 [wpiutil,hal] Update image; init Rio Now() HMB with a FPGA session (#6016)
7d4e515a6b [wpimath] Simplify calculation of C for DARE precondition (#6022)
5200316c14 [ntcore] Update transmit period on topic add/remove (#6021)
ddf79a25d4 [wpiunits] Overload Measure.per(Time) to return Measure<Velocity> (#6018)
a71adef316 [wpiutil] Clean up circular_buffer iterator syntax (#6020)
39a0bf4b98 [examples] Call resetOdometry() when controller command is executed (#5905)
f5fc101fda [build] cmake: Export jars and clean up jar installs (#6014)
38bf024c96 [build] Update to Gradle 8.5 (#6007)
9d11544c18 [wpimath] Rotate traveling salesman solution so input and solution have same initial pose (#6015)
28deba20f5 [wpimath] Commit generated quickbuf Java files (#5994)
c2971c0bb3 [build] cmake: Export apriltag and wpimath (#6012)
41cfc961e4 gitattributes: Add linguist-generated locations (#6004)
14c3ade155 [wpimath] Struct cleanup (#6011)
90757b9e90 [wpilib] Make Color::HexString() constexpr (#5985)
2676b77873 Fix compilation issues that occur when building with bazel (#6008)
d32c10487c [examples] Update C++ examples to use CommandPtr (#5988)
9bc5fcf886 [build] cmake: Default WITH_JAVA_SOURCE to WITH_JAVA (#6005)
d431abba3b [upstream_utils] Fix GCEM namespace usage and add hypot(x, y, z) (#6002)
2bb1409b82 Clean up Java style (#5990)
66172ab288 Remove submodule (#6003)
e8f8c0ceb0 [upstream_utils] Update to latest Eigen HEAD (#5996)
890992a849 [hal] Commit generated usage reporting files (#5993)
a583ca01e1 [wpiutil] Change Struct to allow non-constexpr implementation (#5992)
ca272de400 [build] Fix Gradle compile_commands.json and clang-tidy warnings (#5977)
76ae090570 [wpiutil] type_traits: Add is_constexpr() (#5997)
5172ab8fd0 [commands] C++ CommandPtr: Prevent null initialization (#5991)
96914143ba [build] Bump native-utils to fix compile_commands.json (#5989)
464e6121ef [ci] Report failed status to Azure on failed tests (#2654)
5dad46cd45 [wpimath] Commit generated files (#5986)
54ab65a63a [ntcore] Commit generated files (#5962)
7ed900ae3a [wpilib] Add hex string constructor to Color and Color8Bit (#5063)
74b85b76a9 [wpimath] Make gcem call std functions if not constant-evaluated (#5983)
30816111db [wpimath] Fix TimeInterpolatableBuffer crash (#5972)
5cc923de33 [wpilib] DataLogManager: Use logs subdirectory on USB drives (#5975)
1144115da0 [commands] Add GetName to Subsystem, use in Scheduler tracer epochs (#5836)
ac7d726ac3 [wpimath] Add simulated annealing (#5961)
e09be72ee0 [wpimath] Remove unused SimpleMatrixUtils class (#5979)
0f9ebe92d9 [wpimath] Add generic circular buffer class to Java (#5969)
9fa28eb07a [ci] Bump actions/checkout to v4 (#5736)
ca684ac207 [hal] Add capability to read power distribution data as a stream (#4983)
51eecef2bd [wpimath] Optimize 2nd derivative of quintic splines (#3292)
4fcf0b25a1 [build] Apply a formatter for CMake files (#5973)
9b8011aa67 [build] Pin wpiformat version (#5982)
e00a0e84c1 [build] cmake: fix protobuf dependency finding for certain distributions (#5981)
23dd591394 [upstream_utils] Remove libuv patch that adjusts whitespace (#5976)
b0719942f0 [wpiutil] Timestamp: Report errors on Rio HMB init failure (#5974)
7bc89c4322 [wpilib] Update getAlliance() docs (NFC) (#5971)
841ea682d1 [upstream_utils] Upgrade to LLVM 17.0.5 (#5970)
a74db52dae [cameraserver] Add getVideo() pixelFormat overload (#5966)
a7eb422662 [build] Update native utils for new compile commands files (#5968)
544b231d4d [sysid] Add missing cassert include (#5967)
31cd015970 [wpimath] Add SysId doc links to LinearSystemId in C++ (NFC) (#5960)
9280054eab Revert "[build] Export wpimath protobuf symbols (#5952)"
2aba97c610 Export pb files from wpimath
c80b2d2017 [build] Export wpimath protobuf symbols (#5952)
3c0652c18a [cscore] Replace CS_PixelFormat with WPI_PixelFormat (#5954)
95716eb0cb [wpiunits] Documentation improvements (#5932)
423fd75fa8 [wpilib] Default LiveWindowEnabledInTest to false (#5950)
dfdea9c992 [wpimath] Make KalmanFilter variant for asymmetric updates (#5951)
ca81ced409 [wpiutil] Move RawFrame to wpiutil; add generation of RawFrame for AprilTags (#5923)
437cc91af5 [cscore] CvSink: Allow specifying output PixelFormat (#5943)
25b7dca46b [build] Remove CMake flat install option (#5944)
bb05e20247 [wpimath] Add protobuf/struct for trivial types (#5935)
35744a036e [wpimath] Move struct/proto classes to separate files (#5918)
80d7ad58ea [build] Declare platform launcher dependency explicitly (#5909)
f8d983b154 [ntcore] Protobuf/Struct: Use atomic_bool instead of atomic_flag (#5946)
4a44210ee3 [ntcore] NetworkTableInstance: Suppress unused lambda capture warning (#5947)
bdc8620d55 [upstream_utils] Fix fmt compilation errors on Windows (#5948)
0ca1e9b5f9 [wpimath] Add basic wpiunits support (#5821)
cc30824409 [ntcore] Increase client meta-topic decoding limit (#5934)
b1fad062f7 [wpilib] Use RKDP in DifferentialDrivetrainSim (#5931)
ead9ae5a69 [build] Add generateProto dependency to test and dev (#5933)
cfbff32185 [wpiutil] timestamp: Fix startup race on Rio (#5930)
7d90d0bcc3 [wpimath] Clean up StateSpaceUtil (#5891)
7755e45aac [build] Add generated protobuf headers to C++ test include path (#5926)
3985c031da [ntcore] ProtobufSubscriber: Fix typos (#5928)
7a87fe4b60 [ntcore] ProtobufSubscriber: Make mutex and msg mutable (#5927)
09f3ed6a5f [commands] Add static Trigger factories for robot mode changes (#5902)
79dd795bc0 [wpimath] Clean up VecBuilder and MatBuilder (#5906)
e117274a67 [wpilib] Change default Rio log dir from /home/lvuser to /home/lvuser/logs (#5899)
a8b80ca256 [upstream_utils] Update to libuv 1.47.0 (#5889)
b3a9c3e96b [build] Bump macOS deployment target to 12 (#5890)
0f8129677b [build] Distribute wpimath protobuf headers (#5925)
d105f9e3e9 [wpiutil] ProtobufBuffer: Fix buffer reallocation (#5924)
c5f2f6a0fb [fieldImages] Fix typo in field images artifact name (#5922)
c1a57e422a [commands] Clean up make_vector.h (#5917)
78ebc6e9ec [wpimath] change G to gearing in LinearSystemId factories (#5834)
9ada181866 [hal] DriverStation.h: Add stddef.h include (#5897)
95fa5ec72f [wpilibc,ntcoreffi] DataLogManager: join on Stop() call (#5910)
b6f2d3cc14 [build] Remove usage of Version.parse (#5911)
cc2cbeb04c [examples] Replace gyro rotation with poseEstimator rotation (#5900)
fa6b171e1c [wpiutil] Suppress protobuf warning false positives on GCC 13 (#5907)
d504639bbe [apriltag] Improve AprilTag docs (#5895)
3a1194be40 Replace static_cast<void>() with [[maybe_unused]] attribute (#5892)
70392cbbcb [build] cmake: Add protobuf dependency to wpiutil-config (#5886)
17c1bd5a83 [ntcore] Use json_fwd (#5881)
e69a9efeba [wpilibcExamples] Match array parameter bounds (#5880)
14dcd0d26f Use char instead of uint8_t for json::parse (#5877)
ec1d261984 [hal] Fix garbage data for match info before DS connection (#5879)
63dbf5c614 [wpiutil] MemoryBuffer: Fix normal read and file type check (#5875)
b2e7be9250 [ntcore] Only datalog meta-topics if specifically requested (#5873)
201a42a3cd [wpimath] Reorder TrapezoidProfile.calculate() arguments (#5874)
04a781b4d7 [apriltag] Add GetTags to C++ version of AprilTagFieldLayout (#5872)
87a8a1ced4 [docs] Exclude eigen and protobuf from doxygen (#5871)
git-subtree-dir: third_party/allwpilib
git-subtree-split: ce550705d7cdab117c0153a202973fc026a81274
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ic8645d0551d62b411b0a816c493f0f33291896a1
diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
index 758d2e7..5dcf1a1 100644
--- a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
+++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
@@ -18,21 +18,12 @@
pose.Rotation().Sin()};
}
-template <>
-bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A, const Matrixd<1, 1>& B) {
- return detail::IsStabilizableImpl<1, 1>(A, B);
-}
-
-template <>
-bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A, const Matrixd<2, 1>& B) {
- return detail::IsStabilizableImpl<2, 1>(A, B);
-}
-
-template <>
-bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(const Eigen::MatrixXd& A,
- const Eigen::MatrixXd& B) {
- return detail::IsStabilizableImpl<Eigen::Dynamic, Eigen::Dynamic>(A, B);
-}
+template bool IsStabilizable<1, 1>(const Matrixd<1, 1>& A,
+ const Matrixd<1, 1>& B);
+template bool IsStabilizable<2, 1>(const Matrixd<2, 2>& A,
+ const Matrixd<2, 1>& B);
+template bool IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(
+ const Eigen::MatrixXd& A, const Eigen::MatrixXd& B);
Eigen::Vector3d PoseToVector(const Pose2d& pose) {
return Eigen::Vector3d{pose.X().value(), pose.Y().value(),
diff --git a/wpimath/src/main/native/cpp/controller/PIDController.cpp b/wpimath/src/main/native/cpp/controller/PIDController.cpp
index 2638f9e..a67d56d 100644
--- a/wpimath/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpimath/src/main/native/cpp/controller/PIDController.cpp
@@ -18,10 +18,32 @@
PIDController::PIDController(double Kp, double Ki, double Kd,
units::second_t period)
: m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
+ bool invalidGains = false;
+ if (Kp < 0.0) {
+ wpi::math::MathSharedStore::ReportError(
+ "Kp must be a non-negative number, got {}!", Kp);
+ invalidGains = true;
+ }
+ if (Ki < 0.0) {
+ wpi::math::MathSharedStore::ReportError(
+ "Ki must be a non-negative number, got {}!", Ki);
+ invalidGains = true;
+ }
+ if (Kd < 0.0) {
+ wpi::math::MathSharedStore::ReportError(
+ "Kd must be a non-negative number, got {}!", Kd);
+ invalidGains = true;
+ }
+ if (invalidGains) {
+ m_Kp = 0.0;
+ m_Ki = 0.0;
+ m_Kd = 0.0;
+ wpi::math::MathSharedStore::ReportWarning("PID gains defaulted to 0.");
+ }
+
if (period <= 0_s) {
wpi::math::MathSharedStore::ReportError(
- "Controller period must be a non-zero positive number, got {}!",
- period.value());
+ "Controller period must be a positive number, got {}!", period.value());
m_period = 20_ms;
wpi::math::MathSharedStore::ReportWarning(
"Controller period defaulted to 20ms.");
diff --git a/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp b/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp
new file mode 100644
index 0000000..37a55cf
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/proto/ArmFeedforwardProto.cpp
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/proto/ArmFeedforwardProto.h"
+
+#include "controller.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::ArmFeedforward>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufArmFeedforward>(arena);
+}
+
+frc::ArmFeedforward wpi::Protobuf<frc::ArmFeedforward>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufArmFeedforward*>(&msg);
+ return frc::ArmFeedforward{
+ units::volt_t{m->ks()},
+ units::volt_t{m->kg()},
+ units::unit_t<frc::ArmFeedforward::kv_unit>{m->kv()},
+ units::unit_t<frc::ArmFeedforward::ka_unit>{m->ka()},
+ };
+}
+
+void wpi::Protobuf<frc::ArmFeedforward>::Pack(
+ google::protobuf::Message* msg, const frc::ArmFeedforward& value) {
+ auto m = static_cast<wpi::proto::ProtobufArmFeedforward*>(msg);
+ m->set_ks(value.kS.value());
+ m->set_kg(value.kG.value());
+ m->set_kv(value.kV.value());
+ m->set_ka(value.kA.value());
+}
diff --git a/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp b/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp
new file mode 100644
index 0000000..b0b16b9
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/proto/DifferentialDriveWheelVoltagesProto.cpp
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/proto/DifferentialDriveWheelVoltagesProto.h"
+
+#include "controller.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<
+ frc::DifferentialDriveWheelVoltages>::New(google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufDifferentialDriveWheelVoltages>(arena);
+}
+
+frc::DifferentialDriveWheelVoltages
+wpi::Protobuf<frc::DifferentialDriveWheelVoltages>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m =
+ static_cast<const wpi::proto::ProtobufDifferentialDriveWheelVoltages*>(
+ &msg);
+ return frc::DifferentialDriveWheelVoltages{
+ units::volt_t{m->left()},
+ units::volt_t{m->right()},
+ };
+}
+
+void wpi::Protobuf<frc::DifferentialDriveWheelVoltages>::Pack(
+ google::protobuf::Message* msg,
+ const frc::DifferentialDriveWheelVoltages& value) {
+ auto m =
+ static_cast<wpi::proto::ProtobufDifferentialDriveWheelVoltages*>(msg);
+ m->set_left(value.left.value());
+ m->set_right(value.right.value());
+}
diff --git a/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp b/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp
new file mode 100644
index 0000000..4b4bce2
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/proto/ElevatorFeedforwardProto.cpp
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/proto/ElevatorFeedforwardProto.h"
+
+#include "controller.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::ElevatorFeedforward>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufElevatorFeedforward>(arena);
+}
+
+frc::ElevatorFeedforward wpi::Protobuf<frc::ElevatorFeedforward>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufElevatorFeedforward*>(&msg);
+ return frc::ElevatorFeedforward{
+ units::volt_t{m->ks()},
+ units::volt_t{m->kg()},
+ units::unit_t<frc::ElevatorFeedforward::kv_unit>{m->kv()},
+ units::unit_t<frc::ElevatorFeedforward::ka_unit>{m->ka()},
+ };
+}
+
+void wpi::Protobuf<frc::ElevatorFeedforward>::Pack(
+ google::protobuf::Message* msg, const frc::ElevatorFeedforward& value) {
+ auto m = static_cast<wpi::proto::ProtobufElevatorFeedforward*>(msg);
+ m->set_ks(value.kS());
+ m->set_kg(value.kG());
+ m->set_kv(value.kV());
+ m->set_ka(value.kA());
+}
diff --git a/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp b/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp
new file mode 100644
index 0000000..6b0070c
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/struct/ArmFeedforwardStruct.cpp
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/struct/ArmFeedforwardStruct.h"
+
+namespace {
+constexpr size_t kKsOff = 0;
+constexpr size_t kKgOff = kKsOff + 8;
+constexpr size_t kKvOff = kKgOff + 8;
+constexpr size_t kKaOff = kKvOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::ArmFeedforward>;
+
+frc::ArmFeedforward StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::ArmFeedforward{
+ units::volt_t{wpi::UnpackStruct<double, kKsOff>(data)},
+ units::volt_t{wpi::UnpackStruct<double, kKgOff>(data)},
+ units::unit_t<frc::ArmFeedforward::kv_unit>{
+ wpi::UnpackStruct<double, kKvOff>(data)},
+ units::unit_t<frc::ArmFeedforward::ka_unit>{
+ wpi::UnpackStruct<double, kKaOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::ArmFeedforward& value) {
+ wpi::PackStruct<kKsOff>(data, value.kS());
+ wpi::PackStruct<kKgOff>(data, value.kG());
+ wpi::PackStruct<kKvOff>(data, value.kV());
+ wpi::PackStruct<kKaOff>(data, value.kA());
+}
diff --git a/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp b/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp
new file mode 100644
index 0000000..22a7930
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/struct/DifferentialDriveWheelVoltagesStruct.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/struct/DifferentialDriveWheelVoltagesStruct.h"
+
+namespace {
+constexpr size_t kLeftOff = 0;
+constexpr size_t kRightOff = kLeftOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::DifferentialDriveWheelVoltages>;
+
+frc::DifferentialDriveWheelVoltages StructType::Unpack(
+ std::span<const uint8_t> data) {
+ return frc::DifferentialDriveWheelVoltages{
+ units::volt_t{wpi::UnpackStruct<double, kLeftOff>(data)},
+ units::volt_t{wpi::UnpackStruct<double, kRightOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::DifferentialDriveWheelVoltages& value) {
+ wpi::PackStruct<kLeftOff>(data, value.left.value());
+ wpi::PackStruct<kRightOff>(data, value.right.value());
+}
diff --git a/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp b/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp
new file mode 100644
index 0000000..ff28357
--- /dev/null
+++ b/wpimath/src/main/native/cpp/controller/struct/ElevatorFeedforwardStruct.cpp
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/struct/ElevatorFeedforwardStruct.h"
+
+namespace {
+constexpr size_t kKsOff = 0;
+constexpr size_t kKgOff = kKsOff + 8;
+constexpr size_t kKvOff = kKgOff + 8;
+constexpr size_t kKaOff = kKvOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::ElevatorFeedforward>;
+
+frc::ElevatorFeedforward StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::ElevatorFeedforward{
+ units::volt_t{wpi::UnpackStruct<double, kKsOff>(data)},
+ units::volt_t{wpi::UnpackStruct<double, kKgOff>(data)},
+ units::unit_t<frc::ElevatorFeedforward::kv_unit>{
+ wpi::UnpackStruct<double, kKvOff>(data)},
+ units::unit_t<frc::ElevatorFeedforward::ka_unit>{
+ wpi::UnpackStruct<double, kKaOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::ElevatorFeedforward& value) {
+ wpi::PackStruct<kKsOff>(data, value.kS());
+ wpi::PackStruct<kKgOff>(data, value.kG());
+ wpi::PackStruct<kKvOff>(data, value.kV());
+ wpi::PackStruct<kKaOff>(data, value.kA());
+}
diff --git a/wpimath/src/main/native/cpp/estimator/SteadyStateKalmanFilter.cpp b/wpimath/src/main/native/cpp/estimator/SteadyStateKalmanFilter.cpp
new file mode 100644
index 0000000..2458623
--- /dev/null
+++ b/wpimath/src/main/native/cpp/estimator/SteadyStateKalmanFilter.cpp
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/estimator/SteadyStateKalmanFilter.h"
+
+namespace frc {
+
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+ SteadyStateKalmanFilter<1, 1, 1>;
+template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
+ SteadyStateKalmanFilter<2, 1, 1>;
+
+} // namespace frc
diff --git a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
index 2e15216..53779d0 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
@@ -9,7 +9,6 @@
#include <wpi/json.h>
#include "frc/MathUtil.h"
-#include "geometry2d.pb.h"
using namespace frc;
@@ -109,23 +108,3 @@
pose = Pose2d{json.at("translation").get<Translation2d>(),
json.at("rotation").get<Rotation2d>()};
}
-
-google::protobuf::Message* wpi::Protobuf<frc::Pose2d>::New(
- google::protobuf::Arena* arena) {
- return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
- arena);
-}
-
-frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
- const google::protobuf::Message& msg) {
- auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
- return Pose2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
- wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
- const frc::Pose2d& value) {
- auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
- wpi::PackProtobuf(m->mutable_translation(), value.Translation());
- wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}
diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
index ffbaecb..90f7b49 100644
--- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp
@@ -189,23 +189,3 @@
pose = Pose3d{json.at("translation").get<Translation3d>(),
json.at("rotation").get<Rotation3d>()};
}
-
-google::protobuf::Message* wpi::Protobuf<frc::Pose3d>::New(
- google::protobuf::Arena* arena) {
- return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
- arena);
-}
-
-frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
- const google::protobuf::Message& msg) {
- auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
- return Pose3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
- wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
- const frc::Pose3d& value) {
- auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
- wpi::PackProtobuf(m->mutable_translation(), value.Translation());
- wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}
diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
index 37afbb8..3aad64c 100644
--- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp
@@ -232,24 +232,3 @@
Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
json.at("Y").get<double>(), json.at("Z").get<double>()};
}
-
-google::protobuf::Message* wpi::Protobuf<frc::Quaternion>::New(
- google::protobuf::Arena* arena) {
- return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
- arena);
-}
-
-frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
- const google::protobuf::Message& msg) {
- auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
- return frc::Quaternion{m->w(), m->x(), m->y(), m->z()};
-}
-
-void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
- const frc::Quaternion& value) {
- auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
- m->set_w(value.W());
- m->set_x(value.X());
- m->set_y(value.Y());
- m->set_z(value.Z());
-}
diff --git a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
index 05a644e..6919302 100644
--- a/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -20,21 +20,3 @@
void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
}
-
-google::protobuf::Message* wpi::Protobuf<frc::Rotation2d>::New(
- google::protobuf::Arena* arena) {
- return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
- arena);
-}
-
-frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
- const google::protobuf::Message& msg) {
- auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
- return frc::Rotation2d{units::radian_t{m->value()}};
-}
-
-void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
- const frc::Rotation2d& value) {
- auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
- m->set_value(value.Radians().value());
-}
diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
index 3a68870..072d023 100644
--- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
@@ -262,21 +262,3 @@
void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
}
-
-google::protobuf::Message* wpi::Protobuf<frc::Rotation3d>::New(
- google::protobuf::Arena* arena) {
- return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
- arena);
-}
-
-frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
- const google::protobuf::Message& msg) {
- auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
- return Rotation3d{wpi::UnpackProtobuf<frc::Quaternion>(m->q())};
-}
-
-void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
- const frc::Rotation3d& value) {
- auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
- wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
-}
diff --git a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
index 77f3cee..157359b 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
@@ -22,23 +22,3 @@
Transform2d Transform2d::operator+(const Transform2d& other) const {
return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
}
-
-google::protobuf::Message* wpi::Protobuf<frc::Transform2d>::New(
- google::protobuf::Arena* arena) {
- return google::protobuf::Arena::CreateMessage<
- wpi::proto::ProtobufTransform2d>(arena);
-}
-
-frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
- const google::protobuf::Message& msg) {
- auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
- return Transform2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
- wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
- const frc::Transform2d& value) {
- auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
- wpi::PackProtobuf(m->mutable_translation(), value.Translation());
- wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}
diff --git a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
index de6c253..556a302 100644
--- a/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Transform3d.cpp
@@ -5,7 +5,6 @@
#include "frc/geometry/Transform3d.h"
#include "frc/geometry/Pose3d.h"
-#include "geometry3d.pb.h"
using namespace frc;
@@ -36,23 +35,3 @@
Transform3d Transform3d::operator+(const Transform3d& other) const {
return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
}
-
-google::protobuf::Message* wpi::Protobuf<frc::Transform3d>::New(
- google::protobuf::Arena* arena) {
- return google::protobuf::Arena::CreateMessage<
- wpi::proto::ProtobufTransform3d>(arena);
-}
-
-frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
- const google::protobuf::Message& msg) {
- auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
- return Transform3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
- wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
- const frc::Transform3d& value) {
- auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
- wpi::PackProtobuf(m->mutable_translation(), value.Translation());
- wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}
diff --git a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index 6d5f315..c875582 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
@@ -49,22 +49,3 @@
translation = Translation2d{units::meter_t{json.at("x").get<double>()},
units::meter_t{json.at("y").get<double>()}};
}
-
-google::protobuf::Message* wpi::Protobuf<frc::Translation2d>::New(
- google::protobuf::Arena* arena) {
- return google::protobuf::Arena::CreateMessage<
- wpi::proto::ProtobufTranslation2d>(arena);
-}
-
-frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
- const google::protobuf::Message& msg) {
- auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
- return frc::Translation2d{units::meter_t{m->x()}, units::meter_t{m->y()}};
-}
-
-void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
- const frc::Translation2d& value) {
- auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
- m->set_x(value.X().value());
- m->set_y(value.Y().value());
-}
diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
index 90e94ae..ecfee1c 100644
--- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp
@@ -53,24 +53,3 @@
units::meter_t{json.at("y").get<double>()},
units::meter_t{json.at("z").get<double>()}};
}
-
-google::protobuf::Message* wpi::Protobuf<frc::Translation3d>::New(
- google::protobuf::Arena* arena) {
- return google::protobuf::Arena::CreateMessage<
- wpi::proto::ProtobufTranslation3d>(arena);
-}
-
-frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
- const google::protobuf::Message& msg) {
- auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
- return frc::Translation3d{units::meter_t{m->x()}, units::meter_t{m->y()},
- units::meter_t{m->z()}};
-}
-
-void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
- const frc::Translation3d& value) {
- auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
- m->set_x(value.X().value());
- m->set_y(value.Y().value());
- m->set_z(value.Z().value());
-}
diff --git a/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp
new file mode 100644
index 0000000..2b8bf5f
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/proto/Pose2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose2d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
+ arena);
+}
+
+frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
+ return frc::Pose2d{
+ wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+ wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation()),
+ };
+}
+
+void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
+ const frc::Pose2d& value) {
+ auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
+ wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+ wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp
new file mode 100644
index 0000000..581cafb
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/proto/Pose3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose3d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
+ arena);
+}
+
+frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
+ return frc::Pose3d{
+ wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+ wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation()),
+ };
+}
+
+void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
+ const frc::Pose3d& value) {
+ auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
+ wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+ wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp
new file mode 100644
index 0000000..aadefa0
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/proto/QuaternionProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Quaternion>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
+ arena);
+}
+
+frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
+ return frc::Quaternion{
+ m->w(),
+ m->x(),
+ m->y(),
+ m->z(),
+ };
+}
+
+void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
+ const frc::Quaternion& value) {
+ auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
+ m->set_w(value.W());
+ m->set_x(value.X());
+ m->set_y(value.Y());
+ m->set_z(value.Z());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp
new file mode 100644
index 0000000..c9005f2
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp
@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/proto/Rotation2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation2d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
+ arena);
+}
+
+frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
+ return frc::Rotation2d{
+ units::radian_t{m->value()},
+ };
+}
+
+void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
+ const frc::Rotation2d& value) {
+ auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
+ m->set_value(value.Radians().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp
new file mode 100644
index 0000000..72645e1
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp
@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/proto/Rotation3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation3d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
+ arena);
+}
+
+frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
+ return frc::Rotation3d{
+ wpi::UnpackProtobuf<frc::Quaternion>(m->q()),
+ };
+}
+
+void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
+ const frc::Rotation3d& value) {
+ auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
+ wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp
new file mode 100644
index 0000000..4d38b81
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/proto/Transform2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform2d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufTransform2d>(arena);
+}
+
+frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
+ return frc::Transform2d{
+ wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+ wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation()),
+ };
+}
+
+void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
+ const frc::Transform2d& value) {
+ auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
+ wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+ wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp
new file mode 100644
index 0000000..4a8fbfd
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/proto/Transform3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform3d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufTransform3d>(arena);
+}
+
+frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
+ return frc::Transform3d{
+ wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+ wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation()),
+ };
+}
+
+void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
+ const frc::Transform3d& value) {
+ auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
+ wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+ wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp
new file mode 100644
index 0000000..739dc99
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/proto/Translation2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation2d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufTranslation2d>(arena);
+}
+
+frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
+ return frc::Translation2d{
+ units::meter_t{m->x()},
+ units::meter_t{m->y()},
+ };
+}
+
+void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
+ const frc::Translation2d& value) {
+ auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
+ m->set_x(value.X().value());
+ m->set_y(value.Y().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp b/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp
new file mode 100644
index 0000000..6285b2b
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/proto/Translation3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation3d>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufTranslation3d>(arena);
+}
+
+frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
+ return frc::Translation3d{
+ units::meter_t{m->x()},
+ units::meter_t{m->y()},
+ units::meter_t{m->z()},
+ };
+}
+
+void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
+ const frc::Translation3d& value) {
+ auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
+ m->set_x(value.X().value());
+ m->set_y(value.Y().value());
+ m->set_z(value.Z().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/Twist2d.cpp b/wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp
similarity index 82%
rename from wpimath/src/main/native/cpp/geometry/Twist2d.cpp
rename to wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp
index 6c106eb..2590fc9 100644
--- a/wpimath/src/main/native/cpp/geometry/Twist2d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp
@@ -2,12 +2,10 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
-#include "frc/geometry/Twist2d.h"
+#include "frc/geometry/proto/Twist2dProto.h"
#include "geometry2d.pb.h"
-using namespace frc;
-
google::protobuf::Message* wpi::Protobuf<frc::Twist2d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist2d>(
@@ -17,8 +15,11 @@
frc::Twist2d wpi::Protobuf<frc::Twist2d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTwist2d*>(&msg);
- return frc::Twist2d{units::meter_t{m->dx()}, units::meter_t{m->dy()},
- units::radian_t{m->dtheta()}};
+ return frc::Twist2d{
+ units::meter_t{m->dx()},
+ units::meter_t{m->dy()},
+ units::radian_t{m->dtheta()},
+ };
}
void wpi::Protobuf<frc::Twist2d>::Pack(google::protobuf::Message* msg,
diff --git a/wpimath/src/main/native/cpp/geometry/Twist3d.cpp b/wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp
similarity index 77%
rename from wpimath/src/main/native/cpp/geometry/Twist3d.cpp
rename to wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp
index 4f4ce86..e2d91e4 100644
--- a/wpimath/src/main/native/cpp/geometry/Twist3d.cpp
+++ b/wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp
@@ -2,12 +2,10 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
-#include "frc/geometry/Twist3d.h"
+#include "frc/geometry/proto/Twist3dProto.h"
#include "geometry3d.pb.h"
-using namespace frc;
-
google::protobuf::Message* wpi::Protobuf<frc::Twist3d>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist3d>(
@@ -17,9 +15,11 @@
frc::Twist3d wpi::Protobuf<frc::Twist3d>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufTwist3d*>(&msg);
- return frc::Twist3d{units::meter_t{m->dx()}, units::meter_t{m->dy()},
- units::meter_t{m->dz()}, units::radian_t{m->rx()},
- units::radian_t{m->ry()}, units::radian_t{m->rz()}};
+ return frc::Twist3d{
+ units::meter_t{m->dx()}, units::meter_t{m->dy()},
+ units::meter_t{m->dz()}, units::radian_t{m->rx()},
+ units::radian_t{m->ry()}, units::radian_t{m->rz()},
+ };
}
void wpi::Protobuf<frc::Twist3d>::Pack(google::protobuf::Message* msg,
diff --git a/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp
new file mode 100644
index 0000000..b1bb9ae
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/struct/Pose2dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+ kTranslationOff + wpi::GetStructSize<frc::Translation2d>();
+} // namespace
+
+using StructType = wpi::Struct<frc::Pose2d>;
+
+frc::Pose2d StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::Pose2d{
+ wpi::UnpackStruct<frc::Translation2d, kTranslationOff>(data),
+ wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data),
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Pose2d& value) {
+ wpi::PackStruct<kTranslationOff>(data, value.Translation());
+ wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp
new file mode 100644
index 0000000..104a51b
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/struct/Pose3dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+ kTranslationOff + wpi::GetStructSize<frc::Translation3d>();
+} // namespace
+
+using StructType = wpi::Struct<frc::Pose3d>;
+
+frc::Pose3d StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::Pose3d{
+ wpi::UnpackStruct<frc::Translation3d, kTranslationOff>(data),
+ wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data),
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Pose3d& value) {
+ wpi::PackStruct<kTranslationOff>(data, value.Translation());
+ wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp
new file mode 100644
index 0000000..df5b781
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/struct/QuaternionStruct.h"
+
+namespace {
+constexpr size_t kWOff = 0;
+constexpr size_t kXOff = kWOff + 8;
+constexpr size_t kYOff = kXOff + 8;
+constexpr size_t kZOff = kYOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::Quaternion>;
+
+frc::Quaternion StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::Quaternion{
+ wpi::UnpackStruct<double, kWOff>(data),
+ wpi::UnpackStruct<double, kXOff>(data),
+ wpi::UnpackStruct<double, kYOff>(data),
+ wpi::UnpackStruct<double, kZOff>(data),
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Quaternion& value) {
+ wpi::PackStruct<kWOff>(data, value.W());
+ wpi::PackStruct<kXOff>(data, value.X());
+ wpi::PackStruct<kYOff>(data, value.Y());
+ wpi::PackStruct<kZOff>(data, value.Z());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp
new file mode 100644
index 0000000..16d3b40
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp
@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/struct/Rotation2dStruct.h"
+
+namespace {
+constexpr size_t kValueOff = 0;
+} // namespace
+
+using StructType = wpi::Struct<frc::Rotation2d>;
+
+frc::Rotation2d StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::Rotation2d{
+ units::radian_t{wpi::UnpackStruct<double, kValueOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Rotation2d& value) {
+ wpi::PackStruct<kValueOff>(data, value.Radians().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp
new file mode 100644
index 0000000..926c7c1
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp
@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/struct/Rotation3dStruct.h"
+
+namespace {
+constexpr size_t kQOff = 0;
+} // namespace
+
+using StructType = wpi::Struct<frc::Rotation3d>;
+
+frc::Rotation3d StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::Rotation3d{
+ wpi::UnpackStruct<frc::Quaternion, kQOff>(data),
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Rotation3d& value) {
+ wpi::PackStruct<kQOff>(data, value.GetQuaternion());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp
new file mode 100644
index 0000000..35fa6bf
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/struct/Transform2dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+ kTranslationOff + wpi::GetStructSize<frc::Translation2d>();
+} // namespace
+
+using StructType = wpi::Struct<frc::Transform2d>;
+
+frc::Transform2d StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::Transform2d{
+ wpi::UnpackStruct<frc::Translation2d, kTranslationOff>(data),
+ wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data),
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Transform2d& value) {
+ wpi::PackStruct<kTranslationOff>(data, value.Translation());
+ wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp
new file mode 100644
index 0000000..b428346
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/struct/Transform3dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+ kTranslationOff + wpi::GetStructSize<frc::Translation3d>();
+} // namespace
+
+using StructType = wpi::Struct<frc::Transform3d>;
+
+frc::Transform3d StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::Transform3d{
+ wpi::UnpackStruct<frc::Translation3d, kTranslationOff>(data),
+ wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data),
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Transform3d& value) {
+ wpi::PackStruct<kTranslationOff>(data, value.Translation());
+ wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp
new file mode 100644
index 0000000..edf2574
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/struct/Translation2dStruct.h"
+
+namespace {
+constexpr size_t kXOff = 0;
+constexpr size_t kYOff = kXOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::Translation2d>;
+
+frc::Translation2d StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::Translation2d{
+ units::meter_t{wpi::UnpackStruct<double, kXOff>(data)},
+ units::meter_t{wpi::UnpackStruct<double, kYOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::Translation2d& value) {
+ wpi::PackStruct<kXOff>(data, value.X().value());
+ wpi::PackStruct<kYOff>(data, value.Y().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp
new file mode 100644
index 0000000..f69306c
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp
@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/struct/Translation3dStruct.h"
+
+namespace {
+constexpr size_t kXOff = 0;
+constexpr size_t kYOff = kXOff + 8;
+constexpr size_t kZOff = kYOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::Translation3d>;
+
+frc::Translation3d StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::Translation3d{
+ units::meter_t{wpi::UnpackStruct<double, kXOff>(data)},
+ units::meter_t{wpi::UnpackStruct<double, kYOff>(data)},
+ units::meter_t{wpi::UnpackStruct<double, kZOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::Translation3d& value) {
+ wpi::PackStruct<kXOff>(data, value.X().value());
+ wpi::PackStruct<kYOff>(data, value.Y().value());
+ wpi::PackStruct<kZOff>(data, value.Z().value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp
new file mode 100644
index 0000000..5c71c62
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp
@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/struct/Twist2dStruct.h"
+
+namespace {
+constexpr size_t kDxOff = 0;
+constexpr size_t kDyOff = kDxOff + 8;
+constexpr size_t kDthetaOff = kDyOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::Twist2d>;
+
+frc::Twist2d StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::Twist2d{
+ units::meter_t{wpi::UnpackStruct<double, kDxOff>(data)},
+ units::meter_t{wpi::UnpackStruct<double, kDyOff>(data)},
+ units::radian_t{wpi::UnpackStruct<double, kDthetaOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Twist2d& value) {
+ wpi::PackStruct<kDxOff>(data, value.dx.value());
+ wpi::PackStruct<kDyOff>(data, value.dy.value());
+ wpi::PackStruct<kDthetaOff>(data, value.dtheta.value());
+}
diff --git a/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp b/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp
new file mode 100644
index 0000000..d933d4b
--- /dev/null
+++ b/wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp
@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/struct/Twist3dStruct.h"
+
+namespace {
+constexpr size_t kDxOff = 0;
+constexpr size_t kDyOff = kDxOff + 8;
+constexpr size_t kDzOff = kDyOff + 8;
+constexpr size_t kRxOff = kDzOff + 8;
+constexpr size_t kRyOff = kRxOff + 8;
+constexpr size_t kRzOff = kRyOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::Twist3d>;
+
+frc::Twist3d StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::Twist3d{
+ units::meter_t{wpi::UnpackStruct<double, kDxOff>(data)},
+ units::meter_t{wpi::UnpackStruct<double, kDyOff>(data)},
+ units::meter_t{wpi::UnpackStruct<double, kDzOff>(data)},
+ units::radian_t{wpi::UnpackStruct<double, kRxOff>(data)},
+ units::radian_t{wpi::UnpackStruct<double, kRyOff>(data)},
+ units::radian_t{wpi::UnpackStruct<double, kRzOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::Twist3d& value) {
+ wpi::PackStruct<kDxOff>(data, value.dx.value());
+ wpi::PackStruct<kDyOff>(data, value.dy.value());
+ wpi::PackStruct<kDzOff>(data, value.dz.value());
+ wpi::PackStruct<kRxOff>(data, value.rx.value());
+ wpi::PackStruct<kRyOff>(data, value.ry.value());
+ wpi::PackStruct<kRzOff>(data, value.rz.value());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
index dd30263..0f5de42 100644
--- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
+++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
@@ -16,10 +16,10 @@
units::meters_per_second_t attainableMaxSpeed) {
std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
rearLeft, rearRight};
- units::meters_per_second_t realMaxSpeed = *std::max_element(
+ units::meters_per_second_t realMaxSpeed = units::math::abs(*std::max_element(
wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
return units::math::abs(a) < units::math::abs(b);
- });
+ }));
if (realMaxSpeed > attainableMaxSpeed) {
for (int i = 0; i < 4; ++i) {
diff --git a/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp
new file mode 100644
index 0000000..097a2fb
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/proto/ChassisSpeedsProto.cpp
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/proto/ChassisSpeedsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::ChassisSpeeds>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufChassisSpeeds>(arena);
+}
+
+frc::ChassisSpeeds wpi::Protobuf<frc::ChassisSpeeds>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufChassisSpeeds*>(&msg);
+ return frc::ChassisSpeeds{
+ units::meters_per_second_t{m->vx()},
+ units::meters_per_second_t{m->vy()},
+ units::radians_per_second_t{m->omega()},
+ };
+}
+
+void wpi::Protobuf<frc::ChassisSpeeds>::Pack(google::protobuf::Message* msg,
+ const frc::ChassisSpeeds& value) {
+ auto m = static_cast<wpi::proto::ProtobufChassisSpeeds*>(msg);
+ m->set_vx(value.vx.value());
+ m->set_vy(value.vy.value());
+ m->set_omega(value.omega.value());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp
new file mode 100644
index 0000000..a79e565
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveKinematicsProto.cpp
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/proto/DifferentialDriveKinematicsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::DifferentialDriveKinematics>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufDifferentialDriveKinematics>(arena);
+}
+
+frc::DifferentialDriveKinematics
+wpi::Protobuf<frc::DifferentialDriveKinematics>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m =
+ static_cast<const wpi::proto::ProtobufDifferentialDriveKinematics*>(&msg);
+ return frc::DifferentialDriveKinematics{
+ units::meter_t{m->track_width()},
+ };
+}
+
+void wpi::Protobuf<frc::DifferentialDriveKinematics>::Pack(
+ google::protobuf::Message* msg,
+ const frc::DifferentialDriveKinematics& value) {
+ auto m = static_cast<wpi::proto::ProtobufDifferentialDriveKinematics*>(msg);
+ m->set_track_width(value.trackWidth.value());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp
new file mode 100644
index 0000000..cb38b84
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/proto/DifferentialDriveWheelSpeedsProto.cpp
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/proto/DifferentialDriveWheelSpeedsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<
+ frc::DifferentialDriveWheelSpeeds>::New(google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufDifferentialDriveWheelSpeeds>(arena);
+}
+
+frc::DifferentialDriveWheelSpeeds
+wpi::Protobuf<frc::DifferentialDriveWheelSpeeds>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufDifferentialDriveWheelSpeeds*>(
+ &msg);
+ return frc::DifferentialDriveWheelSpeeds{
+ units::meters_per_second_t{m->left()},
+ units::meters_per_second_t{m->right()},
+ };
+}
+
+void wpi::Protobuf<frc::DifferentialDriveWheelSpeeds>::Pack(
+ google::protobuf::Message* msg,
+ const frc::DifferentialDriveWheelSpeeds& value) {
+ auto m = static_cast<wpi::proto::ProtobufDifferentialDriveWheelSpeeds*>(msg);
+ m->set_left(value.left.value());
+ m->set_right(value.right.value());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp
new file mode 100644
index 0000000..e17102f
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveKinematicsProto.cpp
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/proto/MecanumDriveKinematicsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::MecanumDriveKinematics>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufMecanumDriveKinematics>(arena);
+}
+
+frc::MecanumDriveKinematics wpi::Protobuf<frc::MecanumDriveKinematics>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufMecanumDriveKinematics*>(&msg);
+ return frc::MecanumDriveKinematics{
+ wpi::UnpackProtobuf<frc::Translation2d>(m->front_left()),
+ wpi::UnpackProtobuf<frc::Translation2d>(m->front_right()),
+ wpi::UnpackProtobuf<frc::Translation2d>(m->rear_left()),
+ wpi::UnpackProtobuf<frc::Translation2d>(m->rear_right()),
+ };
+}
+
+void wpi::Protobuf<frc::MecanumDriveKinematics>::Pack(
+ google::protobuf::Message* msg, const frc::MecanumDriveKinematics& value) {
+ auto m = static_cast<wpi::proto::ProtobufMecanumDriveKinematics*>(msg);
+ wpi::PackProtobuf(m->mutable_front_left(), value.GetFrontLeft());
+ wpi::PackProtobuf(m->mutable_front_right(), value.GetFrontRight());
+ wpi::PackProtobuf(m->mutable_rear_left(), value.GetRearLeft());
+ wpi::PackProtobuf(m->mutable_rear_right(), value.GetRearRight());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp
new file mode 100644
index 0000000..94ca982
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelPositionsProto.cpp
@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/proto/MecanumDriveWheelPositionsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::MecanumDriveWheelPositions>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufMecanumDriveWheelPositions>(arena);
+}
+
+frc::MecanumDriveWheelPositions
+wpi::Protobuf<frc::MecanumDriveWheelPositions>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m =
+ static_cast<const wpi::proto::ProtobufMecanumDriveWheelPositions*>(&msg);
+ return frc::MecanumDriveWheelPositions{
+ units::meter_t{m->front_left()},
+ units::meter_t{m->front_right()},
+ units::meter_t{m->rear_left()},
+ units::meter_t{m->rear_right()},
+ };
+}
+
+void wpi::Protobuf<frc::MecanumDriveWheelPositions>::Pack(
+ google::protobuf::Message* msg,
+ const frc::MecanumDriveWheelPositions& value) {
+ auto m = static_cast<wpi::proto::ProtobufMecanumDriveWheelPositions*>(msg);
+ m->set_front_left(value.frontLeft.value());
+ m->set_front_right(value.frontRight.value());
+ m->set_rear_left(value.rearLeft.value());
+ m->set_rear_right(value.rearRight.value());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp
new file mode 100644
index 0000000..049a088
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/proto/MecanumDriveWheelSpeedsProto.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/proto/MecanumDriveWheelSpeedsProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::MecanumDriveWheelSpeeds>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufMecanumDriveWheelSpeeds>(arena);
+}
+
+frc::MecanumDriveWheelSpeeds
+wpi::Protobuf<frc::MecanumDriveWheelSpeeds>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m =
+ static_cast<const wpi::proto::ProtobufMecanumDriveWheelSpeeds*>(&msg);
+ return frc::MecanumDriveWheelSpeeds{
+ units::meters_per_second_t{m->front_left()},
+ units::meters_per_second_t{m->front_right()},
+ units::meters_per_second_t{m->rear_left()},
+ units::meters_per_second_t{m->rear_right()},
+ };
+}
+
+void wpi::Protobuf<frc::MecanumDriveWheelSpeeds>::Pack(
+ google::protobuf::Message* msg, const frc::MecanumDriveWheelSpeeds& value) {
+ auto m = static_cast<wpi::proto::ProtobufMecanumDriveWheelSpeeds*>(msg);
+ m->set_front_left(value.frontLeft.value());
+ m->set_front_right(value.frontRight.value());
+ m->set_rear_left(value.rearLeft.value());
+ m->set_rear_right(value.rearRight.value());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp
new file mode 100644
index 0000000..4e85ec8
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModulePositionProto.cpp
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/proto/SwerveModulePositionProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::SwerveModulePosition>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufSwerveModulePosition>(arena);
+}
+
+frc::SwerveModulePosition wpi::Protobuf<frc::SwerveModulePosition>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufSwerveModulePosition*>(&msg);
+ return frc::SwerveModulePosition{
+ units::meter_t{m->distance()},
+ wpi::UnpackProtobuf<frc::Rotation2d>(m->angle()),
+ };
+}
+
+void wpi::Protobuf<frc::SwerveModulePosition>::Pack(
+ google::protobuf::Message* msg, const frc::SwerveModulePosition& value) {
+ auto m = static_cast<wpi::proto::ProtobufSwerveModulePosition*>(msg);
+ m->set_distance(value.distance.value());
+ wpi::PackProtobuf(m->mutable_angle(), value.angle);
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp
new file mode 100644
index 0000000..f5d5a0f
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/proto/SwerveModuleStateProto.cpp
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/proto/SwerveModuleStateProto.h"
+
+#include "kinematics.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::SwerveModuleState>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufSwerveModuleState>(arena);
+}
+
+frc::SwerveModuleState wpi::Protobuf<frc::SwerveModuleState>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufSwerveModuleState*>(&msg);
+ return frc::SwerveModuleState{
+ units::meters_per_second_t{m->speed()},
+ wpi::UnpackProtobuf<frc::Rotation2d>(m->angle()),
+ };
+}
+
+void wpi::Protobuf<frc::SwerveModuleState>::Pack(
+ google::protobuf::Message* msg, const frc::SwerveModuleState& value) {
+ auto m = static_cast<wpi::proto::ProtobufSwerveModuleState*>(msg);
+ m->set_speed(value.speed.value());
+ wpi::PackProtobuf(m->mutable_angle(), value.angle);
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp
new file mode 100644
index 0000000..3b9ec2b
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/struct/ChassisSpeedsStruct.cpp
@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/struct/ChassisSpeedsStruct.h"
+
+namespace {
+constexpr size_t kVxOff = 0;
+constexpr size_t kVyOff = kVxOff + 8;
+constexpr size_t kOmegaOff = kVyOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::ChassisSpeeds>;
+
+frc::ChassisSpeeds StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::ChassisSpeeds{
+ units::meters_per_second_t{wpi::UnpackStruct<double, kVxOff>(data)},
+ units::meters_per_second_t{wpi::UnpackStruct<double, kVyOff>(data)},
+ units::radians_per_second_t{wpi::UnpackStruct<double, kOmegaOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::ChassisSpeeds& value) {
+ wpi::PackStruct<kVxOff>(data, value.vx.value());
+ wpi::PackStruct<kVyOff>(data, value.vy.value());
+ wpi::PackStruct<kOmegaOff>(data, value.omega.value());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp
new file mode 100644
index 0000000..b7be0d2
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveKinematicsStruct.cpp
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/struct/DifferentialDriveKinematicsStruct.h"
+
+namespace {
+constexpr size_t kTrackWidthOff = 0;
+} // namespace
+
+using StructType = wpi::Struct<frc::DifferentialDriveKinematics>;
+
+frc::DifferentialDriveKinematics StructType::Unpack(
+ std::span<const uint8_t> data) {
+ return frc::DifferentialDriveKinematics{
+ units::meter_t{wpi::UnpackStruct<double, kTrackWidthOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::DifferentialDriveKinematics& value) {
+ wpi::PackStruct<kTrackWidthOff>(data, value.trackWidth.value());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp
new file mode 100644
index 0000000..e2ae131
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/struct/DifferentialDriveWheelSpeedsStruct.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/struct/DifferentialDriveWheelSpeedsStruct.h"
+
+namespace {
+constexpr size_t kLeftOff = 0;
+constexpr size_t kRightOff = kLeftOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::DifferentialDriveWheelSpeeds>;
+
+frc::DifferentialDriveWheelSpeeds StructType::Unpack(
+ std::span<const uint8_t> data) {
+ return frc::DifferentialDriveWheelSpeeds{
+ units::meters_per_second_t{wpi::UnpackStruct<double, kLeftOff>(data)},
+ units::meters_per_second_t{wpi::UnpackStruct<double, kRightOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::DifferentialDriveWheelSpeeds& value) {
+ wpi::PackStruct<kLeftOff>(data, value.left.value());
+ wpi::PackStruct<kRightOff>(data, value.right.value());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp
new file mode 100644
index 0000000..1a27a28
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveKinematicsStruct.cpp
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/struct/MecanumDriveKinematicsStruct.h"
+
+namespace {
+constexpr size_t kFrontLeftOff = 0;
+constexpr size_t kFrontRightOff =
+ kFrontLeftOff + wpi::GetStructSize<frc::Translation2d>();
+constexpr size_t kRearLeftOff =
+ kFrontRightOff + wpi::GetStructSize<frc::Translation2d>();
+constexpr size_t kRearRightOff =
+ kRearLeftOff + wpi::GetStructSize<frc::Translation2d>();
+} // namespace
+
+using StructType = wpi::Struct<frc::MecanumDriveKinematics>;
+
+frc::MecanumDriveKinematics StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::MecanumDriveKinematics{
+ wpi::UnpackStruct<frc::Translation2d, kFrontLeftOff>(data),
+ wpi::UnpackStruct<frc::Translation2d, kFrontRightOff>(data),
+ wpi::UnpackStruct<frc::Translation2d, kRearLeftOff>(data),
+ wpi::UnpackStruct<frc::Translation2d, kRearRightOff>(data),
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::MecanumDriveKinematics& value) {
+ wpi::PackStruct<kFrontLeftOff>(data, value.GetFrontLeft());
+ wpi::PackStruct<kFrontRightOff>(data, value.GetFrontRight());
+ wpi::PackStruct<kRearLeftOff>(data, value.GetRearLeft());
+ wpi::PackStruct<kRearRightOff>(data, value.GetRearRight());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp
new file mode 100644
index 0000000..c857f4d
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelPositionsStruct.cpp
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/struct/MecanumDriveWheelPositionsStruct.h"
+
+namespace {
+constexpr size_t kFrontLeftOff = 0;
+constexpr size_t kFrontRightOff = kFrontLeftOff + 8;
+constexpr size_t kRearLeftOff = kFrontRightOff + 8;
+constexpr size_t kRearRightOff = kRearLeftOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::MecanumDriveWheelPositions>;
+
+frc::MecanumDriveWheelPositions StructType::Unpack(
+ std::span<const uint8_t> data) {
+ return frc::MecanumDriveWheelPositions{
+ units::meter_t{wpi::UnpackStruct<double, kFrontLeftOff>(data)},
+ units::meter_t{wpi::UnpackStruct<double, kFrontRightOff>(data)},
+ units::meter_t{wpi::UnpackStruct<double, kRearLeftOff>(data)},
+ units::meter_t{wpi::UnpackStruct<double, kRearRightOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::MecanumDriveWheelPositions& value) {
+ wpi::PackStruct<kFrontLeftOff>(data, value.frontLeft.value());
+ wpi::PackStruct<kFrontRightOff>(data, value.frontRight.value());
+ wpi::PackStruct<kRearLeftOff>(data, value.rearLeft.value());
+ wpi::PackStruct<kRearRightOff>(data, value.rearRight.value());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp
new file mode 100644
index 0000000..c232cc9
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/struct/MecanumDriveWheelSpeedsStruct.cpp
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/struct/MecanumDriveWheelSpeedsStruct.h"
+
+namespace {
+constexpr size_t kFrontLeftOff = 0;
+constexpr size_t kFrontRightOff = kFrontLeftOff + 8;
+constexpr size_t kRearLeftOff = kFrontRightOff + 8;
+constexpr size_t kRearRightOff = kRearLeftOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::MecanumDriveWheelSpeeds>;
+
+frc::MecanumDriveWheelSpeeds StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::MecanumDriveWheelSpeeds{
+ units::meters_per_second_t{
+ wpi::UnpackStruct<double, kFrontLeftOff>(data)},
+ units::meters_per_second_t{
+ wpi::UnpackStruct<double, kFrontRightOff>(data)},
+ units::meters_per_second_t{wpi::UnpackStruct<double, kRearLeftOff>(data)},
+ units::meters_per_second_t{
+ wpi::UnpackStruct<double, kRearRightOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::MecanumDriveWheelSpeeds& value) {
+ wpi::PackStruct<kFrontLeftOff>(data, value.frontLeft.value());
+ wpi::PackStruct<kFrontRightOff>(data, value.frontRight.value());
+ wpi::PackStruct<kRearLeftOff>(data, value.rearLeft.value());
+ wpi::PackStruct<kRearRightOff>(data, value.rearRight.value());
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp
new file mode 100644
index 0000000..12ae0a1
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModulePositionStruct.cpp
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/struct/SwerveModulePositionStruct.h"
+
+namespace {
+constexpr size_t kDistanceOff = 0;
+constexpr size_t kAngleOff = kDistanceOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::SwerveModulePosition>;
+
+frc::SwerveModulePosition StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::SwerveModulePosition{
+ units::meter_t{wpi::UnpackStruct<double, kDistanceOff>(data)},
+ wpi::UnpackStruct<frc::Rotation2d, kAngleOff>(data),
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::SwerveModulePosition& value) {
+ wpi::PackStruct<kDistanceOff>(data, value.distance.value());
+ wpi::PackStruct<kAngleOff>(data, value.angle);
+}
diff --git a/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp
new file mode 100644
index 0000000..b045178
--- /dev/null
+++ b/wpimath/src/main/native/cpp/kinematics/struct/SwerveModuleStateStruct.cpp
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/kinematics/struct/SwerveModuleStateStruct.h"
+
+namespace {
+constexpr size_t kSpeedOff = 0;
+constexpr size_t kAngleOff = kSpeedOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::SwerveModuleState>;
+
+frc::SwerveModuleState StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::SwerveModuleState{
+ units::meters_per_second_t{wpi::UnpackStruct<double, kSpeedOff>(data)},
+ wpi::UnpackStruct<frc::Rotation2d, kAngleOff>(data),
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data,
+ const frc::SwerveModuleState& value) {
+ wpi::PackStruct<kSpeedOff>(data, value.speed.value());
+ wpi::PackStruct<kAngleOff>(data, value.angle);
+}
diff --git a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
index b643849..3885443 100644
--- a/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
+++ b/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
@@ -10,7 +10,9 @@
wpi::array<double, 2> xInitialControlVector,
wpi::array<double, 2> xFinalControlVector,
wpi::array<double, 2> yInitialControlVector,
- wpi::array<double, 2> yFinalControlVector) {
+ wpi::array<double, 2> yFinalControlVector)
+ : m_initialControlVector{xInitialControlVector, yInitialControlVector},
+ m_finalControlVector{xFinalControlVector, yFinalControlVector} {
const auto hermite = MakeHermiteBasis();
const auto x =
ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
diff --git a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
index 5362b7c..65d7986 100644
--- a/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
+++ b/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
@@ -10,7 +10,9 @@
wpi::array<double, 3> xInitialControlVector,
wpi::array<double, 3> xFinalControlVector,
wpi::array<double, 3> yInitialControlVector,
- wpi::array<double, 3> yFinalControlVector) {
+ wpi::array<double, 3> yFinalControlVector)
+ : m_initialControlVector{xInitialControlVector, yInitialControlVector},
+ m_finalControlVector{xFinalControlVector, yFinalControlVector} {
const auto hermite = MakeHermiteBasis();
const auto x =
ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
diff --git a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
index e8bbb46..c78a54f 100644
--- a/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
@@ -169,6 +169,81 @@
return splines;
}
+std::vector<QuinticHermiteSpline> SplineHelper::OptimizeCurvature(
+ const std::vector<QuinticHermiteSpline>& splines) {
+ // If there's only one spline in the vector, we can't optimize anything so
+ // just return that.
+ if (splines.size() < 2) {
+ return splines;
+ }
+
+ // Implements Section 4.1.2 of
+ // http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf.
+
+ // Cubic splines minimize the integral of the second derivative's absolute
+ // value. Therefore, we can create cubic splines with the same 0th and 1st
+ // derivatives and the provided quintic splines, find the second derivative of
+ // those cubic splines and then use a weighted average for the second
+ // derivatives of the quintic splines.
+
+ std::vector<QuinticHermiteSpline> optimizedSplines;
+ optimizedSplines.reserve(splines.size());
+ optimizedSplines.push_back(splines[0]);
+
+ for (size_t i = 0; i < splines.size() - 1; ++i) {
+ const auto& a = splines[i];
+ const auto& b = splines[i + 1];
+
+ // Get the control vectors that created the quintic splines above.
+ const auto& aInitial = a.GetInitialControlVector();
+ const auto& aFinal = a.GetFinalControlVector();
+ const auto& bInitial = b.GetInitialControlVector();
+ const auto& bFinal = b.GetFinalControlVector();
+
+ // Create cubic splines with the same control vectors.
+ auto Trim = [](const wpi::array<double, 3>& a) {
+ return wpi::array<double, 2>{a[0], a[1]};
+ };
+ CubicHermiteSpline ca{Trim(aInitial.x), Trim(aFinal.x), Trim(aInitial.y),
+ Trim(aFinal.y)};
+ CubicHermiteSpline cb{Trim(bInitial.x), Trim(bFinal.x), Trim(bInitial.y),
+ Trim(bFinal.y)};
+
+ // Calculate the second derivatives at the knot points.
+ frc::Vectord<4> bases{1.0, 1.0, 1.0, 1.0};
+ frc::Vectord<6> combinedA = ca.Coefficients() * bases;
+
+ double ddxA = combinedA(4);
+ double ddyA = combinedA(5);
+ double ddxB = cb.Coefficients()(4, 1);
+ double ddyB = cb.Coefficients()(5, 1);
+
+ // Calculate the parameters for weighted average.
+ double dAB =
+ std::hypot(aFinal.x[0] - aInitial.x[0], aFinal.y[0] - aInitial.y[0]);
+ double dBC =
+ std::hypot(bFinal.x[0] - bInitial.x[0], bFinal.y[0] - bInitial.y[0]);
+ double alpha = dBC / (dAB + dBC);
+ double beta = dAB / (dAB + dBC);
+
+ // Calculate the weighted average.
+ double ddx = alpha * ddxA + beta * ddxB;
+ double ddy = alpha * ddyA + beta * ddyB;
+
+ // Create new splines.
+ optimizedSplines[i] = {aInitial.x,
+ {aFinal.x[0], aFinal.x[1], ddx},
+ aInitial.y,
+ {aFinal.y[0], aFinal.y[1], ddy}};
+ optimizedSplines.push_back({{bInitial.x[0], bInitial.x[1], ddx},
+ bFinal.x,
+ {bInitial.y[0], bInitial.y[1], ddy},
+ bFinal.y});
+ }
+
+ return optimizedSplines;
+}
+
void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
const std::vector<double>& b,
const std::vector<double>& c,
diff --git a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
index 3527092..11c65ea 100644
--- a/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
+++ b/wpimath/src/main/native/cpp/system/plant/LinearSystemId.cpp
@@ -9,23 +9,24 @@
LinearSystem<2, 1, 1> LinearSystemId::ElevatorSystem(DCMotor motor,
units::kilogram_t mass,
units::meter_t radius,
- double G) {
+ double gearing) {
if (mass <= 0_kg) {
throw std::domain_error("mass must be greater than zero.");
}
if (radius <= 0_m) {
throw std::domain_error("radius must be greater than zero.");
}
- if (G <= 0.0) {
- throw std::domain_error("G must be greater than zero.");
+ if (gearing <= 0.0) {
+ throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
- {0.0, (-std::pow(G, 2) * motor.Kt /
+ {0.0, (-std::pow(gearing, 2) * motor.Kt /
(motor.R * units::math::pow<2>(radius) * mass * motor.Kv))
.value()}};
- Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * radius * mass)).value()};
+ Matrixd<2, 1> B{0.0,
+ (gearing * motor.Kt / (motor.R * radius * mass)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
@@ -33,18 +34,19 @@
}
LinearSystem<2, 1, 1> LinearSystemId::SingleJointedArmSystem(
- DCMotor motor, units::kilogram_square_meter_t J, double G) {
+ DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
- if (G <= 0.0) {
- throw std::domain_error("G must be greater than zero.");
+ if (gearing <= 0.0) {
+ throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
- {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
- Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
+ {0.0,
+ (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
+ Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 2> C{1.0, 0.0};
Matrixd<1, 1> D{0.0};
@@ -119,17 +121,17 @@
}
LinearSystem<1, 1, 1> LinearSystemId::FlywheelSystem(
- DCMotor motor, units::kilogram_square_meter_t J, double G) {
+ DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
- if (G <= 0.0) {
- throw std::domain_error("G must be greater than zero.");
+ if (gearing <= 0.0) {
+ throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<1, 1> A{
- (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
- Matrixd<1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
+ (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
+ Matrixd<1, 1> B{(gearing * motor.Kt / (motor.R * J)).value()};
Matrixd<1, 1> C{1.0};
Matrixd<1, 1> D{0.0};
@@ -137,18 +139,19 @@
}
LinearSystem<2, 1, 2> LinearSystemId::DCMotorSystem(
- DCMotor motor, units::kilogram_square_meter_t J, double G) {
+ DCMotor motor, units::kilogram_square_meter_t J, double gearing) {
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
- if (G <= 0.0) {
- throw std::domain_error("G must be greater than zero.");
+ if (gearing <= 0.0) {
+ throw std::domain_error("gearing must be greater than zero.");
}
Matrixd<2, 2> A{
{0.0, 1.0},
- {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
- Matrixd<2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
+ {0.0,
+ (-std::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
+ Matrixd<2, 1> B{0.0, (gearing * motor.Kt / (motor.R * J)).value()};
Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
Matrixd<2, 1> D{0.0, 0.0};
@@ -157,7 +160,7 @@
LinearSystem<2, 2, 2> LinearSystemId::DrivetrainVelocitySystem(
const DCMotor& motor, units::kilogram_t mass, units::meter_t r,
- units::meter_t rb, units::kilogram_square_meter_t J, double G) {
+ units::meter_t rb, units::kilogram_square_meter_t J, double gearing) {
if (mass <= 0_kg) {
throw std::domain_error("mass must be greater than zero.");
}
@@ -170,13 +173,13 @@
if (J <= 0_kg_sq_m) {
throw std::domain_error("J must be greater than zero.");
}
- if (G <= 0.0) {
- throw std::domain_error("G must be greater than zero.");
+ if (gearing <= 0.0) {
+ throw std::domain_error("gearing must be greater than zero.");
}
- auto C1 = -std::pow(G, 2) * motor.Kt /
+ auto C1 = -std::pow(gearing, 2) * motor.Kt /
(motor.Kv * motor.R * units::math::pow<2>(r));
- auto C2 = G * motor.Kt / (motor.R * r);
+ auto C2 = gearing * motor.Kt / (motor.R * r);
Matrixd<2, 2> A{{((1 / mass + units::math::pow<2>(rb) / J) * C1).value(),
((1 / mass - units::math::pow<2>(rb) / J) * C1).value()},
diff --git a/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp b/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp
new file mode 100644
index 0000000..8363270
--- /dev/null
+++ b/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/system/plant/proto/DCMotorProto.h"
+
+#include "plant.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::DCMotor>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufDCMotor>(
+ arena);
+}
+
+frc::DCMotor wpi::Protobuf<frc::DCMotor>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufDCMotor*>(&msg);
+ return frc::DCMotor{
+ units::volt_t{m->nominal_voltage()},
+ units::newton_meter_t{m->stall_torque()},
+ units::ampere_t{m->stall_current()},
+ units::ampere_t{m->free_current()},
+ units::radians_per_second_t{m->free_speed()},
+ };
+}
+
+void wpi::Protobuf<frc::DCMotor>::Pack(google::protobuf::Message* msg,
+ const frc::DCMotor& value) {
+ auto m = static_cast<wpi::proto::ProtobufDCMotor*>(msg);
+ m->set_nominal_voltage(value.nominalVoltage.value());
+ m->set_stall_torque(value.stallTorque.value());
+ m->set_stall_current(value.stallCurrent.value());
+ m->set_free_current(value.freeCurrent.value());
+ m->set_free_speed(value.freeSpeed.value());
+}
diff --git a/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp b/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp
new file mode 100644
index 0000000..e7389a1
--- /dev/null
+++ b/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/system/plant/struct/DCMotorStruct.h"
+
+namespace {
+constexpr size_t kNominalVoltageOff = 0;
+constexpr size_t kStallTorqueOff = kNominalVoltageOff + 8;
+constexpr size_t kStallCurrentOff = kStallTorqueOff + 8;
+constexpr size_t kFreeCurrentOff = kStallCurrentOff + 8;
+constexpr size_t kFreeSpeedOff = kFreeCurrentOff + 8;
+} // namespace
+
+using StructType = wpi::Struct<frc::DCMotor>;
+
+frc::DCMotor StructType::Unpack(std::span<const uint8_t> data) {
+ return frc::DCMotor{
+ units::volt_t{wpi::UnpackStruct<double, kNominalVoltageOff>(data)},
+ units::newton_meter_t{wpi::UnpackStruct<double, kStallTorqueOff>(data)},
+ units::ampere_t{wpi::UnpackStruct<double, kStallCurrentOff>(data)},
+ units::ampere_t{wpi::UnpackStruct<double, kFreeCurrentOff>(data)},
+ units::radians_per_second_t{
+ wpi::UnpackStruct<double, kFreeSpeedOff>(data)},
+ };
+}
+
+void StructType::Pack(std::span<uint8_t> data, const frc::DCMotor& value) {
+ wpi::PackStruct<kNominalVoltageOff>(data, value.nominalVoltage.value());
+ wpi::PackStruct<kStallTorqueOff>(data, value.stallTorque.value());
+ wpi::PackStruct<kStallCurrentOff>(data, value.stallCurrent.value());
+ wpi::PackStruct<kFreeCurrentOff>(data, value.freeCurrent.value());
+ wpi::PackStruct<kFreeSpeedOff>(data, value.freeSpeed.value());
+}
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
index c7a7e9a..922ace3 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -121,8 +121,8 @@
std::vector<SplineParameterizer::PoseWithCurvature> points;
try {
- points = SplinePointsFromSplines(
- SplineHelper::QuinticSplinesFromWaypoints(newWaypoints));
+ points = SplinePointsFromSplines(SplineHelper::OptimizeCurvature(
+ SplineHelper::QuinticSplinesFromWaypoints(newWaypoints)));
} catch (SplineParameterizer::MalformedSplineException& e) {
ReportError(e.what());
return kDoNothingTrajectory;
diff --git a/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
index da9c955..d9cb853 100644
--- a/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
+++ b/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
@@ -35,7 +35,7 @@
throw std::runtime_error(fmt::format("Cannot open file: {}", path));
}
- wpi::json json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
+ wpi::json json = wpi::json::parse(fileBuffer->GetCharBuffer());
return Trajectory{json.get<std::vector<Trajectory::State>>()};
}
diff --git a/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp
new file mode 100644
index 0000000..d5bbe2b
--- /dev/null
+++ b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryProto.cpp
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/trajectory/proto/TrajectoryProto.h"
+
+#include "trajectory.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Trajectory>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTrajectory>(
+ arena);
+}
+
+frc::Trajectory wpi::Protobuf<frc::Trajectory>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTrajectory*>(&msg);
+ std::vector<frc::Trajectory::State> states;
+ states.reserve(m->states().size());
+ for (const auto& protoState : m->states()) {
+ states.push_back(wpi::UnpackProtobuf<frc::Trajectory::State>(protoState));
+ }
+ return frc::Trajectory{states};
+}
+
+void wpi::Protobuf<frc::Trajectory>::Pack(google::protobuf::Message* msg,
+ const frc::Trajectory& value) {
+ auto m = static_cast<wpi::proto::ProtobufTrajectory*>(msg);
+ m->mutable_states()->Reserve(value.States().size());
+ for (const auto& state : value.States()) {
+ wpi::PackProtobuf(m->add_states(), state);
+ }
+}
diff --git a/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp
new file mode 100644
index 0000000..f2b80ab
--- /dev/null
+++ b/wpimath/src/main/native/cpp/trajectory/proto/TrajectoryStateProto.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/trajectory/proto/TrajectoryStateProto.h"
+
+#include "trajectory.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Trajectory::State>::New(
+ google::protobuf::Arena* arena) {
+ return google::protobuf::Arena::CreateMessage<
+ wpi::proto::ProtobufTrajectoryState>(arena);
+}
+
+frc::Trajectory::State wpi::Protobuf<frc::Trajectory::State>::Unpack(
+ const google::protobuf::Message& msg) {
+ auto m = static_cast<const wpi::proto::ProtobufTrajectoryState*>(&msg);
+ return frc::Trajectory::State{
+ units::second_t{m->time()},
+ units::meters_per_second_t{m->velocity()},
+ units::meters_per_second_squared_t{m->acceleration()},
+ wpi::UnpackProtobuf<frc::Pose2d>(m->pose()),
+ units::curvature_t{m->curvature()},
+ };
+}
+
+void wpi::Protobuf<frc::Trajectory::State>::Pack(
+ google::protobuf::Message* msg, const frc::Trajectory::State& value) {
+ auto m = static_cast<wpi::proto::ProtobufTrajectoryState*>(msg);
+ m->set_time(value.t.value());
+ m->set_velocity(value.velocity.value());
+ m->set_acceleration(value.acceleration.value());
+ wpi::PackProtobuf(m->mutable_pose(), value.pose);
+ m->set_curvature(value.curvature.value());
+}