mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-25 01:41:43 +00:00
SCRIPT namespace replacements
This commit is contained in:
committed by
Peter Johnson
parent
ae6c043632
commit
9aca8e0fd6
@@ -13,18 +13,18 @@
|
||||
#include "wpi/math/linalg/EigenCore.hpp"
|
||||
#include "wpi/math/system/NumericalIntegration.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
units::volt_t ArmFeedforward::Calculate(
|
||||
units::unit_t<Angle> currentAngle, units::unit_t<Velocity> currentVelocity,
|
||||
units::unit_t<Velocity> nextVelocity) const {
|
||||
wpi::units::volt_t ArmFeedforward::Calculate(
|
||||
wpi::units::unit_t<Angle> currentAngle, wpi::units::unit_t<Velocity> currentVelocity,
|
||||
wpi::units::unit_t<Velocity> nextVelocity) const {
|
||||
using VarMat = slp::VariableMatrix;
|
||||
|
||||
// Small kₐ values make the solver ill-conditioned
|
||||
if (kA < units::unit_t<ka_unit>{1e-1}) {
|
||||
if (kA < wpi::units::unit_t<ka_unit>{1e-1}) {
|
||||
auto acceleration = (nextVelocity - currentVelocity) / m_dt;
|
||||
return kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
|
||||
kA * acceleration + kG * units::math::cos(currentAngle);
|
||||
return kS * wpi::util::sgn(currentVelocity.value()) + kV * currentVelocity +
|
||||
kA * acceleration + kG * wpi::units::math::cos(currentAngle);
|
||||
}
|
||||
|
||||
// Arm dynamics
|
||||
@@ -43,8 +43,8 @@ units::volt_t ArmFeedforward::Calculate(
|
||||
|
||||
// Initial guess
|
||||
auto acceleration = (nextVelocity - currentVelocity) / m_dt;
|
||||
u_k.set_value((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
|
||||
kA * acceleration + kG * units::math::cos(currentAngle))
|
||||
u_k.set_value((kS * wpi::util::sgn(currentVelocity.value()) + kV * currentVelocity +
|
||||
kA * acceleration + kG * wpi::units::math::cos(currentAngle))
|
||||
.value());
|
||||
|
||||
auto r_k1 = RK4<decltype(f), VarMat, VarMat>(f, r_k, u_k, m_dt);
|
||||
@@ -107,5 +107,5 @@ units::volt_t ArmFeedforward::Calculate(
|
||||
}
|
||||
}
|
||||
|
||||
return units::volt_t{u_k.value()};
|
||||
return wpi::units::volt_t{u_k.value()};
|
||||
}
|
||||
|
||||
@@ -6,9 +6,9 @@
|
||||
|
||||
#include "wpi/util/sendable/SendableBuilder.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
void BangBangController::InitSendable(wpi::SendableBuilder& builder) {
|
||||
void BangBangController::InitSendable(wpi::util::SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("BangBangController");
|
||||
builder.AddDoubleProperty(
|
||||
"tolerance", [this] { return GetTolerance(); },
|
||||
|
||||
@@ -6,12 +6,12 @@
|
||||
|
||||
#include <Eigen/QR>
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate(
|
||||
units::meters_per_second_t leftVelocity,
|
||||
units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
|
||||
units::volt_t rightVoltage) {
|
||||
wpi::units::meters_per_second_t leftVelocity,
|
||||
wpi::units::meters_per_second_t rightVelocity, wpi::units::volt_t leftVoltage,
|
||||
wpi::units::volt_t rightVoltage) {
|
||||
Vectord<2> u{leftVoltage.value(), rightVoltage.value()};
|
||||
|
||||
// Find unconstrained wheel accelerations
|
||||
@@ -56,5 +56,5 @@ DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate(
|
||||
// u = B⁻¹(dx/dt - Ax)
|
||||
u = m_system.B().householderQr().solve(dxdt - m_system.A() * x);
|
||||
|
||||
return {units::volt_t{u(0)}, units::volt_t{u(1)}};
|
||||
return {wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)}};
|
||||
}
|
||||
|
||||
@@ -8,17 +8,17 @@
|
||||
|
||||
#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate(
|
||||
units::meters_per_second_t currentLeftVelocity,
|
||||
units::meters_per_second_t nextLeftVelocity,
|
||||
units::meters_per_second_t currentRightVelocity,
|
||||
units::meters_per_second_t nextRightVelocity, units::second_t dt) {
|
||||
frc::LinearPlantInversionFeedforward<2, 2> feedforward{m_plant, dt};
|
||||
wpi::units::meters_per_second_t currentLeftVelocity,
|
||||
wpi::units::meters_per_second_t nextLeftVelocity,
|
||||
wpi::units::meters_per_second_t currentRightVelocity,
|
||||
wpi::units::meters_per_second_t nextRightVelocity, wpi::units::second_t dt) {
|
||||
wpi::math::LinearPlantInversionFeedforward<2, 2> feedforward{m_plant, dt};
|
||||
|
||||
Eigen::Vector2d r{currentLeftVelocity, currentRightVelocity};
|
||||
Eigen::Vector2d nextR{nextLeftVelocity, nextRightVelocity};
|
||||
auto u = feedforward.Calculate(r, nextR);
|
||||
return {units::volt_t{u(0)}, units::volt_t{u(1)}};
|
||||
return {wpi::units::volt_t{u(0)}, wpi::units::volt_t{u(1)}};
|
||||
}
|
||||
|
||||
@@ -10,13 +10,13 @@
|
||||
#include "wpi/math/system/Discretization.hpp"
|
||||
#include "wpi/math/util/MathUtil.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
|
||||
const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
|
||||
units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
|
||||
units::meters_per_second_t leftVelocityRef,
|
||||
units::meters_per_second_t rightVelocityRef) {
|
||||
const Pose2d& currentPose, wpi::units::meters_per_second_t leftVelocity,
|
||||
wpi::units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
|
||||
wpi::units::meters_per_second_t leftVelocityRef,
|
||||
wpi::units::meters_per_second_t rightVelocityRef) {
|
||||
// This implements the linear time-varying differential drive controller in
|
||||
// theorem 8.7.4 of https://controls-in-frc.link/
|
||||
//
|
||||
@@ -26,11 +26,11 @@ DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
|
||||
// [vₗ]
|
||||
// [vᵣ]
|
||||
|
||||
units::meters_per_second_t velocity{(leftVelocity + rightVelocity) / 2.0};
|
||||
wpi::units::meters_per_second_t velocity{(leftVelocity + rightVelocity) / 2.0};
|
||||
|
||||
// The DARE is ill-conditioned if the velocity is close to zero, so don't
|
||||
// let the system stop.
|
||||
if (units::math::abs(velocity) < 1e-4_mps) {
|
||||
if (wpi::units::math::abs(velocity) < 1e-4_mps) {
|
||||
velocity = 1e-4_mps;
|
||||
}
|
||||
|
||||
@@ -42,7 +42,7 @@ DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
|
||||
leftVelocity.value(), rightVelocity.value()};
|
||||
|
||||
m_error = r - x;
|
||||
m_error(2) = frc::AngleModulus(units::radian_t{m_error(2)}).value();
|
||||
m_error(2) = wpi::math::AngleModulus(wpi::units::radian_t{m_error(2)}).value();
|
||||
|
||||
Eigen::Matrix<double, 5, 5> A{
|
||||
{0.0, 0.0, 0.0, 0.5, 0.5},
|
||||
@@ -76,6 +76,6 @@ DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate(
|
||||
|
||||
Eigen::Vector2d u = K * inRobotFrame * m_error;
|
||||
|
||||
return DifferentialDriveWheelVoltages{units::volt_t{u(0)},
|
||||
units::volt_t{u(1)}};
|
||||
return DifferentialDriveWheelVoltages{wpi::units::volt_t{u(0)},
|
||||
wpi::units::volt_t{u(1)}};
|
||||
}
|
||||
|
||||
@@ -8,12 +8,12 @@
|
||||
#include "wpi/math/system/Discretization.hpp"
|
||||
#include "wpi/units/math.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
ChassisSpeeds LTVUnicycleController::Calculate(
|
||||
const Pose2d& currentPose, const Pose2d& poseRef,
|
||||
units::meters_per_second_t linearVelocityRef,
|
||||
units::radians_per_second_t angularVelocityRef) {
|
||||
wpi::units::meters_per_second_t linearVelocityRef,
|
||||
wpi::units::radians_per_second_t angularVelocityRef) {
|
||||
// The change in global pose for a unicycle is defined by the following three
|
||||
// equations.
|
||||
//
|
||||
@@ -53,7 +53,7 @@ ChassisSpeeds LTVUnicycleController::Calculate(
|
||||
|
||||
// The DARE is ill-conditioned if the velocity is close to zero, so don't
|
||||
// let the system stop.
|
||||
if (units::math::abs(linearVelocityRef) < 1e-4_mps) {
|
||||
if (wpi::units::math::abs(linearVelocityRef) < 1e-4_mps) {
|
||||
linearVelocityRef = 1e-4_mps;
|
||||
}
|
||||
|
||||
@@ -78,7 +78,7 @@ ChassisSpeeds LTVUnicycleController::Calculate(
|
||||
m_poseError.Rotation().Radians().value()};
|
||||
Eigen::Vector2d u = K * e;
|
||||
|
||||
return ChassisSpeeds{linearVelocityRef + units::meters_per_second_t{u(0)},
|
||||
return ChassisSpeeds{linearVelocityRef + wpi::units::meters_per_second_t{u(0)},
|
||||
0_mps,
|
||||
angularVelocityRef + units::radians_per_second_t{u(1)}};
|
||||
angularVelocityRef + wpi::units::radians_per_second_t{u(1)}};
|
||||
}
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
#include "wpi/math/controller/LinearQuadraticRegulator.hpp"
|
||||
|
||||
namespace frc {
|
||||
namespace wpi::math {
|
||||
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
LinearQuadraticRegulator<1, 1>;
|
||||
@@ -13,4 +13,4 @@ template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
LinearQuadraticRegulator<2, 2>;
|
||||
|
||||
} // namespace frc
|
||||
} // namespace wpi::math
|
||||
|
||||
@@ -6,9 +6,9 @@
|
||||
|
||||
#include "wpi/util/sendable/SendableBuilder.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
void PIDController::InitSendable(wpi::SendableBuilder& builder) {
|
||||
void PIDController::InitSendable(wpi::util::SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("PIDController");
|
||||
builder.AddDoubleProperty(
|
||||
"p", [this] { return GetP(); }, [this](double value) { SetP(value); });
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
#include "wpi/math/controller/ProfiledPIDController.hpp"
|
||||
|
||||
int frc::detail::IncrementAndGetProfiledPIDControllerInstances() {
|
||||
int wpi::math::detail::IncrementAndGetProfiledPIDControllerInstances() {
|
||||
static int instances = 0;
|
||||
return ++instances;
|
||||
}
|
||||
|
||||
@@ -8,23 +8,23 @@
|
||||
|
||||
#include "wpimath/protobuf/controller.npb.h"
|
||||
|
||||
std::optional<frc::ArmFeedforward> wpi::Protobuf<frc::ArmFeedforward>::Unpack(
|
||||
std::optional<wpi::math::ArmFeedforward> wpi::util::Protobuf<wpi::math::ArmFeedforward>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi_proto_ProtobufArmFeedforward msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::ArmFeedforward{
|
||||
units::volt_t{msg.ks},
|
||||
units::volt_t{msg.kg},
|
||||
units::unit_t<frc::ArmFeedforward::kv_unit>{msg.kv},
|
||||
units::unit_t<frc::ArmFeedforward::ka_unit>{msg.ka},
|
||||
return wpi::math::ArmFeedforward{
|
||||
wpi::units::volt_t{msg.ks},
|
||||
wpi::units::volt_t{msg.kg},
|
||||
wpi::units::unit_t<wpi::math::ArmFeedforward::kv_unit>{msg.kv},
|
||||
wpi::units::unit_t<wpi::math::ArmFeedforward::ka_unit>{msg.ka},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::ArmFeedforward>::Pack(
|
||||
OutputStream& stream, const frc::ArmFeedforward& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::ArmFeedforward>::Pack(
|
||||
OutputStream& stream, const wpi::math::ArmFeedforward& value) {
|
||||
wpi_proto_ProtobufArmFeedforward msg{
|
||||
.ks = value.GetKs().value(),
|
||||
.kg = value.GetKg().value(),
|
||||
|
||||
@@ -6,14 +6,14 @@
|
||||
|
||||
#include "wpimath/protobuf/controller.npb.h"
|
||||
|
||||
std::optional<frc::DifferentialDriveFeedforward>
|
||||
wpi::Protobuf<frc::DifferentialDriveFeedforward>::Unpack(InputStream& stream) {
|
||||
std::optional<wpi::math::DifferentialDriveFeedforward>
|
||||
wpi::util::Protobuf<wpi::math::DifferentialDriveFeedforward>::Unpack(InputStream& stream) {
|
||||
wpi_proto_ProtobufDifferentialDriveFeedforward msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::DifferentialDriveFeedforward{
|
||||
return wpi::math::DifferentialDriveFeedforward{
|
||||
decltype(1_V / 1_mps){msg.kv_linear},
|
||||
decltype(1_V / 1_mps_sq){msg.ka_linear},
|
||||
decltype(1_V / 1_mps){msg.kv_angular},
|
||||
@@ -21,8 +21,8 @@ wpi::Protobuf<frc::DifferentialDriveFeedforward>::Unpack(InputStream& stream) {
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::DifferentialDriveFeedforward>::Pack(
|
||||
OutputStream& stream, const frc::DifferentialDriveFeedforward& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::DifferentialDriveFeedforward>::Pack(
|
||||
OutputStream& stream, const wpi::math::DifferentialDriveFeedforward& value) {
|
||||
wpi_proto_ProtobufDifferentialDriveFeedforward msg{
|
||||
.kv_linear = value.m_kVLinear.value(),
|
||||
.ka_linear = value.m_kALinear.value(),
|
||||
|
||||
@@ -8,21 +8,21 @@
|
||||
|
||||
#include "wpimath/protobuf/controller.npb.h"
|
||||
|
||||
std::optional<frc::DifferentialDriveWheelVoltages> wpi::Protobuf<
|
||||
frc::DifferentialDriveWheelVoltages>::Unpack(InputStream& stream) {
|
||||
std::optional<wpi::math::DifferentialDriveWheelVoltages> wpi::util::Protobuf<
|
||||
wpi::math::DifferentialDriveWheelVoltages>::Unpack(InputStream& stream) {
|
||||
wpi_proto_ProtobufDifferentialDriveWheelVoltages msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::DifferentialDriveWheelVoltages{
|
||||
units::volt_t{msg.left},
|
||||
units::volt_t{msg.right},
|
||||
return wpi::math::DifferentialDriveWheelVoltages{
|
||||
wpi::units::volt_t{msg.left},
|
||||
wpi::units::volt_t{msg.right},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::DifferentialDriveWheelVoltages>::Pack(
|
||||
OutputStream& stream, const frc::DifferentialDriveWheelVoltages& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::DifferentialDriveWheelVoltages>::Pack(
|
||||
OutputStream& stream, const wpi::math::DifferentialDriveWheelVoltages& value) {
|
||||
wpi_proto_ProtobufDifferentialDriveWheelVoltages msg{
|
||||
.left = value.left.value(),
|
||||
.right = value.right.value(),
|
||||
|
||||
@@ -8,23 +8,23 @@
|
||||
|
||||
#include "wpimath/protobuf/controller.npb.h"
|
||||
|
||||
std::optional<frc::ElevatorFeedforward>
|
||||
wpi::Protobuf<frc::ElevatorFeedforward>::Unpack(InputStream& stream) {
|
||||
std::optional<wpi::math::ElevatorFeedforward>
|
||||
wpi::util::Protobuf<wpi::math::ElevatorFeedforward>::Unpack(InputStream& stream) {
|
||||
wpi_proto_ProtobufElevatorFeedforward msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::ElevatorFeedforward{
|
||||
units::volt_t{msg.ks},
|
||||
units::volt_t{msg.kg},
|
||||
units::unit_t<frc::ElevatorFeedforward::kv_unit>{msg.kv},
|
||||
units::unit_t<frc::ElevatorFeedforward::ka_unit>{msg.ka},
|
||||
return wpi::math::ElevatorFeedforward{
|
||||
wpi::units::volt_t{msg.ks},
|
||||
wpi::units::volt_t{msg.kg},
|
||||
wpi::units::unit_t<wpi::math::ElevatorFeedforward::kv_unit>{msg.kv},
|
||||
wpi::units::unit_t<wpi::math::ElevatorFeedforward::ka_unit>{msg.ka},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::ElevatorFeedforward>::Pack(
|
||||
OutputStream& stream, const frc::ElevatorFeedforward& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::ElevatorFeedforward>::Pack(
|
||||
OutputStream& stream, const wpi::math::ElevatorFeedforward& value) {
|
||||
wpi_proto_ProtobufElevatorFeedforward msg{
|
||||
.ks = value.GetKs().value(),
|
||||
.kg = value.GetKg().value(),
|
||||
|
||||
@@ -11,23 +11,23 @@ constexpr size_t kKvOff = kKgOff + 8;
|
||||
constexpr size_t kKaOff = kKvOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::ArmFeedforward>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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)},
|
||||
wpi::math::ArmFeedforward StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::ArmFeedforward{
|
||||
wpi::units::volt_t{wpi::util::UnpackStruct<double, kKsOff>(data)},
|
||||
wpi::units::volt_t{wpi::util::UnpackStruct<double, kKgOff>(data)},
|
||||
wpi::units::unit_t<wpi::math::ArmFeedforward::kv_unit>{
|
||||
wpi::util::UnpackStruct<double, kKvOff>(data)},
|
||||
wpi::units::unit_t<wpi::math::ArmFeedforward::ka_unit>{
|
||||
wpi::util::UnpackStruct<double, kKaOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
void StructType::Pack(std::span<uint8_t> data,
|
||||
const frc::ArmFeedforward& value) {
|
||||
wpi::PackStruct<kKsOff>(data, value.GetKs().value());
|
||||
wpi::PackStruct<kKgOff>(data, value.GetKg().value());
|
||||
wpi::PackStruct<kKvOff>(data, value.GetKv().value());
|
||||
wpi::PackStruct<kKaOff>(data, value.GetKa().value());
|
||||
const wpi::math::ArmFeedforward& value) {
|
||||
wpi::util::PackStruct<kKsOff>(data, value.GetKs().value());
|
||||
wpi::util::PackStruct<kKgOff>(data, value.GetKg().value());
|
||||
wpi::util::PackStruct<kKvOff>(data, value.GetKv().value());
|
||||
wpi::util::PackStruct<kKaOff>(data, value.GetKa().value());
|
||||
}
|
||||
|
||||
@@ -11,19 +11,19 @@ constexpr size_t kKvAngularOff = kKaLinearOff + 8;
|
||||
constexpr size_t kKaAngularOff = kKvAngularOff + 8;
|
||||
} // namespace
|
||||
|
||||
frc::DifferentialDriveFeedforward wpi::Struct<
|
||||
frc::DifferentialDriveFeedforward>::Unpack(std::span<const uint8_t> data) {
|
||||
wpi::math::DifferentialDriveFeedforward wpi::util::Struct<
|
||||
wpi::math::DifferentialDriveFeedforward>::Unpack(std::span<const uint8_t> data) {
|
||||
return {
|
||||
decltype(1_V / 1_mps){wpi::UnpackStruct<double, kKvLinearOff>(data)},
|
||||
decltype(1_V / 1_mps_sq){wpi::UnpackStruct<double, kKaLinearOff>(data)},
|
||||
decltype(1_V / 1_mps){wpi::UnpackStruct<double, kKvAngularOff>(data)},
|
||||
decltype(1_V / 1_mps_sq){wpi::UnpackStruct<double, kKaAngularOff>(data)}};
|
||||
decltype(1_V / 1_mps){wpi::util::UnpackStruct<double, kKvLinearOff>(data)},
|
||||
decltype(1_V / 1_mps_sq){wpi::util::UnpackStruct<double, kKaLinearOff>(data)},
|
||||
decltype(1_V / 1_mps){wpi::util::UnpackStruct<double, kKvAngularOff>(data)},
|
||||
decltype(1_V / 1_mps_sq){wpi::util::UnpackStruct<double, kKaAngularOff>(data)}};
|
||||
}
|
||||
|
||||
void wpi::Struct<frc::DifferentialDriveFeedforward>::Pack(
|
||||
std::span<uint8_t> data, const frc::DifferentialDriveFeedforward& value) {
|
||||
wpi::PackStruct<kKvLinearOff>(data, value.m_kVLinear.value());
|
||||
wpi::PackStruct<kKaLinearOff>(data, value.m_kALinear.value());
|
||||
wpi::PackStruct<kKvAngularOff>(data, value.m_kVAngular.value());
|
||||
wpi::PackStruct<kKaAngularOff>(data, value.m_kAAngular.value());
|
||||
void wpi::util::Struct<wpi::math::DifferentialDriveFeedforward>::Pack(
|
||||
std::span<uint8_t> data, const wpi::math::DifferentialDriveFeedforward& value) {
|
||||
wpi::util::PackStruct<kKvLinearOff>(data, value.m_kVLinear.value());
|
||||
wpi::util::PackStruct<kKaLinearOff>(data, value.m_kALinear.value());
|
||||
wpi::util::PackStruct<kKvAngularOff>(data, value.m_kVAngular.value());
|
||||
wpi::util::PackStruct<kKaAngularOff>(data, value.m_kAAngular.value());
|
||||
}
|
||||
|
||||
@@ -9,18 +9,18 @@ constexpr size_t kLeftOff = 0;
|
||||
constexpr size_t kRightOff = kLeftOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::DifferentialDriveWheelVoltages>;
|
||||
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelVoltages>;
|
||||
|
||||
frc::DifferentialDriveWheelVoltages StructType::Unpack(
|
||||
wpi::math::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)},
|
||||
return wpi::math::DifferentialDriveWheelVoltages{
|
||||
wpi::units::volt_t{wpi::util::UnpackStruct<double, kLeftOff>(data)},
|
||||
wpi::units::volt_t{wpi::util::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());
|
||||
const wpi::math::DifferentialDriveWheelVoltages& value) {
|
||||
wpi::util::PackStruct<kLeftOff>(data, value.left.value());
|
||||
wpi::util::PackStruct<kRightOff>(data, value.right.value());
|
||||
}
|
||||
|
||||
@@ -11,23 +11,23 @@ constexpr size_t kKvOff = kKgOff + 8;
|
||||
constexpr size_t kKaOff = kKvOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::ElevatorFeedforward>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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)},
|
||||
wpi::math::ElevatorFeedforward StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::ElevatorFeedforward{
|
||||
wpi::units::volt_t{wpi::util::UnpackStruct<double, kKsOff>(data)},
|
||||
wpi::units::volt_t{wpi::util::UnpackStruct<double, kKgOff>(data)},
|
||||
wpi::units::unit_t<wpi::math::ElevatorFeedforward::kv_unit>{
|
||||
wpi::util::UnpackStruct<double, kKvOff>(data)},
|
||||
wpi::units::unit_t<wpi::math::ElevatorFeedforward::ka_unit>{
|
||||
wpi::util::UnpackStruct<double, kKaOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
void StructType::Pack(std::span<uint8_t> data,
|
||||
const frc::ElevatorFeedforward& value) {
|
||||
wpi::PackStruct<kKsOff>(data, value.GetKs().value());
|
||||
wpi::PackStruct<kKgOff>(data, value.GetKg().value());
|
||||
wpi::PackStruct<kKvOff>(data, value.GetKv().value());
|
||||
wpi::PackStruct<kKaOff>(data, value.GetKa().value());
|
||||
const wpi::math::ElevatorFeedforward& value) {
|
||||
wpi::util::PackStruct<kKsOff>(data, value.GetKs().value());
|
||||
wpi::util::PackStruct<kKgOff>(data, value.GetKg().value());
|
||||
wpi::util::PackStruct<kKvOff>(data, value.GetKv().value());
|
||||
wpi::util::PackStruct<kKaOff>(data, value.GetKa().value());
|
||||
}
|
||||
|
||||
@@ -6,11 +6,11 @@
|
||||
|
||||
#include <vector>
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
|
||||
DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
|
||||
units::meter_t leftDistance, units::meter_t rightDistance,
|
||||
wpi::units::meter_t leftDistance, wpi::units::meter_t rightDistance,
|
||||
const Pose2d& initialPose)
|
||||
: DifferentialDrivePoseEstimator{
|
||||
kinematics, gyroAngle, leftDistance, rightDistance,
|
||||
@@ -18,9 +18,9 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
|
||||
|
||||
DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
|
||||
DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle,
|
||||
units::meter_t leftDistance, units::meter_t rightDistance,
|
||||
const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
|
||||
const wpi::array<double, 3>& visionMeasurementStdDevs)
|
||||
wpi::units::meter_t leftDistance, wpi::units::meter_t rightDistance,
|
||||
const Pose2d& initialPose, const wpi::util::array<double, 3>& stateStdDevs,
|
||||
const wpi::util::array<double, 3>& visionMeasurementStdDevs)
|
||||
: PoseEstimator(kinematics, m_odometryImpl, stateStdDevs,
|
||||
visionMeasurementStdDevs),
|
||||
m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {
|
||||
|
||||
@@ -4,11 +4,11 @@
|
||||
|
||||
#include "wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d(
|
||||
DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle,
|
||||
units::meter_t leftDistance, units::meter_t rightDistance,
|
||||
wpi::units::meter_t leftDistance, wpi::units::meter_t rightDistance,
|
||||
const Pose3d& initialPose)
|
||||
: DifferentialDrivePoseEstimator3d{
|
||||
kinematics, gyroAngle, leftDistance,
|
||||
@@ -17,9 +17,9 @@ DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d(
|
||||
|
||||
DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d(
|
||||
DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle,
|
||||
units::meter_t leftDistance, units::meter_t rightDistance,
|
||||
const Pose3d& initialPose, const wpi::array<double, 4>& stateStdDevs,
|
||||
const wpi::array<double, 4>& visionMeasurementStdDevs)
|
||||
wpi::units::meter_t leftDistance, wpi::units::meter_t rightDistance,
|
||||
const Pose3d& initialPose, const wpi::util::array<double, 4>& stateStdDevs,
|
||||
const wpi::util::array<double, 4>& visionMeasurementStdDevs)
|
||||
: PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs,
|
||||
visionMeasurementStdDevs),
|
||||
m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {
|
||||
|
||||
@@ -4,9 +4,9 @@
|
||||
|
||||
#include "wpi/math/estimator/KalmanFilter.hpp"
|
||||
|
||||
namespace frc {
|
||||
namespace wpi::math {
|
||||
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) KalmanFilter<1, 1, 1>;
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) KalmanFilter<2, 1, 1>;
|
||||
|
||||
} // namespace frc
|
||||
} // namespace wpi::math
|
||||
|
||||
@@ -9,20 +9,20 @@
|
||||
#include "wpi/math/util/StateSpaceUtil.hpp"
|
||||
#include "wpi/util/timestamp.h"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
|
||||
wpi::math::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
|
||||
MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
|
||||
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
|
||||
: MecanumDrivePoseEstimator{kinematics, gyroAngle,
|
||||
wheelPositions, initialPose,
|
||||
{0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}} {}
|
||||
|
||||
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
|
||||
wpi::math::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
|
||||
MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle,
|
||||
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
|
||||
const wpi::array<double, 3>& stateStdDevs,
|
||||
const wpi::array<double, 3>& visionMeasurementStdDevs)
|
||||
const wpi::util::array<double, 3>& stateStdDevs,
|
||||
const wpi::util::array<double, 3>& visionMeasurementStdDevs)
|
||||
: PoseEstimator(kinematics, m_odometryImpl, stateStdDevs,
|
||||
visionMeasurementStdDevs),
|
||||
m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {
|
||||
|
||||
@@ -9,9 +9,9 @@
|
||||
#include "wpi/math/util/StateSpaceUtil.hpp"
|
||||
#include "wpi/util/timestamp.h"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d(
|
||||
wpi::math::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d(
|
||||
MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle,
|
||||
const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose)
|
||||
: MecanumDrivePoseEstimator3d{
|
||||
@@ -19,11 +19,11 @@ frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d(
|
||||
wheelPositions, initialPose,
|
||||
{0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}} {}
|
||||
|
||||
frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d(
|
||||
wpi::math::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d(
|
||||
MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle,
|
||||
const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose,
|
||||
const wpi::array<double, 4>& stateStdDevs,
|
||||
const wpi::array<double, 4>& visionMeasurementStdDevs)
|
||||
const wpi::util::array<double, 4>& stateStdDevs,
|
||||
const wpi::util::array<double, 4>& visionMeasurementStdDevs)
|
||||
: PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs,
|
||||
visionMeasurementStdDevs),
|
||||
m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {
|
||||
|
||||
@@ -4,11 +4,11 @@
|
||||
|
||||
#include "wpi/math/estimator/MerweUKF.hpp"
|
||||
|
||||
namespace frc {
|
||||
namespace wpi::math {
|
||||
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
UnscentedKalmanFilter<3, 3, 1, MerweScaledSigmaPoints<3>>;
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
UnscentedKalmanFilter<5, 3, 3, MerweScaledSigmaPoints<5>>;
|
||||
|
||||
} // namespace frc
|
||||
} // namespace wpi::math
|
||||
|
||||
@@ -4,11 +4,11 @@
|
||||
|
||||
#include "wpi/math/estimator/S3UKF.hpp"
|
||||
|
||||
namespace frc {
|
||||
namespace wpi::math {
|
||||
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
UnscentedKalmanFilter<3, 3, 1, S3SigmaPoints<3>>;
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
UnscentedKalmanFilter<5, 3, 3, S3SigmaPoints<5>>;
|
||||
|
||||
} // namespace frc
|
||||
} // namespace wpi::math
|
||||
|
||||
@@ -4,11 +4,11 @@
|
||||
|
||||
#include "wpi/math/estimator/SteadyStateKalmanFilter.hpp"
|
||||
|
||||
namespace frc {
|
||||
namespace wpi::math {
|
||||
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
SteadyStateKalmanFilter<1, 1, 1>;
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
SteadyStateKalmanFilter<2, 1, 1>;
|
||||
|
||||
} // namespace frc
|
||||
} // namespace wpi::math
|
||||
|
||||
@@ -4,9 +4,9 @@
|
||||
|
||||
#include "wpi/math/estimator/SwerveDrivePoseEstimator.hpp"
|
||||
|
||||
namespace frc {
|
||||
namespace wpi::math {
|
||||
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
SwerveDrivePoseEstimator<4>;
|
||||
|
||||
} // namespace frc
|
||||
} // namespace wpi::math
|
||||
|
||||
@@ -4,9 +4,9 @@
|
||||
|
||||
#include "wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp"
|
||||
|
||||
namespace frc {
|
||||
namespace wpi::math {
|
||||
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
SwerveDrivePoseEstimator3d<4>;
|
||||
|
||||
} // namespace frc
|
||||
} // namespace wpi::math
|
||||
|
||||
@@ -6,9 +6,9 @@
|
||||
|
||||
#include "wpi/math/util/MathShared.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
|
||||
Debouncer::Debouncer(wpi::units::second_t debounceTime, DebounceType type)
|
||||
: m_debounceTime(debounceTime), m_debounceType(type) {
|
||||
m_baseline = m_debounceType == DebounceType::kFalling;
|
||||
ResetTimer();
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
|
||||
#include <sleipnir/optimization/problem.hpp>
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
Translation2d Ellipse2d::Nearest(const Translation2d& point) const {
|
||||
// Check if already in ellipse
|
||||
@@ -41,8 +41,8 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const {
|
||||
|
||||
problem.solve();
|
||||
|
||||
rotPoint = frc::Translation2d{units::meter_t{x.value()},
|
||||
units::meter_t{y.value()}};
|
||||
rotPoint = wpi::math::Translation2d{wpi::units::meter_t{x.value()},
|
||||
wpi::units::meter_t{y.value()}};
|
||||
}
|
||||
|
||||
// Undo rotation
|
||||
|
||||
@@ -6,12 +6,12 @@
|
||||
|
||||
#include "wpi/util/json.hpp"
|
||||
|
||||
void frc::to_json(wpi::json& json, const Pose2d& pose) {
|
||||
json = wpi::json{{"translation", pose.Translation()},
|
||||
void wpi::math::to_json(wpi::util::json& json, const Pose2d& pose) {
|
||||
json = wpi::util::json{{"translation", pose.Translation()},
|
||||
{"rotation", pose.Rotation()}};
|
||||
}
|
||||
|
||||
void frc::from_json(const wpi::json& json, Pose2d& pose) {
|
||||
void wpi::math::from_json(const wpi::util::json& json, Pose2d& pose) {
|
||||
pose = Pose2d{json.at("translation").get<Translation2d>(),
|
||||
json.at("rotation").get<Rotation2d>()};
|
||||
}
|
||||
|
||||
@@ -6,12 +6,12 @@
|
||||
|
||||
#include "wpi/util/json.hpp"
|
||||
|
||||
void frc::to_json(wpi::json& json, const Pose3d& pose) {
|
||||
json = wpi::json{{"translation", pose.Translation()},
|
||||
void wpi::math::to_json(wpi::util::json& json, const Pose3d& pose) {
|
||||
json = wpi::util::json{{"translation", pose.Translation()},
|
||||
{"rotation", pose.Rotation()}};
|
||||
}
|
||||
|
||||
void frc::from_json(const wpi::json& json, Pose3d& pose) {
|
||||
void wpi::math::from_json(const wpi::util::json& json, Pose3d& pose) {
|
||||
pose = Pose3d{json.at("translation").get<Translation3d>(),
|
||||
json.at("rotation").get<Rotation3d>()};
|
||||
}
|
||||
|
||||
@@ -6,14 +6,14 @@
|
||||
|
||||
#include "wpi/util/json.hpp"
|
||||
|
||||
void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
|
||||
json = wpi::json{{"W", quaternion.W()},
|
||||
void wpi::math::to_json(wpi::util::json& json, const Quaternion& quaternion) {
|
||||
json = wpi::util::json{{"W", quaternion.W()},
|
||||
{"X", quaternion.X()},
|
||||
{"Y", quaternion.Y()},
|
||||
{"Z", quaternion.Z()}};
|
||||
}
|
||||
|
||||
void frc::from_json(const wpi::json& json, Quaternion& quaternion) {
|
||||
void wpi::math::from_json(const wpi::util::json& json, Quaternion& quaternion) {
|
||||
quaternion =
|
||||
Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
|
||||
json.at("Y").get<double>(), json.at("Z").get<double>()};
|
||||
|
||||
@@ -6,10 +6,10 @@
|
||||
|
||||
#include "wpi/util/json.hpp"
|
||||
|
||||
void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
|
||||
json = wpi::json{{"radians", rotation.Radians().value()}};
|
||||
void wpi::math::to_json(wpi::util::json& json, const Rotation2d& rotation) {
|
||||
json = wpi::util::json{{"radians", rotation.Radians().value()}};
|
||||
}
|
||||
|
||||
void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
|
||||
rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
|
||||
void wpi::math::from_json(const wpi::util::json& json, Rotation2d& rotation) {
|
||||
rotation = Rotation2d{wpi::units::radian_t{json.at("radians").get<double>()}};
|
||||
}
|
||||
|
||||
@@ -6,10 +6,10 @@
|
||||
|
||||
#include "wpi/util/json.hpp"
|
||||
|
||||
void frc::to_json(wpi::json& json, const Rotation3d& rotation) {
|
||||
json = wpi::json{{"quaternion", rotation.GetQuaternion()}};
|
||||
void wpi::math::to_json(wpi::util::json& json, const Rotation3d& rotation) {
|
||||
json = wpi::util::json{{"quaternion", rotation.GetQuaternion()}};
|
||||
}
|
||||
|
||||
void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
|
||||
void wpi::math::from_json(const wpi::util::json& json, Rotation3d& rotation) {
|
||||
rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
|
||||
}
|
||||
|
||||
@@ -6,12 +6,12 @@
|
||||
|
||||
#include "wpi/util/json.hpp"
|
||||
|
||||
void frc::to_json(wpi::json& json, const Translation2d& translation) {
|
||||
void wpi::math::to_json(wpi::util::json& json, const Translation2d& translation) {
|
||||
json =
|
||||
wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
|
||||
wpi::util::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
|
||||
}
|
||||
|
||||
void frc::from_json(const wpi::json& json, Translation2d& translation) {
|
||||
translation = Translation2d{units::meter_t{json.at("x").get<double>()},
|
||||
units::meter_t{json.at("y").get<double>()}};
|
||||
void wpi::math::from_json(const wpi::util::json& json, Translation2d& translation) {
|
||||
translation = Translation2d{wpi::units::meter_t{json.at("x").get<double>()},
|
||||
wpi::units::meter_t{json.at("y").get<double>()}};
|
||||
}
|
||||
|
||||
@@ -6,14 +6,14 @@
|
||||
|
||||
#include "wpi/util/json.hpp"
|
||||
|
||||
void frc::to_json(wpi::json& json, const Translation3d& translation) {
|
||||
json = wpi::json{{"x", translation.X().value()},
|
||||
void wpi::math::to_json(wpi::util::json& json, const Translation3d& translation) {
|
||||
json = wpi::util::json{{"x", translation.X().value()},
|
||||
{"y", translation.Y().value()},
|
||||
{"z", translation.Z().value()}};
|
||||
}
|
||||
|
||||
void frc::from_json(const wpi::json& json, Translation3d& translation) {
|
||||
translation = Translation3d{units::meter_t{json.at("x").get<double>()},
|
||||
units::meter_t{json.at("y").get<double>()},
|
||||
units::meter_t{json.at("z").get<double>()}};
|
||||
void wpi::math::from_json(const wpi::util::json& json, Translation3d& translation) {
|
||||
translation = Translation3d{wpi::units::meter_t{json.at("x").get<double>()},
|
||||
wpi::units::meter_t{json.at("y").get<double>()},
|
||||
wpi::units::meter_t{json.at("z").get<double>()}};
|
||||
}
|
||||
|
||||
@@ -7,9 +7,9 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/geometry2d.npb.h"
|
||||
|
||||
std::optional<frc::Ellipse2d> wpi::Protobuf<frc::Ellipse2d>::Unpack(
|
||||
std::optional<wpi::math::Ellipse2d> wpi::util::Protobuf<wpi::math::Ellipse2d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi::UnpackCallback<frc::Pose2d> pose;
|
||||
wpi::util::UnpackCallback<wpi::math::Pose2d> pose;
|
||||
wpi_proto_ProtobufEllipse2d msg{
|
||||
.center = pose.Callback(),
|
||||
.xSemiAxis = 0,
|
||||
@@ -25,16 +25,16 @@ std::optional<frc::Ellipse2d> wpi::Protobuf<frc::Ellipse2d>::Unpack(
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Ellipse2d{
|
||||
return wpi::math::Ellipse2d{
|
||||
ipose[0],
|
||||
units::meter_t{msg.xSemiAxis},
|
||||
units::meter_t{msg.ySemiAxis},
|
||||
wpi::units::meter_t{msg.xSemiAxis},
|
||||
wpi::units::meter_t{msg.ySemiAxis},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Ellipse2d>::Pack(OutputStream& stream,
|
||||
const frc::Ellipse2d& value) {
|
||||
wpi::PackCallback pose{&value.Center()};
|
||||
bool wpi::util::Protobuf<wpi::math::Ellipse2d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Ellipse2d& value) {
|
||||
wpi::util::PackCallback pose{&value.Center()};
|
||||
wpi_proto_ProtobufEllipse2d msg{
|
||||
.center = pose.Callback(),
|
||||
.xSemiAxis = value.XSemiAxis().value(),
|
||||
|
||||
@@ -7,10 +7,10 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/geometry2d.npb.h"
|
||||
|
||||
std::optional<frc::Pose2d> wpi::Protobuf<frc::Pose2d>::Unpack(
|
||||
std::optional<wpi::math::Pose2d> wpi::util::Protobuf<wpi::math::Pose2d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi::UnpackCallback<frc::Translation2d> tsln;
|
||||
wpi::UnpackCallback<frc::Rotation2d> rot;
|
||||
wpi::util::UnpackCallback<wpi::math::Translation2d> tsln;
|
||||
wpi::util::UnpackCallback<wpi::math::Rotation2d> rot;
|
||||
wpi_proto_ProtobufPose2d msg{
|
||||
.translation = tsln.Callback(),
|
||||
.rotation = rot.Callback(),
|
||||
@@ -26,16 +26,16 @@ std::optional<frc::Pose2d> wpi::Protobuf<frc::Pose2d>::Unpack(
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Pose2d{
|
||||
return wpi::math::Pose2d{
|
||||
itsln[0],
|
||||
irot[0],
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Pose2d>::Pack(OutputStream& stream,
|
||||
const frc::Pose2d& value) {
|
||||
wpi::PackCallback tsln{&value.Translation()};
|
||||
wpi::PackCallback rot{&value.Rotation()};
|
||||
bool wpi::util::Protobuf<wpi::math::Pose2d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Pose2d& value) {
|
||||
wpi::util::PackCallback tsln{&value.Translation()};
|
||||
wpi::util::PackCallback rot{&value.Rotation()};
|
||||
wpi_proto_ProtobufPose2d msg{
|
||||
.translation = tsln.Callback(),
|
||||
.rotation = rot.Callback(),
|
||||
|
||||
@@ -9,10 +9,10 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/geometry3d.npb.h"
|
||||
|
||||
std::optional<frc::Pose3d> wpi::Protobuf<frc::Pose3d>::Unpack(
|
||||
std::optional<wpi::math::Pose3d> wpi::util::Protobuf<wpi::math::Pose3d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi::UnpackCallback<frc::Translation3d> tsln;
|
||||
wpi::UnpackCallback<frc::Rotation3d> rot;
|
||||
wpi::util::UnpackCallback<wpi::math::Translation3d> tsln;
|
||||
wpi::util::UnpackCallback<wpi::math::Rotation3d> rot;
|
||||
wpi_proto_ProtobufPose3d msg{
|
||||
.translation = tsln.Callback(),
|
||||
.rotation = rot.Callback(),
|
||||
@@ -28,16 +28,16 @@ std::optional<frc::Pose3d> wpi::Protobuf<frc::Pose3d>::Unpack(
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Pose3d{
|
||||
return wpi::math::Pose3d{
|
||||
itsln[0],
|
||||
irot[0],
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Pose3d>::Pack(OutputStream& stream,
|
||||
const frc::Pose3d& value) {
|
||||
wpi::PackCallback tsln{&value.Translation()};
|
||||
wpi::PackCallback rot{&value.Rotation()};
|
||||
bool wpi::util::Protobuf<wpi::math::Pose3d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Pose3d& value) {
|
||||
wpi::util::PackCallback tsln{&value.Translation()};
|
||||
wpi::util::PackCallback rot{&value.Rotation()};
|
||||
wpi_proto_ProtobufPose3d msg{
|
||||
.translation = tsln.Callback(),
|
||||
.rotation = rot.Callback(),
|
||||
|
||||
@@ -6,14 +6,14 @@
|
||||
|
||||
#include "wpimath/protobuf/geometry3d.npb.h"
|
||||
|
||||
std::optional<frc::Quaternion> wpi::Protobuf<frc::Quaternion>::Unpack(
|
||||
std::optional<wpi::math::Quaternion> wpi::util::Protobuf<wpi::math::Quaternion>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi_proto_ProtobufQuaternion msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Quaternion{
|
||||
return wpi::math::Quaternion{
|
||||
msg.w,
|
||||
msg.x,
|
||||
msg.y,
|
||||
@@ -21,8 +21,8 @@ std::optional<frc::Quaternion> wpi::Protobuf<frc::Quaternion>::Unpack(
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Quaternion>::Pack(OutputStream& stream,
|
||||
const frc::Quaternion& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::Quaternion>::Pack(OutputStream& stream,
|
||||
const wpi::math::Quaternion& value) {
|
||||
wpi_proto_ProtobufQuaternion msg{
|
||||
.w = value.W(),
|
||||
.x = value.X(),
|
||||
|
||||
@@ -7,9 +7,9 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/geometry2d.npb.h"
|
||||
|
||||
std::optional<frc::Rectangle2d> wpi::Protobuf<frc::Rectangle2d>::Unpack(
|
||||
std::optional<wpi::math::Rectangle2d> wpi::util::Protobuf<wpi::math::Rectangle2d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi::UnpackCallback<frc::Pose2d> pose;
|
||||
wpi::util::UnpackCallback<wpi::math::Pose2d> pose;
|
||||
wpi_proto_ProtobufRectangle2d msg{
|
||||
.center = pose.Callback(),
|
||||
.xWidth = 0,
|
||||
@@ -25,16 +25,16 @@ std::optional<frc::Rectangle2d> wpi::Protobuf<frc::Rectangle2d>::Unpack(
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Rectangle2d{
|
||||
return wpi::math::Rectangle2d{
|
||||
ipose[0],
|
||||
units::meter_t{msg.xWidth},
|
||||
units::meter_t{msg.yWidth},
|
||||
wpi::units::meter_t{msg.xWidth},
|
||||
wpi::units::meter_t{msg.yWidth},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Rectangle2d>::Pack(OutputStream& stream,
|
||||
const frc::Rectangle2d& value) {
|
||||
wpi::PackCallback pose{&value.Center()};
|
||||
bool wpi::util::Protobuf<wpi::math::Rectangle2d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Rectangle2d& value) {
|
||||
wpi::util::PackCallback pose{&value.Center()};
|
||||
wpi_proto_ProtobufRectangle2d msg{
|
||||
.center = pose.Callback(),
|
||||
.xWidth = value.XWidth().value(),
|
||||
|
||||
@@ -6,20 +6,20 @@
|
||||
|
||||
#include "wpimath/protobuf/geometry2d.npb.h"
|
||||
|
||||
std::optional<frc::Rotation2d> wpi::Protobuf<frc::Rotation2d>::Unpack(
|
||||
std::optional<wpi::math::Rotation2d> wpi::util::Protobuf<wpi::math::Rotation2d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi_proto_ProtobufRotation2d msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Rotation2d{
|
||||
units::radian_t{msg.value},
|
||||
return wpi::math::Rotation2d{
|
||||
wpi::units::radian_t{msg.value},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Rotation2d>::Pack(OutputStream& stream,
|
||||
const frc::Rotation2d& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::Rotation2d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Rotation2d& value) {
|
||||
wpi_proto_ProtobufRotation2d msg{
|
||||
.value = value.Radians().value(),
|
||||
};
|
||||
|
||||
@@ -7,9 +7,9 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/geometry3d.npb.h"
|
||||
|
||||
std::optional<frc::Rotation3d> wpi::Protobuf<frc::Rotation3d>::Unpack(
|
||||
std::optional<wpi::math::Rotation3d> wpi::util::Protobuf<wpi::math::Rotation3d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi::UnpackCallback<frc::Quaternion> quat;
|
||||
wpi::util::UnpackCallback<wpi::math::Quaternion> quat;
|
||||
wpi_proto_ProtobufRotation3d msg{
|
||||
.q = quat.Callback(),
|
||||
};
|
||||
@@ -23,14 +23,14 @@ std::optional<frc::Rotation3d> wpi::Protobuf<frc::Rotation3d>::Unpack(
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Rotation3d{
|
||||
return wpi::math::Rotation3d{
|
||||
iquat[0],
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Rotation3d>::Pack(OutputStream& stream,
|
||||
const frc::Rotation3d& value) {
|
||||
wpi::PackCallback quat{&value.GetQuaternion()};
|
||||
bool wpi::util::Protobuf<wpi::math::Rotation3d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Rotation3d& value) {
|
||||
wpi::util::PackCallback quat{&value.GetQuaternion()};
|
||||
wpi_proto_ProtobufRotation3d msg{
|
||||
.q = quat.Callback(),
|
||||
};
|
||||
|
||||
@@ -7,10 +7,10 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/geometry2d.npb.h"
|
||||
|
||||
std::optional<frc::Transform2d> wpi::Protobuf<frc::Transform2d>::Unpack(
|
||||
std::optional<wpi::math::Transform2d> wpi::util::Protobuf<wpi::math::Transform2d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi::UnpackCallback<frc::Translation2d> tsln;
|
||||
wpi::UnpackCallback<frc::Rotation2d> rot;
|
||||
wpi::util::UnpackCallback<wpi::math::Translation2d> tsln;
|
||||
wpi::util::UnpackCallback<wpi::math::Rotation2d> rot;
|
||||
wpi_proto_ProtobufTransform2d msg{
|
||||
.translation = tsln.Callback(),
|
||||
.rotation = rot.Callback(),
|
||||
@@ -26,16 +26,16 @@ std::optional<frc::Transform2d> wpi::Protobuf<frc::Transform2d>::Unpack(
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Transform2d{
|
||||
return wpi::math::Transform2d{
|
||||
itsln[0],
|
||||
irot[0],
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Transform2d>::Pack(OutputStream& stream,
|
||||
const frc::Transform2d& value) {
|
||||
wpi::PackCallback tsln{&value.Translation()};
|
||||
wpi::PackCallback rot{&value.Rotation()};
|
||||
bool wpi::util::Protobuf<wpi::math::Transform2d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Transform2d& value) {
|
||||
wpi::util::PackCallback tsln{&value.Translation()};
|
||||
wpi::util::PackCallback rot{&value.Rotation()};
|
||||
wpi_proto_ProtobufTransform2d msg{
|
||||
.translation = tsln.Callback(),
|
||||
.rotation = rot.Callback(),
|
||||
|
||||
@@ -7,10 +7,10 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/geometry3d.npb.h"
|
||||
|
||||
std::optional<frc::Transform3d> wpi::Protobuf<frc::Transform3d>::Unpack(
|
||||
std::optional<wpi::math::Transform3d> wpi::util::Protobuf<wpi::math::Transform3d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi::UnpackCallback<frc::Translation3d> tsln;
|
||||
wpi::UnpackCallback<frc::Rotation3d> rot;
|
||||
wpi::util::UnpackCallback<wpi::math::Translation3d> tsln;
|
||||
wpi::util::UnpackCallback<wpi::math::Rotation3d> rot;
|
||||
wpi_proto_ProtobufTransform3d msg{
|
||||
.translation = tsln.Callback(),
|
||||
.rotation = rot.Callback(),
|
||||
@@ -26,16 +26,16 @@ std::optional<frc::Transform3d> wpi::Protobuf<frc::Transform3d>::Unpack(
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Transform3d{
|
||||
return wpi::math::Transform3d{
|
||||
itsln[0],
|
||||
irot[0],
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Transform3d>::Pack(OutputStream& stream,
|
||||
const frc::Transform3d& value) {
|
||||
wpi::PackCallback tsln{&value.Translation()};
|
||||
wpi::PackCallback rot{&value.Rotation()};
|
||||
bool wpi::util::Protobuf<wpi::math::Transform3d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Transform3d& value) {
|
||||
wpi::util::PackCallback tsln{&value.Translation()};
|
||||
wpi::util::PackCallback rot{&value.Rotation()};
|
||||
wpi_proto_ProtobufTransform3d msg{
|
||||
.translation = tsln.Callback(),
|
||||
.rotation = rot.Callback(),
|
||||
|
||||
@@ -6,21 +6,21 @@
|
||||
|
||||
#include "wpimath/protobuf/geometry2d.npb.h"
|
||||
|
||||
std::optional<frc::Translation2d> wpi::Protobuf<frc::Translation2d>::Unpack(
|
||||
std::optional<wpi::math::Translation2d> wpi::util::Protobuf<wpi::math::Translation2d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi_proto_ProtobufTranslation2d msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Translation2d{
|
||||
units::meter_t{msg.x},
|
||||
units::meter_t{msg.y},
|
||||
return wpi::math::Translation2d{
|
||||
wpi::units::meter_t{msg.x},
|
||||
wpi::units::meter_t{msg.y},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Translation2d>::Pack(OutputStream& stream,
|
||||
const frc::Translation2d& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::Translation2d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Translation2d& value) {
|
||||
wpi_proto_ProtobufTranslation2d msg{
|
||||
.x = value.X().value(),
|
||||
.y = value.Y().value(),
|
||||
|
||||
@@ -6,22 +6,22 @@
|
||||
|
||||
#include "wpimath/protobuf/geometry3d.npb.h"
|
||||
|
||||
std::optional<frc::Translation3d> wpi::Protobuf<frc::Translation3d>::Unpack(
|
||||
std::optional<wpi::math::Translation3d> wpi::util::Protobuf<wpi::math::Translation3d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi_proto_ProtobufTranslation3d msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Translation3d{
|
||||
units::meter_t{msg.x},
|
||||
units::meter_t{msg.y},
|
||||
units::meter_t{msg.z},
|
||||
return wpi::math::Translation3d{
|
||||
wpi::units::meter_t{msg.x},
|
||||
wpi::units::meter_t{msg.y},
|
||||
wpi::units::meter_t{msg.z},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Translation3d>::Pack(OutputStream& stream,
|
||||
const frc::Translation3d& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::Translation3d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Translation3d& value) {
|
||||
wpi_proto_ProtobufTranslation3d msg{
|
||||
.x = value.X().value(),
|
||||
.y = value.Y().value(),
|
||||
|
||||
@@ -6,22 +6,22 @@
|
||||
|
||||
#include "wpimath/protobuf/geometry2d.npb.h"
|
||||
|
||||
std::optional<frc::Twist2d> wpi::Protobuf<frc::Twist2d>::Unpack(
|
||||
std::optional<wpi::math::Twist2d> wpi::util::Protobuf<wpi::math::Twist2d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi_proto_ProtobufTwist2d msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Twist2d{
|
||||
units::meter_t{msg.dx},
|
||||
units::meter_t{msg.dy},
|
||||
units::radian_t{msg.dtheta},
|
||||
return wpi::math::Twist2d{
|
||||
wpi::units::meter_t{msg.dx},
|
||||
wpi::units::meter_t{msg.dy},
|
||||
wpi::units::radian_t{msg.dtheta},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Twist2d>::Pack(OutputStream& stream,
|
||||
const frc::Twist2d& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::Twist2d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Twist2d& value) {
|
||||
wpi_proto_ProtobufTwist2d msg{
|
||||
.dx = value.dx.value(),
|
||||
.dy = value.dy.value(),
|
||||
|
||||
@@ -6,21 +6,21 @@
|
||||
|
||||
#include "wpimath/protobuf/geometry3d.npb.h"
|
||||
|
||||
std::optional<frc::Twist3d> wpi::Protobuf<frc::Twist3d>::Unpack(
|
||||
std::optional<wpi::math::Twist3d> wpi::util::Protobuf<wpi::math::Twist3d>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi_proto_ProtobufTwist3d msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::Twist3d{
|
||||
units::meter_t{msg.dx}, units::meter_t{msg.dy}, units::meter_t{msg.dz},
|
||||
units::radian_t{msg.rx}, units::radian_t{msg.ry}, units::radian_t{msg.rz},
|
||||
return wpi::math::Twist3d{
|
||||
wpi::units::meter_t{msg.dx}, wpi::units::meter_t{msg.dy}, wpi::units::meter_t{msg.dz},
|
||||
wpi::units::radian_t{msg.rx}, wpi::units::radian_t{msg.ry}, wpi::units::radian_t{msg.rz},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::Twist3d>::Pack(OutputStream& stream,
|
||||
const frc::Twist3d& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::Twist3d>::Pack(OutputStream& stream,
|
||||
const wpi::math::Twist3d& value) {
|
||||
wpi_proto_ProtobufTwist3d msg{
|
||||
.dx = value.dx.value(),
|
||||
.dy = value.dy.value(),
|
||||
|
||||
@@ -6,22 +6,22 @@
|
||||
|
||||
namespace {
|
||||
constexpr size_t kCenterOff = 0;
|
||||
constexpr size_t kXSemiAxisOff = kCenterOff + wpi::GetStructSize<frc::Pose2d>();
|
||||
constexpr size_t kXSemiAxisOff = kCenterOff + wpi::util::GetStructSize<wpi::math::Pose2d>();
|
||||
constexpr size_t kYSemiAxisOff = kXSemiAxisOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Ellipse2d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::Ellipse2d>;
|
||||
|
||||
frc::Ellipse2d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return frc::Ellipse2d{
|
||||
wpi::UnpackStruct<frc::Pose2d, kCenterOff>(data),
|
||||
units::meter_t{wpi::UnpackStruct<double, kXSemiAxisOff>(data)},
|
||||
units::meter_t{wpi::UnpackStruct<double, kYSemiAxisOff>(data)},
|
||||
wpi::math::Ellipse2d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Ellipse2d{
|
||||
wpi::util::UnpackStruct<wpi::math::Pose2d, kCenterOff>(data),
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kXSemiAxisOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kYSemiAxisOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
void StructType::Pack(std::span<uint8_t> data, const frc::Ellipse2d& value) {
|
||||
wpi::PackStruct<kCenterOff>(data, value.Center());
|
||||
wpi::PackStruct<kXSemiAxisOff>(data, value.XSemiAxis().value());
|
||||
wpi::PackStruct<kYSemiAxisOff>(data, value.YSemiAxis().value());
|
||||
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Ellipse2d& value) {
|
||||
wpi::util::PackStruct<kCenterOff>(data, value.Center());
|
||||
wpi::util::PackStruct<kXSemiAxisOff>(data, value.XSemiAxis().value());
|
||||
wpi::util::PackStruct<kYSemiAxisOff>(data, value.YSemiAxis().value());
|
||||
}
|
||||
|
||||
@@ -7,19 +7,19 @@
|
||||
namespace {
|
||||
constexpr size_t kTranslationOff = 0;
|
||||
constexpr size_t kRotationOff =
|
||||
kTranslationOff + wpi::GetStructSize<frc::Translation2d>();
|
||||
kTranslationOff + wpi::util::GetStructSize<wpi::math::Translation2d>();
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Pose2d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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),
|
||||
wpi::math::Pose2d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Pose2d{
|
||||
wpi::util::UnpackStruct<wpi::math::Translation2d, kTranslationOff>(data),
|
||||
wpi::util::UnpackStruct<wpi::math::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());
|
||||
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Pose2d& value) {
|
||||
wpi::util::PackStruct<kTranslationOff>(data, value.Translation());
|
||||
wpi::util::PackStruct<kRotationOff>(data, value.Rotation());
|
||||
}
|
||||
|
||||
@@ -7,19 +7,19 @@
|
||||
namespace {
|
||||
constexpr size_t kTranslationOff = 0;
|
||||
constexpr size_t kRotationOff =
|
||||
kTranslationOff + wpi::GetStructSize<frc::Translation3d>();
|
||||
kTranslationOff + wpi::util::GetStructSize<wpi::math::Translation3d>();
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Pose3d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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),
|
||||
wpi::math::Pose3d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Pose3d{
|
||||
wpi::util::UnpackStruct<wpi::math::Translation3d, kTranslationOff>(data),
|
||||
wpi::util::UnpackStruct<wpi::math::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());
|
||||
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Pose3d& value) {
|
||||
wpi::util::PackStruct<kTranslationOff>(data, value.Translation());
|
||||
wpi::util::PackStruct<kRotationOff>(data, value.Rotation());
|
||||
}
|
||||
|
||||
@@ -11,20 +11,20 @@ constexpr size_t kYOff = kXOff + 8;
|
||||
constexpr size_t kZOff = kYOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Quaternion>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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),
|
||||
wpi::math::Quaternion StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Quaternion{
|
||||
wpi::util::UnpackStruct<double, kWOff>(data),
|
||||
wpi::util::UnpackStruct<double, kXOff>(data),
|
||||
wpi::util::UnpackStruct<double, kYOff>(data),
|
||||
wpi::util::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());
|
||||
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Quaternion& value) {
|
||||
wpi::util::PackStruct<kWOff>(data, value.W());
|
||||
wpi::util::PackStruct<kXOff>(data, value.X());
|
||||
wpi::util::PackStruct<kYOff>(data, value.Y());
|
||||
wpi::util::PackStruct<kZOff>(data, value.Z());
|
||||
}
|
||||
|
||||
@@ -6,22 +6,22 @@
|
||||
|
||||
namespace {
|
||||
constexpr size_t kCenterOff = 0;
|
||||
constexpr size_t kXWidthOff = kCenterOff + wpi::GetStructSize<frc::Pose2d>();
|
||||
constexpr size_t kXWidthOff = kCenterOff + wpi::util::GetStructSize<wpi::math::Pose2d>();
|
||||
constexpr size_t kYWidthOff = kXWidthOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Rectangle2d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::Rectangle2d>;
|
||||
|
||||
frc::Rectangle2d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return frc::Rectangle2d{
|
||||
wpi::UnpackStruct<frc::Pose2d, kCenterOff>(data),
|
||||
units::meter_t{wpi::UnpackStruct<double, kXWidthOff>(data)},
|
||||
units::meter_t{wpi::UnpackStruct<double, kYWidthOff>(data)},
|
||||
wpi::math::Rectangle2d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Rectangle2d{
|
||||
wpi::util::UnpackStruct<wpi::math::Pose2d, kCenterOff>(data),
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kXWidthOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kYWidthOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
void StructType::Pack(std::span<uint8_t> data, const frc::Rectangle2d& value) {
|
||||
wpi::PackStruct<kCenterOff>(data, value.Center());
|
||||
wpi::PackStruct<kXWidthOff>(data, value.XWidth().value());
|
||||
wpi::PackStruct<kYWidthOff>(data, value.YWidth().value());
|
||||
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Rectangle2d& value) {
|
||||
wpi::util::PackStruct<kCenterOff>(data, value.Center());
|
||||
wpi::util::PackStruct<kXWidthOff>(data, value.XWidth().value());
|
||||
wpi::util::PackStruct<kYWidthOff>(data, value.YWidth().value());
|
||||
}
|
||||
|
||||
@@ -8,14 +8,14 @@ namespace {
|
||||
constexpr size_t kValueOff = 0;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Rotation2d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::Rotation2d>;
|
||||
|
||||
frc::Rotation2d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return frc::Rotation2d{
|
||||
units::radian_t{wpi::UnpackStruct<double, kValueOff>(data)},
|
||||
wpi::math::Rotation2d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Rotation2d{
|
||||
wpi::units::radian_t{wpi::util::UnpackStruct<double, kValueOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
void StructType::Pack(std::span<uint8_t> data, const frc::Rotation2d& value) {
|
||||
wpi::PackStruct<kValueOff>(data, value.Radians().value());
|
||||
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Rotation2d& value) {
|
||||
wpi::util::PackStruct<kValueOff>(data, value.Radians().value());
|
||||
}
|
||||
|
||||
@@ -8,14 +8,14 @@ namespace {
|
||||
constexpr size_t kQOff = 0;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Rotation3d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::Rotation3d>;
|
||||
|
||||
frc::Rotation3d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return frc::Rotation3d{
|
||||
wpi::UnpackStruct<frc::Quaternion, kQOff>(data),
|
||||
wpi::math::Rotation3d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Rotation3d{
|
||||
wpi::util::UnpackStruct<wpi::math::Quaternion, kQOff>(data),
|
||||
};
|
||||
}
|
||||
|
||||
void StructType::Pack(std::span<uint8_t> data, const frc::Rotation3d& value) {
|
||||
wpi::PackStruct<kQOff>(data, value.GetQuaternion());
|
||||
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Rotation3d& value) {
|
||||
wpi::util::PackStruct<kQOff>(data, value.GetQuaternion());
|
||||
}
|
||||
|
||||
@@ -7,19 +7,19 @@
|
||||
namespace {
|
||||
constexpr size_t kTranslationOff = 0;
|
||||
constexpr size_t kRotationOff =
|
||||
kTranslationOff + wpi::GetStructSize<frc::Translation2d>();
|
||||
kTranslationOff + wpi::util::GetStructSize<wpi::math::Translation2d>();
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Transform2d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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),
|
||||
wpi::math::Transform2d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Transform2d{
|
||||
wpi::util::UnpackStruct<wpi::math::Translation2d, kTranslationOff>(data),
|
||||
wpi::util::UnpackStruct<wpi::math::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());
|
||||
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Transform2d& value) {
|
||||
wpi::util::PackStruct<kTranslationOff>(data, value.Translation());
|
||||
wpi::util::PackStruct<kRotationOff>(data, value.Rotation());
|
||||
}
|
||||
|
||||
@@ -7,19 +7,19 @@
|
||||
namespace {
|
||||
constexpr size_t kTranslationOff = 0;
|
||||
constexpr size_t kRotationOff =
|
||||
kTranslationOff + wpi::GetStructSize<frc::Translation3d>();
|
||||
kTranslationOff + wpi::util::GetStructSize<wpi::math::Translation3d>();
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Transform3d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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),
|
||||
wpi::math::Transform3d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Transform3d{
|
||||
wpi::util::UnpackStruct<wpi::math::Translation3d, kTranslationOff>(data),
|
||||
wpi::util::UnpackStruct<wpi::math::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());
|
||||
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Transform3d& value) {
|
||||
wpi::util::PackStruct<kTranslationOff>(data, value.Translation());
|
||||
wpi::util::PackStruct<kRotationOff>(data, value.Rotation());
|
||||
}
|
||||
|
||||
@@ -9,17 +9,17 @@ constexpr size_t kXOff = 0;
|
||||
constexpr size_t kYOff = kXOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Translation2d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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)},
|
||||
wpi::math::Translation2d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Translation2d{
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kXOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::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());
|
||||
const wpi::math::Translation2d& value) {
|
||||
wpi::util::PackStruct<kXOff>(data, value.X().value());
|
||||
wpi::util::PackStruct<kYOff>(data, value.Y().value());
|
||||
}
|
||||
|
||||
@@ -10,19 +10,19 @@ constexpr size_t kYOff = kXOff + 8;
|
||||
constexpr size_t kZOff = kYOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Translation3d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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)},
|
||||
wpi::math::Translation3d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Translation3d{
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kXOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kYOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::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());
|
||||
const wpi::math::Translation3d& value) {
|
||||
wpi::util::PackStruct<kXOff>(data, value.X().value());
|
||||
wpi::util::PackStruct<kYOff>(data, value.Y().value());
|
||||
wpi::util::PackStruct<kZOff>(data, value.Z().value());
|
||||
}
|
||||
|
||||
@@ -10,18 +10,18 @@ constexpr size_t kDyOff = kDxOff + 8;
|
||||
constexpr size_t kDthetaOff = kDyOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Twist2d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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)},
|
||||
wpi::math::Twist2d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Twist2d{
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kDxOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kDyOff>(data)},
|
||||
wpi::units::radian_t{wpi::util::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());
|
||||
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Twist2d& value) {
|
||||
wpi::util::PackStruct<kDxOff>(data, value.dx.value());
|
||||
wpi::util::PackStruct<kDyOff>(data, value.dy.value());
|
||||
wpi::util::PackStruct<kDthetaOff>(data, value.dtheta.value());
|
||||
}
|
||||
|
||||
@@ -13,24 +13,24 @@ constexpr size_t kRyOff = kRxOff + 8;
|
||||
constexpr size_t kRzOff = kRyOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::Twist3d>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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)},
|
||||
wpi::math::Twist3d StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::Twist3d{
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kDxOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kDyOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kDzOff>(data)},
|
||||
wpi::units::radian_t{wpi::util::UnpackStruct<double, kRxOff>(data)},
|
||||
wpi::units::radian_t{wpi::util::UnpackStruct<double, kRyOff>(data)},
|
||||
wpi::units::radian_t{wpi::util::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());
|
||||
void StructType::Pack(std::span<uint8_t> data, const wpi::math::Twist3d& value) {
|
||||
wpi::util::PackStruct<kDxOff>(data, value.dx.value());
|
||||
wpi::util::PackStruct<kDyOff>(data, value.dy.value());
|
||||
wpi::util::PackStruct<kDzOff>(data, value.dz.value());
|
||||
wpi::util::PackStruct<kRxOff>(data, value.rx.value());
|
||||
wpi::util::PackStruct<kRyOff>(data, value.ry.value());
|
||||
wpi::util::PackStruct<kRzOff>(data, value.rz.value());
|
||||
}
|
||||
|
||||
@@ -8,7 +8,7 @@
|
||||
#include "wpi/math/controller/ArmFeedforward.hpp"
|
||||
#include "wpi/util/jni_util.hpp"
|
||||
|
||||
using namespace wpi::java;
|
||||
using namespace wpi::util::java;
|
||||
|
||||
extern "C" {
|
||||
|
||||
@@ -23,13 +23,13 @@ Java_org_wpilib_math_jni_ArmFeedforwardJNI_calculate
|
||||
jdouble currentAngle, jdouble currentVelocity, jdouble nextVelocity,
|
||||
jdouble dt)
|
||||
{
|
||||
return frc::ArmFeedforward{units::volt_t{ks}, units::volt_t{kg},
|
||||
units::unit_t<frc::ArmFeedforward::kv_unit>{kv},
|
||||
units::unit_t<frc::ArmFeedforward::ka_unit>{ka},
|
||||
units::second_t{dt}}
|
||||
.Calculate(units::radian_t{currentAngle},
|
||||
units::radians_per_second_t{currentVelocity},
|
||||
units::radians_per_second_t{nextVelocity})
|
||||
return wpi::math::ArmFeedforward{wpi::units::volt_t{ks}, wpi::units::volt_t{kg},
|
||||
wpi::units::unit_t<wpi::math::ArmFeedforward::kv_unit>{kv},
|
||||
wpi::units::unit_t<wpi::math::ArmFeedforward::ka_unit>{ka},
|
||||
wpi::units::second_t{dt}}
|
||||
.Calculate(wpi::units::radian_t{currentAngle},
|
||||
wpi::units::radians_per_second_t{currentVelocity},
|
||||
wpi::units::radians_per_second_t{nextVelocity})
|
||||
.value();
|
||||
}
|
||||
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
#include "wpi/math/linalg/DARE.hpp"
|
||||
#include "wpi/util/jni_util.hpp"
|
||||
|
||||
using namespace wpi::java;
|
||||
using namespace wpi::util::java;
|
||||
|
||||
extern "C" {
|
||||
|
||||
@@ -46,7 +46,7 @@ Java_org_wpilib_math_jni_DAREJNI_dareNoPrecondABQR
|
||||
Rmat{nativeR.data(), inputs, inputs};
|
||||
|
||||
auto result =
|
||||
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, false)
|
||||
wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, false)
|
||||
.value();
|
||||
|
||||
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
|
||||
@@ -84,7 +84,7 @@ Java_org_wpilib_math_jni_DAREJNI_dareNoPrecondABQRN
|
||||
Eigen::RowMajor>>
|
||||
Nmat{nativeN.data(), states, inputs};
|
||||
|
||||
auto result = frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat,
|
||||
auto result = wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat,
|
||||
Rmat, Nmat, false)
|
||||
.value();
|
||||
|
||||
@@ -120,21 +120,21 @@ Java_org_wpilib_math_jni_DAREJNI_dareABQR
|
||||
Rmat{nativeR.data(), inputs, inputs};
|
||||
|
||||
if (auto result =
|
||||
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat)) {
|
||||
wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat)) {
|
||||
env->SetDoubleArrayRegion(S, 0, states * states, result.value().data());
|
||||
// K = (BᵀSB + R)⁻¹BᵀSA
|
||||
} else if (result.error() == frc::DAREError::QNotSymmetric ||
|
||||
result.error() == frc::DAREError::QNotPositiveSemidefinite) {
|
||||
} else if (result.error() == wpi::math::DAREError::QNotSymmetric ||
|
||||
result.error() == wpi::math::DAREError::QNotPositiveSemidefinite) {
|
||||
illegalArgEx.Throw(
|
||||
env, fmt::format("{}\n\nQ =\n{}\n", to_string(result.error()), Qmat));
|
||||
} else if (result.error() == frc::DAREError::RNotSymmetric ||
|
||||
result.error() == frc::DAREError::RNotPositiveDefinite) {
|
||||
} else if (result.error() == wpi::math::DAREError::RNotSymmetric ||
|
||||
result.error() == wpi::math::DAREError::RNotPositiveDefinite) {
|
||||
illegalArgEx.Throw(
|
||||
env, fmt::format("{}\n\nR =\n{}\n", to_string(result.error()), Rmat));
|
||||
} else if (result.error() == frc::DAREError::ABNotStabilizable) {
|
||||
} else if (result.error() == wpi::math::DAREError::ABNotStabilizable) {
|
||||
illegalArgEx.Throw(env, fmt::format("{}\n\nA =\n{}\nB =\n{}\n",
|
||||
to_string(result.error()), Amat, Bmat));
|
||||
} else if (result.error() == frc::DAREError::ACNotDetectable) {
|
||||
} else if (result.error() == wpi::math::DAREError::ACNotDetectable) {
|
||||
illegalArgEx.Throw(env, fmt::format("{}\n\nA =\n{}\nQ =\n{}\n",
|
||||
to_string(result.error()), Amat, Qmat));
|
||||
}
|
||||
@@ -172,23 +172,23 @@ Java_org_wpilib_math_jni_DAREJNI_dareABQRN
|
||||
Eigen::RowMajor>>
|
||||
Nmat{nativeN.data(), states, inputs};
|
||||
|
||||
if (auto result = frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat,
|
||||
if (auto result = wpi::math::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat,
|
||||
Rmat, Nmat)) {
|
||||
env->SetDoubleArrayRegion(S, 0, states * states, result.value().data());
|
||||
} else if (result.error() == frc::DAREError::QNotSymmetric ||
|
||||
result.error() == frc::DAREError::QNotPositiveSemidefinite) {
|
||||
} else if (result.error() == wpi::math::DAREError::QNotSymmetric ||
|
||||
result.error() == wpi::math::DAREError::QNotPositiveSemidefinite) {
|
||||
illegalArgEx.Throw(
|
||||
env, fmt::format("{}\n\nQ =\n{}\n", to_string(result.error()), Qmat));
|
||||
} else if (result.error() == frc::DAREError::RNotSymmetric ||
|
||||
result.error() == frc::DAREError::RNotPositiveDefinite) {
|
||||
} else if (result.error() == wpi::math::DAREError::RNotSymmetric ||
|
||||
result.error() == wpi::math::DAREError::RNotPositiveDefinite) {
|
||||
illegalArgEx.Throw(
|
||||
env, fmt::format("{}\n\nR =\n{}\n", to_string(result.error()), Rmat));
|
||||
} else if (result.error() == frc::DAREError::ABNotStabilizable) {
|
||||
} else if (result.error() == wpi::math::DAREError::ABNotStabilizable) {
|
||||
illegalArgEx.Throw(
|
||||
env,
|
||||
fmt::format("{}\n\nA =\n{}\nB =\n{}\n", to_string(result.error()),
|
||||
Amat - Bmat * Rmat.llt().solve(Nmat.transpose()), Bmat));
|
||||
} else if (result.error() == frc::DAREError::ACNotDetectable) {
|
||||
} else if (result.error() == wpi::math::DAREError::ACNotDetectable) {
|
||||
auto R_llt = Rmat.llt();
|
||||
illegalArgEx.Throw(
|
||||
env, fmt::format("{}\n\nA =\n{}\nQ =\n{}\n", to_string(result.error()),
|
||||
|
||||
@@ -12,7 +12,7 @@
|
||||
#include "org_wpilib_math_jni_EigenJNI.h"
|
||||
#include "wpi/util/jni_util.hpp"
|
||||
|
||||
using namespace wpi::java;
|
||||
using namespace wpi::util::java;
|
||||
|
||||
extern "C" {
|
||||
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
#include "wpi/util/array.hpp"
|
||||
#include "wpi/util/jni_util.hpp"
|
||||
|
||||
using namespace wpi::java;
|
||||
using namespace wpi::util::java;
|
||||
|
||||
extern "C" {
|
||||
|
||||
@@ -25,13 +25,13 @@ Java_org_wpilib_math_jni_Ellipse2dJNI_nearest
|
||||
jdoubleArray nearestPoint)
|
||||
{
|
||||
auto point =
|
||||
frc::Ellipse2d{
|
||||
frc::Pose2d{units::meter_t{centerX}, units::meter_t{centerY},
|
||||
units::radian_t{centerHeading}},
|
||||
units::meter_t{xSemiAxis}, units::meter_t{ySemiAxis}}
|
||||
.Nearest({units::meter_t{pointX}, units::meter_t{pointY}});
|
||||
wpi::math::Ellipse2d{
|
||||
wpi::math::Pose2d{wpi::units::meter_t{centerX}, wpi::units::meter_t{centerY},
|
||||
wpi::units::radian_t{centerHeading}},
|
||||
wpi::units::meter_t{xSemiAxis}, wpi::units::meter_t{ySemiAxis}}
|
||||
.Nearest({wpi::units::meter_t{pointX}, wpi::units::meter_t{pointY}});
|
||||
|
||||
wpi::array buf{point.X().value(), point.Y().value()};
|
||||
wpi::util::array buf{point.X().value(), point.Y().value()};
|
||||
env->SetDoubleArrayRegion(nearestPoint, 0, 2, buf.data());
|
||||
}
|
||||
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
|
||||
#include <jni.h>
|
||||
|
||||
using namespace wpi::java;
|
||||
using namespace wpi::util::java;
|
||||
|
||||
//
|
||||
// Globals and load/unload
|
||||
|
||||
@@ -6,6 +6,6 @@
|
||||
|
||||
#include "wpi/util/jni_util.hpp"
|
||||
|
||||
extern wpi::java::JException illegalArgEx;
|
||||
extern wpi::java::JException ioEx;
|
||||
extern wpi::java::JException trajectorySerializationEx;
|
||||
extern wpi::util::java::JException illegalArgEx;
|
||||
extern wpi::util::java::JException ioEx;
|
||||
extern wpi::util::java::JException trajectorySerializationEx;
|
||||
|
||||
@@ -10,7 +10,7 @@
|
||||
#include "wpi/math/util/StateSpaceUtil.hpp"
|
||||
#include "wpi/util/jni_util.hpp"
|
||||
|
||||
using namespace wpi::java;
|
||||
using namespace wpi::util::java;
|
||||
|
||||
extern "C" {
|
||||
|
||||
@@ -36,7 +36,7 @@ Java_org_wpilib_math_jni_StateSpaceUtilJNI_isStabilizable
|
||||
B{nativeB.data(), states, inputs};
|
||||
|
||||
bool isStabilizable =
|
||||
frc::IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(A, B);
|
||||
wpi::math::IsStabilizable<Eigen::Dynamic, Eigen::Dynamic>(A, B);
|
||||
|
||||
return isStabilizable;
|
||||
}
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
#include "wpi/units/length.hpp"
|
||||
#include "wpi/util/jni_util.hpp"
|
||||
|
||||
using namespace wpi::java;
|
||||
using namespace wpi::util::java;
|
||||
|
||||
extern "C" {
|
||||
|
||||
@@ -27,11 +27,11 @@ Java_org_wpilib_math_jni_Transform3dJNI_log
|
||||
(JNIEnv* env, jclass, jdouble relX, jdouble relY, jdouble relZ, jdouble relQw,
|
||||
jdouble relQx, jdouble relQy, jdouble relQz)
|
||||
{
|
||||
frc::Transform3d transform3d{
|
||||
units::meter_t{relX}, units::meter_t{relY}, units::meter_t{relZ},
|
||||
frc::Rotation3d{frc::Quaternion{relQw, relQx, relQy, relQz}}};
|
||||
wpi::math::Transform3d transform3d{
|
||||
wpi::units::meter_t{relX}, wpi::units::meter_t{relY}, wpi::units::meter_t{relZ},
|
||||
wpi::math::Rotation3d{wpi::math::Quaternion{relQw, relQx, relQy, relQz}}};
|
||||
|
||||
frc::Twist3d result = transform3d.Log();
|
||||
wpi::math::Twist3d result = transform3d.Log();
|
||||
|
||||
return MakeJDoubleArray(
|
||||
env, {{result.dx.value(), result.dy.value(), result.dz.value(),
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
#include "wpi/units/length.hpp"
|
||||
#include "wpi/util/jni_util.hpp"
|
||||
|
||||
using namespace wpi::java;
|
||||
using namespace wpi::util::java;
|
||||
|
||||
extern "C" {
|
||||
|
||||
@@ -25,11 +25,11 @@ Java_org_wpilib_math_jni_Twist3dJNI_exp
|
||||
(JNIEnv* env, jclass, jdouble twistDx, jdouble twistDy, jdouble twistDz,
|
||||
jdouble twistRx, jdouble twistRy, jdouble twistRz)
|
||||
{
|
||||
frc::Twist3d twist{units::meter_t{twistDx}, units::meter_t{twistDy},
|
||||
units::meter_t{twistDz}, units::radian_t{twistRx},
|
||||
units::radian_t{twistRy}, units::radian_t{twistRz}};
|
||||
wpi::math::Twist3d twist{wpi::units::meter_t{twistDx}, wpi::units::meter_t{twistDy},
|
||||
wpi::units::meter_t{twistDz}, wpi::units::radian_t{twistRx},
|
||||
wpi::units::radian_t{twistRy}, wpi::units::radian_t{twistRz}};
|
||||
|
||||
frc::Transform3d result = twist.Exp();
|
||||
wpi::math::Transform3d result = twist.Exp();
|
||||
|
||||
const auto& resultQuaternion = result.Rotation().GetQuaternion();
|
||||
return MakeJDoubleArray(
|
||||
|
||||
@@ -6,11 +6,11 @@
|
||||
|
||||
#include "wpi/math/util/MathShared.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
DifferentialDriveOdometry::DifferentialDriveOdometry(
|
||||
const Rotation2d& gyroAngle, units::meter_t leftDistance,
|
||||
units::meter_t rightDistance, const Pose2d& initialPose)
|
||||
const Rotation2d& gyroAngle, wpi::units::meter_t leftDistance,
|
||||
wpi::units::meter_t rightDistance, const Pose2d& initialPose)
|
||||
: Odometry(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
|
||||
initialPose) {
|
||||
wpi::math::MathSharedStore::ReportUsage("DifferentialDriveOdometry", "");
|
||||
|
||||
@@ -6,11 +6,11 @@
|
||||
|
||||
#include "wpi/math/util/MathShared.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
DifferentialDriveOdometry3d::DifferentialDriveOdometry3d(
|
||||
const Rotation3d& gyroAngle, units::meter_t leftDistance,
|
||||
units::meter_t rightDistance, const Pose3d& initialPose)
|
||||
const Rotation3d& gyroAngle, wpi::units::meter_t leftDistance,
|
||||
wpi::units::meter_t rightDistance, const Pose3d& initialPose)
|
||||
: Odometry3d(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance},
|
||||
initialPose) {
|
||||
wpi::math::MathSharedStore::ReportUsage("DifferentialDriveOdometry3d", "");
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
|
||||
#include "wpi/math/kinematics/MecanumDriveKinematics.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
|
||||
const ChassisSpeeds& chassisSpeeds,
|
||||
@@ -28,10 +28,10 @@ MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
|
||||
Eigen::Vector4d wheelsVector = m_inverseKinematics * chassisSpeedsVector;
|
||||
|
||||
MecanumDriveWheelSpeeds wheelSpeeds;
|
||||
wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
|
||||
wheelSpeeds.frontRight = units::meters_per_second_t{wheelsVector(1)};
|
||||
wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsVector(2)};
|
||||
wheelSpeeds.rearRight = units::meters_per_second_t{wheelsVector(3)};
|
||||
wheelSpeeds.frontLeft = wpi::units::meters_per_second_t{wheelsVector(0)};
|
||||
wheelSpeeds.frontRight = wpi::units::meters_per_second_t{wheelsVector(1)};
|
||||
wheelSpeeds.rearLeft = wpi::units::meters_per_second_t{wheelsVector(2)};
|
||||
wheelSpeeds.rearRight = wpi::units::meters_per_second_t{wheelsVector(3)};
|
||||
return wheelSpeeds;
|
||||
}
|
||||
|
||||
@@ -44,9 +44,9 @@ ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
|
||||
Eigen::Vector3d chassisSpeedsVector =
|
||||
m_forwardKinematics.solve(wheelSpeedsVector);
|
||||
|
||||
return {units::meters_per_second_t{chassisSpeedsVector(0)}, // NOLINT
|
||||
units::meters_per_second_t{chassisSpeedsVector(1)},
|
||||
units::radians_per_second_t{chassisSpeedsVector(2)}};
|
||||
return {wpi::units::meters_per_second_t{chassisSpeedsVector(0)}, // NOLINT
|
||||
wpi::units::meters_per_second_t{chassisSpeedsVector(1)},
|
||||
wpi::units::radians_per_second_t{chassisSpeedsVector(2)}};
|
||||
}
|
||||
|
||||
Twist2d MecanumDriveKinematics::ToTwist2d(
|
||||
@@ -60,8 +60,8 @@ Twist2d MecanumDriveKinematics::ToTwist2d(
|
||||
|
||||
Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
|
||||
|
||||
return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
|
||||
units::radian_t{twistVector(2)}};
|
||||
return {wpi::units::meter_t{twistVector(0)}, wpi::units::meter_t{twistVector(1)},
|
||||
wpi::units::radian_t{twistVector(2)}};
|
||||
}
|
||||
|
||||
Twist2d MecanumDriveKinematics::ToTwist2d(
|
||||
@@ -72,8 +72,8 @@ Twist2d MecanumDriveKinematics::ToTwist2d(
|
||||
|
||||
Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
|
||||
|
||||
return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
|
||||
units::radian_t{twistVector(2)}};
|
||||
return {wpi::units::meter_t{twistVector(0)}, wpi::units::meter_t{twistVector(1)},
|
||||
wpi::units::radian_t{twistVector(2)}};
|
||||
}
|
||||
|
||||
void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
|
||||
#include "wpi/math/util/MathShared.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
MecanumDriveOdometry::MecanumDriveOdometry(
|
||||
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
|
||||
#include "wpi/math/util/MathShared.hpp"
|
||||
|
||||
using namespace frc;
|
||||
using namespace wpi::math;
|
||||
|
||||
MecanumDriveOdometry3d::MecanumDriveOdometry3d(
|
||||
MecanumDriveKinematics kinematics, const Rotation3d& gyroAngle,
|
||||
|
||||
@@ -4,9 +4,9 @@
|
||||
|
||||
#include "wpi/math/kinematics/SwerveDriveKinematics.hpp"
|
||||
|
||||
namespace frc {
|
||||
namespace wpi::math {
|
||||
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
SwerveDriveKinematics<4>;
|
||||
|
||||
} // namespace frc
|
||||
} // namespace wpi::math
|
||||
|
||||
@@ -4,8 +4,8 @@
|
||||
|
||||
#include "wpi/math/kinematics/SwerveDriveOdometry.hpp"
|
||||
|
||||
namespace frc {
|
||||
namespace wpi::math {
|
||||
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) SwerveDriveOdometry<4>;
|
||||
|
||||
} // namespace frc
|
||||
} // namespace wpi::math
|
||||
|
||||
@@ -4,9 +4,9 @@
|
||||
|
||||
#include "wpi/math/kinematics/SwerveDriveOdometry3d.hpp"
|
||||
|
||||
namespace frc {
|
||||
namespace wpi::math {
|
||||
|
||||
template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT)
|
||||
SwerveDriveOdometry3d<4>;
|
||||
|
||||
} // namespace frc
|
||||
} // namespace wpi::math
|
||||
|
||||
@@ -6,22 +6,22 @@
|
||||
|
||||
#include "wpimath/protobuf/kinematics.npb.h"
|
||||
|
||||
std::optional<frc::ChassisSpeeds> wpi::Protobuf<frc::ChassisSpeeds>::Unpack(
|
||||
std::optional<wpi::math::ChassisSpeeds> wpi::util::Protobuf<wpi::math::ChassisSpeeds>::Unpack(
|
||||
InputStream& stream) {
|
||||
wpi_proto_ProtobufChassisSpeeds msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::ChassisSpeeds{
|
||||
units::meters_per_second_t{msg.vx},
|
||||
units::meters_per_second_t{msg.vy},
|
||||
units::radians_per_second_t{msg.omega},
|
||||
return wpi::math::ChassisSpeeds{
|
||||
wpi::units::meters_per_second_t{msg.vx},
|
||||
wpi::units::meters_per_second_t{msg.vy},
|
||||
wpi::units::radians_per_second_t{msg.omega},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::ChassisSpeeds>::Pack(OutputStream& stream,
|
||||
const frc::ChassisSpeeds& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::ChassisSpeeds>::Pack(OutputStream& stream,
|
||||
const wpi::math::ChassisSpeeds& value) {
|
||||
wpi_proto_ProtobufChassisSpeeds msg{
|
||||
.vx = value.vx.value(),
|
||||
.vy = value.vy.value(),
|
||||
|
||||
@@ -6,20 +6,20 @@
|
||||
|
||||
#include "wpimath/protobuf/kinematics.npb.h"
|
||||
|
||||
std::optional<frc::DifferentialDriveKinematics>
|
||||
wpi::Protobuf<frc::DifferentialDriveKinematics>::Unpack(InputStream& stream) {
|
||||
std::optional<wpi::math::DifferentialDriveKinematics>
|
||||
wpi::util::Protobuf<wpi::math::DifferentialDriveKinematics>::Unpack(InputStream& stream) {
|
||||
wpi_proto_ProtobufDifferentialDriveKinematics msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::DifferentialDriveKinematics{
|
||||
units::meter_t{msg.trackwidth},
|
||||
return wpi::math::DifferentialDriveKinematics{
|
||||
wpi::units::meter_t{msg.trackwidth},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::DifferentialDriveKinematics>::Pack(
|
||||
OutputStream& stream, const frc::DifferentialDriveKinematics& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::DifferentialDriveKinematics>::Pack(
|
||||
OutputStream& stream, const wpi::math::DifferentialDriveKinematics& value) {
|
||||
wpi_proto_ProtobufDifferentialDriveKinematics msg{
|
||||
.trackwidth = value.trackwidth.value(),
|
||||
};
|
||||
|
||||
@@ -6,21 +6,21 @@
|
||||
|
||||
#include "wpimath/protobuf/kinematics.npb.h"
|
||||
|
||||
std::optional<frc::DifferentialDriveWheelPositions> wpi::Protobuf<
|
||||
frc::DifferentialDriveWheelPositions>::Unpack(InputStream& stream) {
|
||||
std::optional<wpi::math::DifferentialDriveWheelPositions> wpi::util::Protobuf<
|
||||
wpi::math::DifferentialDriveWheelPositions>::Unpack(InputStream& stream) {
|
||||
wpi_proto_ProtobufDifferentialDriveWheelPositions msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::DifferentialDriveWheelPositions{
|
||||
units::meter_t{msg.left},
|
||||
units::meter_t{msg.right},
|
||||
return wpi::math::DifferentialDriveWheelPositions{
|
||||
wpi::units::meter_t{msg.left},
|
||||
wpi::units::meter_t{msg.right},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::DifferentialDriveWheelPositions>::Pack(
|
||||
OutputStream& stream, const frc::DifferentialDriveWheelPositions& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::DifferentialDriveWheelPositions>::Pack(
|
||||
OutputStream& stream, const wpi::math::DifferentialDriveWheelPositions& value) {
|
||||
wpi_proto_ProtobufDifferentialDriveWheelPositions msg{
|
||||
.left = value.left.value(),
|
||||
.right = value.right.value(),
|
||||
|
||||
@@ -6,21 +6,21 @@
|
||||
|
||||
#include "wpimath/protobuf/kinematics.npb.h"
|
||||
|
||||
std::optional<frc::DifferentialDriveWheelSpeeds>
|
||||
wpi::Protobuf<frc::DifferentialDriveWheelSpeeds>::Unpack(InputStream& stream) {
|
||||
std::optional<wpi::math::DifferentialDriveWheelSpeeds>
|
||||
wpi::util::Protobuf<wpi::math::DifferentialDriveWheelSpeeds>::Unpack(InputStream& stream) {
|
||||
wpi_proto_ProtobufDifferentialDriveWheelSpeeds msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::DifferentialDriveWheelSpeeds{
|
||||
units::meters_per_second_t{msg.left},
|
||||
units::meters_per_second_t{msg.right},
|
||||
return wpi::math::DifferentialDriveWheelSpeeds{
|
||||
wpi::units::meters_per_second_t{msg.left},
|
||||
wpi::units::meters_per_second_t{msg.right},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::DifferentialDriveWheelSpeeds>::Pack(
|
||||
OutputStream& stream, const frc::DifferentialDriveWheelSpeeds& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::DifferentialDriveWheelSpeeds>::Pack(
|
||||
OutputStream& stream, const wpi::math::DifferentialDriveWheelSpeeds& value) {
|
||||
wpi_proto_ProtobufDifferentialDriveWheelSpeeds msg{
|
||||
.left = value.left.value(),
|
||||
.right = value.right.value(),
|
||||
|
||||
@@ -7,12 +7,12 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/kinematics.npb.h"
|
||||
|
||||
std::optional<frc::MecanumDriveKinematics>
|
||||
wpi::Protobuf<frc::MecanumDriveKinematics>::Unpack(InputStream& stream) {
|
||||
wpi::UnpackCallback<frc::Translation2d> frontLeft;
|
||||
wpi::UnpackCallback<frc::Translation2d> frontRight;
|
||||
wpi::UnpackCallback<frc::Translation2d> rearLeft;
|
||||
wpi::UnpackCallback<frc::Translation2d> rearRight;
|
||||
std::optional<wpi::math::MecanumDriveKinematics>
|
||||
wpi::util::Protobuf<wpi::math::MecanumDriveKinematics>::Unpack(InputStream& stream) {
|
||||
wpi::util::UnpackCallback<wpi::math::Translation2d> frontLeft;
|
||||
wpi::util::UnpackCallback<wpi::math::Translation2d> frontRight;
|
||||
wpi::util::UnpackCallback<wpi::math::Translation2d> rearLeft;
|
||||
wpi::util::UnpackCallback<wpi::math::Translation2d> rearRight;
|
||||
wpi_proto_ProtobufMecanumDriveKinematics msg{
|
||||
.front_left = frontLeft.Callback(),
|
||||
.front_right = frontRight.Callback(),
|
||||
@@ -33,7 +33,7 @@ wpi::Protobuf<frc::MecanumDriveKinematics>::Unpack(InputStream& stream) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::MecanumDriveKinematics{
|
||||
return wpi::math::MecanumDriveKinematics{
|
||||
ifrontLeft[0],
|
||||
ifrontRight[0],
|
||||
irearLeft[0],
|
||||
@@ -41,12 +41,12 @@ wpi::Protobuf<frc::MecanumDriveKinematics>::Unpack(InputStream& stream) {
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::MecanumDriveKinematics>::Pack(
|
||||
OutputStream& stream, const frc::MecanumDriveKinematics& value) {
|
||||
wpi::PackCallback frontLeft{&value.GetFrontLeft()};
|
||||
wpi::PackCallback frontRight{&value.GetFrontRight()};
|
||||
wpi::PackCallback rearLeft{&value.GetRearLeft()};
|
||||
wpi::PackCallback rearRight{&value.GetRearRight()};
|
||||
bool wpi::util::Protobuf<wpi::math::MecanumDriveKinematics>::Pack(
|
||||
OutputStream& stream, const wpi::math::MecanumDriveKinematics& value) {
|
||||
wpi::util::PackCallback frontLeft{&value.GetFrontLeft()};
|
||||
wpi::util::PackCallback frontRight{&value.GetFrontRight()};
|
||||
wpi::util::PackCallback rearLeft{&value.GetRearLeft()};
|
||||
wpi::util::PackCallback rearRight{&value.GetRearRight()};
|
||||
wpi_proto_ProtobufMecanumDriveKinematics msg{
|
||||
.front_left = frontLeft.Callback(),
|
||||
.front_right = frontRight.Callback(),
|
||||
|
||||
@@ -6,23 +6,23 @@
|
||||
|
||||
#include "wpimath/protobuf/kinematics.npb.h"
|
||||
|
||||
std::optional<frc::MecanumDriveWheelPositions>
|
||||
wpi::Protobuf<frc::MecanumDriveWheelPositions>::Unpack(InputStream& stream) {
|
||||
std::optional<wpi::math::MecanumDriveWheelPositions>
|
||||
wpi::util::Protobuf<wpi::math::MecanumDriveWheelPositions>::Unpack(InputStream& stream) {
|
||||
wpi_proto_ProtobufMecanumDriveWheelPositions msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::MecanumDriveWheelPositions{
|
||||
units::meter_t{msg.front_left},
|
||||
units::meter_t{msg.front_right},
|
||||
units::meter_t{msg.rear_left},
|
||||
units::meter_t{msg.rear_right},
|
||||
return wpi::math::MecanumDriveWheelPositions{
|
||||
wpi::units::meter_t{msg.front_left},
|
||||
wpi::units::meter_t{msg.front_right},
|
||||
wpi::units::meter_t{msg.rear_left},
|
||||
wpi::units::meter_t{msg.rear_right},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::MecanumDriveWheelPositions>::Pack(
|
||||
OutputStream& stream, const frc::MecanumDriveWheelPositions& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::MecanumDriveWheelPositions>::Pack(
|
||||
OutputStream& stream, const wpi::math::MecanumDriveWheelPositions& value) {
|
||||
wpi_proto_ProtobufMecanumDriveWheelPositions msg{
|
||||
.front_left = value.frontLeft.value(),
|
||||
.front_right = value.frontRight.value(),
|
||||
|
||||
@@ -6,23 +6,23 @@
|
||||
|
||||
#include "wpimath/protobuf/kinematics.npb.h"
|
||||
|
||||
std::optional<frc::MecanumDriveWheelSpeeds>
|
||||
wpi::Protobuf<frc::MecanumDriveWheelSpeeds>::Unpack(InputStream& stream) {
|
||||
std::optional<wpi::math::MecanumDriveWheelSpeeds>
|
||||
wpi::util::Protobuf<wpi::math::MecanumDriveWheelSpeeds>::Unpack(InputStream& stream) {
|
||||
wpi_proto_ProtobufMecanumDriveWheelSpeeds msg;
|
||||
if (!stream.Decode(msg)) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::MecanumDriveWheelSpeeds{
|
||||
units::meters_per_second_t{msg.front_left},
|
||||
units::meters_per_second_t{msg.front_right},
|
||||
units::meters_per_second_t{msg.rear_left},
|
||||
units::meters_per_second_t{msg.rear_right},
|
||||
return wpi::math::MecanumDriveWheelSpeeds{
|
||||
wpi::units::meters_per_second_t{msg.front_left},
|
||||
wpi::units::meters_per_second_t{msg.front_right},
|
||||
wpi::units::meters_per_second_t{msg.rear_left},
|
||||
wpi::units::meters_per_second_t{msg.rear_right},
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::MecanumDriveWheelSpeeds>::Pack(
|
||||
OutputStream& stream, const frc::MecanumDriveWheelSpeeds& value) {
|
||||
bool wpi::util::Protobuf<wpi::math::MecanumDriveWheelSpeeds>::Pack(
|
||||
OutputStream& stream, const wpi::math::MecanumDriveWheelSpeeds& value) {
|
||||
wpi_proto_ProtobufMecanumDriveWheelSpeeds msg{
|
||||
.front_left = value.frontLeft.value(),
|
||||
.front_right = value.frontRight.value(),
|
||||
|
||||
@@ -7,9 +7,9 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/kinematics.npb.h"
|
||||
|
||||
std::optional<frc::SwerveModulePosition>
|
||||
wpi::Protobuf<frc::SwerveModulePosition>::Unpack(InputStream& stream) {
|
||||
wpi::UnpackCallback<frc::Rotation2d> angle;
|
||||
std::optional<wpi::math::SwerveModulePosition>
|
||||
wpi::util::Protobuf<wpi::math::SwerveModulePosition>::Unpack(InputStream& stream) {
|
||||
wpi::util::UnpackCallback<wpi::math::Rotation2d> angle;
|
||||
wpi_proto_ProtobufSwerveModulePosition msg{
|
||||
.distance = 0,
|
||||
.angle = angle.Callback(),
|
||||
@@ -24,15 +24,15 @@ wpi::Protobuf<frc::SwerveModulePosition>::Unpack(InputStream& stream) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::SwerveModulePosition{
|
||||
units::meter_t{msg.distance},
|
||||
return wpi::math::SwerveModulePosition{
|
||||
wpi::units::meter_t{msg.distance},
|
||||
iangle[0],
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::SwerveModulePosition>::Pack(
|
||||
OutputStream& stream, const frc::SwerveModulePosition& value) {
|
||||
wpi::PackCallback angle{&value.angle};
|
||||
bool wpi::util::Protobuf<wpi::math::SwerveModulePosition>::Pack(
|
||||
OutputStream& stream, const wpi::math::SwerveModulePosition& value) {
|
||||
wpi::util::PackCallback angle{&value.angle};
|
||||
wpi_proto_ProtobufSwerveModulePosition msg{
|
||||
.distance = value.distance.value(),
|
||||
.angle = angle.Callback(),
|
||||
|
||||
@@ -7,9 +7,9 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/kinematics.npb.h"
|
||||
|
||||
std::optional<frc::SwerveModuleState>
|
||||
wpi::Protobuf<frc::SwerveModuleState>::Unpack(InputStream& stream) {
|
||||
wpi::UnpackCallback<frc::Rotation2d> angle;
|
||||
std::optional<wpi::math::SwerveModuleState>
|
||||
wpi::util::Protobuf<wpi::math::SwerveModuleState>::Unpack(InputStream& stream) {
|
||||
wpi::util::UnpackCallback<wpi::math::Rotation2d> angle;
|
||||
wpi_proto_ProtobufSwerveModuleState msg{
|
||||
.speed = 0,
|
||||
.angle = angle.Callback(),
|
||||
@@ -24,15 +24,15 @@ wpi::Protobuf<frc::SwerveModuleState>::Unpack(InputStream& stream) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::SwerveModuleState{
|
||||
units::meters_per_second_t{msg.speed},
|
||||
return wpi::math::SwerveModuleState{
|
||||
wpi::units::meters_per_second_t{msg.speed},
|
||||
iangle[0],
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::SwerveModuleState>::Pack(
|
||||
OutputStream& stream, const frc::SwerveModuleState& value) {
|
||||
wpi::PackCallback angle{&value.angle};
|
||||
bool wpi::util::Protobuf<wpi::math::SwerveModuleState>::Pack(
|
||||
OutputStream& stream, const wpi::math::SwerveModuleState& value) {
|
||||
wpi::util::PackCallback angle{&value.angle};
|
||||
wpi_proto_ProtobufSwerveModuleState msg{
|
||||
.speed = value.speed.value(),
|
||||
.angle = angle.Callback(),
|
||||
|
||||
@@ -10,19 +10,19 @@ constexpr size_t kVyOff = kVxOff + 8;
|
||||
constexpr size_t kOmegaOff = kVyOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::ChassisSpeeds>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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)},
|
||||
wpi::math::ChassisSpeeds StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::ChassisSpeeds{
|
||||
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kVxOff>(data)},
|
||||
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kVyOff>(data)},
|
||||
wpi::units::radians_per_second_t{wpi::util::UnpackStruct<double, kOmegaOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
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());
|
||||
const wpi::math::ChassisSpeeds& value) {
|
||||
wpi::util::PackStruct<kVxOff>(data, value.vx.value());
|
||||
wpi::util::PackStruct<kVyOff>(data, value.vy.value());
|
||||
wpi::util::PackStruct<kOmegaOff>(data, value.omega.value());
|
||||
}
|
||||
|
||||
@@ -8,16 +8,16 @@ namespace {
|
||||
constexpr size_t kTrackwidthOff = 0;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::DifferentialDriveKinematics>;
|
||||
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveKinematics>;
|
||||
|
||||
frc::DifferentialDriveKinematics StructType::Unpack(
|
||||
wpi::math::DifferentialDriveKinematics StructType::Unpack(
|
||||
std::span<const uint8_t> data) {
|
||||
return frc::DifferentialDriveKinematics{
|
||||
units::meter_t{wpi::UnpackStruct<double, kTrackwidthOff>(data)},
|
||||
return wpi::math::DifferentialDriveKinematics{
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kTrackwidthOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
void StructType::Pack(std::span<uint8_t> data,
|
||||
const frc::DifferentialDriveKinematics& value) {
|
||||
wpi::PackStruct<kTrackwidthOff>(data, value.trackwidth.value());
|
||||
const wpi::math::DifferentialDriveKinematics& value) {
|
||||
wpi::util::PackStruct<kTrackwidthOff>(data, value.trackwidth.value());
|
||||
}
|
||||
|
||||
@@ -9,18 +9,18 @@ constexpr size_t kLeftOff = 0;
|
||||
constexpr size_t kRightOff = kLeftOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::DifferentialDriveWheelPositions>;
|
||||
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelPositions>;
|
||||
|
||||
frc::DifferentialDriveWheelPositions StructType::Unpack(
|
||||
wpi::math::DifferentialDriveWheelPositions StructType::Unpack(
|
||||
std::span<const uint8_t> data) {
|
||||
return frc::DifferentialDriveWheelPositions{
|
||||
units::meter_t{wpi::UnpackStruct<double, kLeftOff>(data)},
|
||||
units::meter_t{wpi::UnpackStruct<double, kRightOff>(data)},
|
||||
return wpi::math::DifferentialDriveWheelPositions{
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kLeftOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kRightOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
void StructType::Pack(std::span<uint8_t> data,
|
||||
const frc::DifferentialDriveWheelPositions& value) {
|
||||
wpi::PackStruct<kLeftOff>(data, value.left.value());
|
||||
wpi::PackStruct<kRightOff>(data, value.right.value());
|
||||
const wpi::math::DifferentialDriveWheelPositions& value) {
|
||||
wpi::util::PackStruct<kLeftOff>(data, value.left.value());
|
||||
wpi::util::PackStruct<kRightOff>(data, value.right.value());
|
||||
}
|
||||
|
||||
@@ -9,18 +9,18 @@ constexpr size_t kLeftOff = 0;
|
||||
constexpr size_t kRightOff = kLeftOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::DifferentialDriveWheelSpeeds>;
|
||||
using StructType = wpi::util::Struct<wpi::math::DifferentialDriveWheelSpeeds>;
|
||||
|
||||
frc::DifferentialDriveWheelSpeeds StructType::Unpack(
|
||||
wpi::math::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)},
|
||||
return wpi::math::DifferentialDriveWheelSpeeds{
|
||||
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kLeftOff>(data)},
|
||||
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kRightOff>(data)},
|
||||
};
|
||||
}
|
||||
|
||||
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());
|
||||
const wpi::math::DifferentialDriveWheelSpeeds& value) {
|
||||
wpi::util::PackStruct<kLeftOff>(data, value.left.value());
|
||||
wpi::util::PackStruct<kRightOff>(data, value.right.value());
|
||||
}
|
||||
|
||||
@@ -7,28 +7,28 @@
|
||||
namespace {
|
||||
constexpr size_t kFrontLeftOff = 0;
|
||||
constexpr size_t kFrontRightOff =
|
||||
kFrontLeftOff + wpi::GetStructSize<frc::Translation2d>();
|
||||
kFrontLeftOff + wpi::util::GetStructSize<wpi::math::Translation2d>();
|
||||
constexpr size_t kRearLeftOff =
|
||||
kFrontRightOff + wpi::GetStructSize<frc::Translation2d>();
|
||||
kFrontRightOff + wpi::util::GetStructSize<wpi::math::Translation2d>();
|
||||
constexpr size_t kRearRightOff =
|
||||
kRearLeftOff + wpi::GetStructSize<frc::Translation2d>();
|
||||
kRearLeftOff + wpi::util::GetStructSize<wpi::math::Translation2d>();
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::MecanumDriveKinematics>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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),
|
||||
wpi::math::MecanumDriveKinematics StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::MecanumDriveKinematics{
|
||||
wpi::util::UnpackStruct<wpi::math::Translation2d, kFrontLeftOff>(data),
|
||||
wpi::util::UnpackStruct<wpi::math::Translation2d, kFrontRightOff>(data),
|
||||
wpi::util::UnpackStruct<wpi::math::Translation2d, kRearLeftOff>(data),
|
||||
wpi::util::UnpackStruct<wpi::math::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());
|
||||
const wpi::math::MecanumDriveKinematics& value) {
|
||||
wpi::util::PackStruct<kFrontLeftOff>(data, value.GetFrontLeft());
|
||||
wpi::util::PackStruct<kFrontRightOff>(data, value.GetFrontRight());
|
||||
wpi::util::PackStruct<kRearLeftOff>(data, value.GetRearLeft());
|
||||
wpi::util::PackStruct<kRearRightOff>(data, value.GetRearRight());
|
||||
}
|
||||
|
||||
@@ -11,22 +11,22 @@ constexpr size_t kRearLeftOff = kFrontRightOff + 8;
|
||||
constexpr size_t kRearRightOff = kRearLeftOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::MecanumDriveWheelPositions>;
|
||||
using StructType = wpi::util::Struct<wpi::math::MecanumDriveWheelPositions>;
|
||||
|
||||
frc::MecanumDriveWheelPositions StructType::Unpack(
|
||||
wpi::math::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)},
|
||||
return wpi::math::MecanumDriveWheelPositions{
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kFrontLeftOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kFrontRightOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kRearLeftOff>(data)},
|
||||
wpi::units::meter_t{wpi::util::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());
|
||||
const wpi::math::MecanumDriveWheelPositions& value) {
|
||||
wpi::util::PackStruct<kFrontLeftOff>(data, value.frontLeft.value());
|
||||
wpi::util::PackStruct<kFrontRightOff>(data, value.frontRight.value());
|
||||
wpi::util::PackStruct<kRearLeftOff>(data, value.rearLeft.value());
|
||||
wpi::util::PackStruct<kRearRightOff>(data, value.rearRight.value());
|
||||
}
|
||||
|
||||
@@ -11,24 +11,24 @@ constexpr size_t kRearLeftOff = kFrontRightOff + 8;
|
||||
constexpr size_t kRearRightOff = kRearLeftOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::MecanumDriveWheelSpeeds>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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)},
|
||||
wpi::math::MecanumDriveWheelSpeeds StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::MecanumDriveWheelSpeeds{
|
||||
wpi::units::meters_per_second_t{
|
||||
wpi::util::UnpackStruct<double, kFrontLeftOff>(data)},
|
||||
wpi::units::meters_per_second_t{
|
||||
wpi::util::UnpackStruct<double, kFrontRightOff>(data)},
|
||||
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kRearLeftOff>(data)},
|
||||
wpi::units::meters_per_second_t{
|
||||
wpi::util::UnpackStruct<double, 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());
|
||||
const wpi::math::MecanumDriveWheelSpeeds& value) {
|
||||
wpi::util::PackStruct<kFrontLeftOff>(data, value.frontLeft.value());
|
||||
wpi::util::PackStruct<kFrontRightOff>(data, value.frontRight.value());
|
||||
wpi::util::PackStruct<kRearLeftOff>(data, value.rearLeft.value());
|
||||
wpi::util::PackStruct<kRearRightOff>(data, value.rearRight.value());
|
||||
}
|
||||
|
||||
@@ -9,17 +9,17 @@ constexpr size_t kDistanceOff = 0;
|
||||
constexpr size_t kAngleOff = kDistanceOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::SwerveModulePosition>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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),
|
||||
wpi::math::SwerveModulePosition StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::SwerveModulePosition{
|
||||
wpi::units::meter_t{wpi::util::UnpackStruct<double, kDistanceOff>(data)},
|
||||
wpi::util::UnpackStruct<wpi::math::Rotation2d, kAngleOff>(data),
|
||||
};
|
||||
}
|
||||
|
||||
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);
|
||||
const wpi::math::SwerveModulePosition& value) {
|
||||
wpi::util::PackStruct<kDistanceOff>(data, value.distance.value());
|
||||
wpi::util::PackStruct<kAngleOff>(data, value.angle);
|
||||
}
|
||||
|
||||
@@ -9,17 +9,17 @@ constexpr size_t kSpeedOff = 0;
|
||||
constexpr size_t kAngleOff = kSpeedOff + 8;
|
||||
} // namespace
|
||||
|
||||
using StructType = wpi::Struct<frc::SwerveModuleState>;
|
||||
using StructType = wpi::util::Struct<wpi::math::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),
|
||||
wpi::math::SwerveModuleState StructType::Unpack(std::span<const uint8_t> data) {
|
||||
return wpi::math::SwerveModuleState{
|
||||
wpi::units::meters_per_second_t{wpi::util::UnpackStruct<double, kSpeedOff>(data)},
|
||||
wpi::util::UnpackStruct<wpi::math::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);
|
||||
const wpi::math::SwerveModuleState& value) {
|
||||
wpi::util::PackStruct<kSpeedOff>(data, value.speed.value());
|
||||
wpi::util::PackStruct<kAngleOff>(data, value.angle);
|
||||
}
|
||||
|
||||
@@ -7,12 +7,12 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/spline.npb.h"
|
||||
|
||||
std::optional<frc::CubicHermiteSpline>
|
||||
wpi::Protobuf<frc::CubicHermiteSpline>::Unpack(InputStream& stream) {
|
||||
wpi::WpiArrayUnpackCallback<double, 2> xInitial;
|
||||
wpi::WpiArrayUnpackCallback<double, 2> xFinal;
|
||||
wpi::WpiArrayUnpackCallback<double, 2> yInitial;
|
||||
wpi::WpiArrayUnpackCallback<double, 2> yFinal;
|
||||
std::optional<wpi::math::CubicHermiteSpline>
|
||||
wpi::util::Protobuf<wpi::math::CubicHermiteSpline>::Unpack(InputStream& stream) {
|
||||
wpi::util::WpiArrayUnpackCallback<double, 2> xInitial;
|
||||
wpi::util::WpiArrayUnpackCallback<double, 2> xFinal;
|
||||
wpi::util::WpiArrayUnpackCallback<double, 2> yInitial;
|
||||
wpi::util::WpiArrayUnpackCallback<double, 2> yFinal;
|
||||
wpi_proto_ProtobufCubicHermiteSpline msg{
|
||||
.x_initial = xInitial.Callback(),
|
||||
.x_final = xFinal.Callback(),
|
||||
@@ -28,7 +28,7 @@ wpi::Protobuf<frc::CubicHermiteSpline>::Unpack(InputStream& stream) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::CubicHermiteSpline{
|
||||
return wpi::math::CubicHermiteSpline{
|
||||
xInitial.Array(),
|
||||
xFinal.Array(),
|
||||
yInitial.Array(),
|
||||
@@ -36,12 +36,12 @@ wpi::Protobuf<frc::CubicHermiteSpline>::Unpack(InputStream& stream) {
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::CubicHermiteSpline>::Pack(
|
||||
OutputStream& stream, const frc::CubicHermiteSpline& value) {
|
||||
wpi::PackCallback<double> xInitial{value.GetInitialControlVector().x};
|
||||
wpi::PackCallback<double> xFinal{value.GetFinalControlVector().x};
|
||||
wpi::PackCallback<double> yInitial{value.GetInitialControlVector().y};
|
||||
wpi::PackCallback<double> yFinal{value.GetFinalControlVector().y};
|
||||
bool wpi::util::Protobuf<wpi::math::CubicHermiteSpline>::Pack(
|
||||
OutputStream& stream, const wpi::math::CubicHermiteSpline& value) {
|
||||
wpi::util::PackCallback<double> xInitial{value.GetInitialControlVector().x};
|
||||
wpi::util::PackCallback<double> xFinal{value.GetFinalControlVector().x};
|
||||
wpi::util::PackCallback<double> yInitial{value.GetInitialControlVector().y};
|
||||
wpi::util::PackCallback<double> yFinal{value.GetFinalControlVector().y};
|
||||
wpi_proto_ProtobufCubicHermiteSpline msg{
|
||||
.x_initial = xInitial.Callback(),
|
||||
.x_final = xFinal.Callback(),
|
||||
|
||||
@@ -7,12 +7,12 @@
|
||||
#include "wpi/util/protobuf/ProtobufCallbacks.hpp"
|
||||
#include "wpimath/protobuf/spline.npb.h"
|
||||
|
||||
std::optional<frc::QuinticHermiteSpline>
|
||||
wpi::Protobuf<frc::QuinticHermiteSpline>::Unpack(InputStream& stream) {
|
||||
wpi::WpiArrayUnpackCallback<double, 3> xInitial;
|
||||
wpi::WpiArrayUnpackCallback<double, 3> xFinal;
|
||||
wpi::WpiArrayUnpackCallback<double, 3> yInitial;
|
||||
wpi::WpiArrayUnpackCallback<double, 3> yFinal;
|
||||
std::optional<wpi::math::QuinticHermiteSpline>
|
||||
wpi::util::Protobuf<wpi::math::QuinticHermiteSpline>::Unpack(InputStream& stream) {
|
||||
wpi::util::WpiArrayUnpackCallback<double, 3> xInitial;
|
||||
wpi::util::WpiArrayUnpackCallback<double, 3> xFinal;
|
||||
wpi::util::WpiArrayUnpackCallback<double, 3> yInitial;
|
||||
wpi::util::WpiArrayUnpackCallback<double, 3> yFinal;
|
||||
wpi_proto_ProtobufQuinticHermiteSpline msg{
|
||||
.x_initial = xInitial.Callback(),
|
||||
.x_final = xFinal.Callback(),
|
||||
@@ -28,7 +28,7 @@ wpi::Protobuf<frc::QuinticHermiteSpline>::Unpack(InputStream& stream) {
|
||||
return {};
|
||||
}
|
||||
|
||||
return frc::QuinticHermiteSpline{
|
||||
return wpi::math::QuinticHermiteSpline{
|
||||
xInitial.Array(),
|
||||
xFinal.Array(),
|
||||
yInitial.Array(),
|
||||
@@ -36,12 +36,12 @@ wpi::Protobuf<frc::QuinticHermiteSpline>::Unpack(InputStream& stream) {
|
||||
};
|
||||
}
|
||||
|
||||
bool wpi::Protobuf<frc::QuinticHermiteSpline>::Pack(
|
||||
OutputStream& stream, const frc::QuinticHermiteSpline& value) {
|
||||
wpi::PackCallback<double> xInitial{value.GetInitialControlVector().x};
|
||||
wpi::PackCallback<double> xFinal{value.GetFinalControlVector().x};
|
||||
wpi::PackCallback<double> yInitial{value.GetInitialControlVector().y};
|
||||
wpi::PackCallback<double> yFinal{value.GetFinalControlVector().y};
|
||||
bool wpi::util::Protobuf<wpi::math::QuinticHermiteSpline>::Pack(
|
||||
OutputStream& stream, const wpi::math::QuinticHermiteSpline& value) {
|
||||
wpi::util::PackCallback<double> xInitial{value.GetInitialControlVector().x};
|
||||
wpi::util::PackCallback<double> xFinal{value.GetFinalControlVector().x};
|
||||
wpi::util::PackCallback<double> yInitial{value.GetInitialControlVector().y};
|
||||
wpi::util::PackCallback<double> yFinal{value.GetFinalControlVector().y};
|
||||
wpi_proto_ProtobufQuinticHermiteSpline msg{
|
||||
.x_initial = xInitial.Callback(),
|
||||
.x_final = xFinal.Callback(),
|
||||
|
||||
@@ -11,21 +11,21 @@ constexpr size_t kYInitialOff = kXFinalOff + 2 * 8;
|
||||
constexpr size_t kYFinalOff = kYInitialOff + 2 * 8;
|
||||
} // namespace
|
||||
|
||||
frc::CubicHermiteSpline wpi::Struct<frc::CubicHermiteSpline>::Unpack(
|
||||
wpi::math::CubicHermiteSpline wpi::util::Struct<wpi::math::CubicHermiteSpline>::Unpack(
|
||||
std::span<const uint8_t> data) {
|
||||
return frc::CubicHermiteSpline{
|
||||
wpi::UnpackStructArray<double, kXInitialOff, 2>(data),
|
||||
wpi::UnpackStructArray<double, kXFinalOff, 2>(data),
|
||||
wpi::UnpackStructArray<double, kYInitialOff, 2>(data),
|
||||
wpi::UnpackStructArray<double, kYFinalOff, 2>(data)};
|
||||
return wpi::math::CubicHermiteSpline{
|
||||
wpi::util::UnpackStructArray<double, kXInitialOff, 2>(data),
|
||||
wpi::util::UnpackStructArray<double, kXFinalOff, 2>(data),
|
||||
wpi::util::UnpackStructArray<double, kYInitialOff, 2>(data),
|
||||
wpi::util::UnpackStructArray<double, kYFinalOff, 2>(data)};
|
||||
}
|
||||
|
||||
void wpi::Struct<frc::CubicHermiteSpline>::Pack(
|
||||
std::span<uint8_t> data, const frc::CubicHermiteSpline& value) {
|
||||
wpi::PackStructArray<kXInitialOff, 2>(data,
|
||||
void wpi::util::Struct<wpi::math::CubicHermiteSpline>::Pack(
|
||||
std::span<uint8_t> data, const wpi::math::CubicHermiteSpline& value) {
|
||||
wpi::util::PackStructArray<kXInitialOff, 2>(data,
|
||||
value.GetInitialControlVector().x);
|
||||
wpi::PackStructArray<kXFinalOff, 2>(data, value.GetFinalControlVector().x);
|
||||
wpi::PackStructArray<kYInitialOff, 2>(data,
|
||||
wpi::util::PackStructArray<kXFinalOff, 2>(data, value.GetFinalControlVector().x);
|
||||
wpi::util::PackStructArray<kYInitialOff, 2>(data,
|
||||
value.GetInitialControlVector().y);
|
||||
wpi::PackStructArray<kYFinalOff, 2>(data, value.GetFinalControlVector().y);
|
||||
wpi::util::PackStructArray<kYFinalOff, 2>(data, value.GetFinalControlVector().y);
|
||||
}
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user