Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add remaining struct and protobuf implementations (#5953)

Browse Source
This commit is contained in:
Joseph Eng
2024-07-29 07:55:44 -07:00
committed by GitHub
parent 3e1e3fb4ca
commit 073192d513
112 changed files with 3989 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -15,14 +15,22 @@ DifferentialDriveFeedforward::DifferentialDriveFeedforward(
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_rad_per_s) kVAngular,
decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
: m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
kVLinear, kALinear, kVAngular, kAAngular, trackwidth)} {}
// See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps),
// decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V /
// 1_rad_per_s_sq))
: DifferentialDriveFeedforward{kVLinear, kALinear,
kVAngular * 2.0 / trackwidth * 1_rad,
kAAngular * 2.0 / trackwidth * 1_rad} {}
DifferentialDriveFeedforward::DifferentialDriveFeedforward(
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular)
: m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
kVLinear, kALinear, kVAngular, kAAngular)} {}
kVLinear, kALinear, kVAngular, kAAngular)},
m_kVLinear{kVLinear},
m_kALinear{kALinear},
m_kVAngular{kVAngular},
m_kAAngular{kAAngular} {}
DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate(
units::meters_per_second_t currentLeftVelocity,

36
wpimath/src/main/native/cpp/controller/proto/DifferentialDriveFeedforwardProto.cpp Normal file
View File

@@ -0,0 +1,36 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/controller/proto/DifferentialDriveFeedforwardProto.h"
#include <wpi/ProtoHelper.h>
#include "controller.pb.h"
google::protobuf::Message* wpi::Protobuf<
frc::DifferentialDriveFeedforward>::New(google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufDifferentialDriveFeedforward>(
arena);
}
frc::DifferentialDriveFeedforward
wpi::Protobuf<frc::DifferentialDriveFeedforward>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufDifferentialDriveFeedforward*>(
&msg);
return {decltype(1_V / 1_mps){m->kv_linear()},
decltype(1_V / 1_mps_sq){m->ka_linear()},
decltype(1_V / 1_mps){m->kv_angular()},
decltype(1_V / 1_mps_sq){m->ka_angular()}};
}
void wpi::Protobuf<frc::DifferentialDriveFeedforward>::Pack(
google::protobuf::Message* msg,
const frc::DifferentialDriveFeedforward& value) {
auto m = static_cast<wpi::proto::ProtobufDifferentialDriveFeedforward*>(msg);
m->set_kv_linear(value.m_kVLinear.value());
m->set_ka_linear(value.m_kALinear.value());
m->set_kv_angular(value.m_kVAngular.value());
m->set_ka_angular(value.m_kAAngular.value());
}

29
wpimath/src/main/native/cpp/controller/struct/DifferentialDriveFeedforwardStruct.cpp Normal file
View File

@@ -0,0 +1,29 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/controller/struct/DifferentialDriveFeedforwardStruct.h"
namespace {
constexpr size_t kKvLinearOff = 0;
constexpr size_t kKaLinearOff = kKvLinearOff + 8;
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) {
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)}};
}
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());
}

35
wpimath/src/main/native/cpp/spline/proto/CubicHermiteSplineProto.cpp Normal file
View File

@@ -0,0 +1,35 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/spline/proto/CubicHermiteSplineProto.h"
#include <wpi/ProtoHelper.h>
#include "spline.pb.h"
google::protobuf::Message* wpi::Protobuf<frc::CubicHermiteSpline>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufCubicHermiteSpline>(arena);
}
frc::CubicHermiteSpline wpi::Protobuf<frc::CubicHermiteSpline>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufCubicHermiteSpline*>(&msg);
return frc::CubicHermiteSpline{
wpi::UnpackProtobufArray<double, 2>(m->x_initial()),
wpi::UnpackProtobufArray<double, 2>(m->x_final()),
wpi::UnpackProtobufArray<double, 2>(m->y_initial()),
wpi::UnpackProtobufArray<double, 2>(m->y_final())};
}
void wpi::Protobuf<frc::CubicHermiteSpline>::Pack(
google::protobuf::Message* msg, const frc::CubicHermiteSpline& value) {
auto m = static_cast<wpi::proto::ProtobufCubicHermiteSpline*>(msg);
wpi::PackProtobufArray(m->mutable_x_initial(),
value.GetInitialControlVector().x);
wpi::PackProtobufArray(m->mutable_x_final(), value.GetFinalControlVector().x);
wpi::PackProtobufArray(m->mutable_y_initial(),
value.GetInitialControlVector().y);
wpi::PackProtobufArray(m->mutable_y_final(), value.GetFinalControlVector().y);
}

35
wpimath/src/main/native/cpp/spline/proto/QuinticHermiteSplineProto.cpp Normal file
View File

@@ -0,0 +1,35 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/spline/proto/QuinticHermiteSplineProto.h"
#include <wpi/ProtoHelper.h>
#include "spline.pb.h"
google::protobuf::Message* wpi::Protobuf<frc::QuinticHermiteSpline>::New(
google::protobuf::Arena* arena) {
return wpi::CreateMessage<wpi::proto::ProtobufQuinticHermiteSpline>(arena);
}
frc::QuinticHermiteSpline wpi::Protobuf<frc::QuinticHermiteSpline>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const wpi::proto::ProtobufQuinticHermiteSpline*>(&msg);
return frc::QuinticHermiteSpline{
wpi::UnpackProtobufArray<double, 3>(m->x_initial()),
wpi::UnpackProtobufArray<double, 3>(m->x_final()),
wpi::UnpackProtobufArray<double, 3>(m->y_initial()),
wpi::UnpackProtobufArray<double, 3>(m->y_final())};
}
void wpi::Protobuf<frc::QuinticHermiteSpline>::Pack(
google::protobuf::Message* msg, const frc::QuinticHermiteSpline& value) {
auto m = static_cast<wpi::proto::ProtobufQuinticHermiteSpline*>(msg);
wpi::PackProtobufArray(m->mutable_x_initial(),
value.GetInitialControlVector().x);
wpi::PackProtobufArray(m->mutable_x_final(), value.GetFinalControlVector().x);
wpi::PackProtobufArray(m->mutable_y_initial(),
value.GetInitialControlVector().y);
wpi::PackProtobufArray(m->mutable_y_final(), value.GetFinalControlVector().y);
}

31
wpimath/src/main/native/cpp/spline/struct/CubicHermiteSplineStruct.cpp Normal file
View File

@@ -0,0 +1,31 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/spline/struct/CubicHermiteSplineStruct.h"
namespace {
constexpr size_t kXInitialOff = 0;
constexpr size_t kXFinalOff = kXInitialOff + 2 * 8;
constexpr size_t kYInitialOff = kXFinalOff + 2 * 8;
constexpr size_t kYFinalOff = kYInitialOff + 2 * 8;
} // namespace
frc::CubicHermiteSpline wpi::Struct<frc::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)};
}
void wpi::Struct<frc::CubicHermiteSpline>::Pack(
std::span<uint8_t> data, const frc::CubicHermiteSpline& value) {
wpi::PackStructArray<kXInitialOff, 2>(data,
value.GetInitialControlVector().x);
wpi::PackStructArray<kXFinalOff, 2>(data, value.GetFinalControlVector().x);
wpi::PackStructArray<kYInitialOff, 2>(data,
value.GetInitialControlVector().y);
wpi::PackStructArray<kYFinalOff, 2>(data, value.GetFinalControlVector().y);
}

31
wpimath/src/main/native/cpp/spline/struct/QuinticHermiteSplineStruct.cpp Normal file
View File

@@ -0,0 +1,31 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "frc/spline/struct/QuinticHermiteSplineStruct.h"
namespace {
constexpr size_t kXInitialOff = 0;
constexpr size_t kXFinalOff = kXInitialOff + 3 * 8;
constexpr size_t kYInitialOff = kXFinalOff + 3 * 8;
constexpr size_t kYFinalOff = kYInitialOff + 3 * 8;
} // namespace
frc::QuinticHermiteSpline wpi::Struct<frc::QuinticHermiteSpline>::Unpack(
std::span<const uint8_t> data) {
return frc::QuinticHermiteSpline{
wpi::UnpackStructArray<double, kXInitialOff, 3>(data),
wpi::UnpackStructArray<double, kXFinalOff, 3>(data),
wpi::UnpackStructArray<double, kYInitialOff, 3>(data),
wpi::UnpackStructArray<double, kYFinalOff, 3>(data)};
}
void wpi::Struct<frc::QuinticHermiteSpline>::Pack(
std::span<uint8_t> data, const frc::QuinticHermiteSpline& value) {
wpi::PackStructArray<kXInitialOff, 3>(data,
value.GetInitialControlVector().x);
wpi::PackStructArray<kXFinalOff, 3>(data, value.GetFinalControlVector().x);
wpi::PackStructArray<kYInitialOff, 3>(data,
value.GetInitialControlVector().y);
wpi::PackStructArray<kYFinalOff, 3>(data, value.GetFinalControlVector().y);
}