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

Merge branch 'main' into 2022

Browse Source
This commit is contained in:
Peter Johnson
2021-04-19 18:45:31 -07:00
parent 8d961dfd25 aaf24e2552
commit 05ebe93180
44 changed files with 1762 additions and 702 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp
View File

@@ -4,6 +4,13 @@
#include "frc/smartdashboard/FieldObject2d.h"
#include <vector>
#include <wpi/Endian.h>
#include <wpi/MathExtras.h>
#include "frc/trajectory/Trajectory.h"
using namespace frc;
FieldObject2d::FieldObject2d(FieldObject2d&& rhs) {
@@ -48,6 +55,16 @@ void FieldObject2d::SetPoses(std::initializer_list<Pose2d> poses) {
SetPoses(wpi::makeArrayRef(poses.begin(), poses.end()));
}
void FieldObject2d::SetTrajectory(const Trajectory& trajectory) {
std::scoped_lock lock(m_mutex);
m_poses.clear();
m_poses.reserve(trajectory.States().size());
for (auto&& state : trajectory.States()) {
m_poses.push_back(state.pose);
}
UpdateEntry();
}
std::vector<Pose2d> FieldObject2d::GetPoses() const {
std::scoped_lock lock(m_mutex);
UpdateFromEntry();
@@ -66,17 +83,41 @@ void FieldObject2d::UpdateEntry(bool setDefault) {
if (!m_entry) {
return;
}
wpi::SmallVector<double, 9> arr;
for (auto&& pose : m_poses) {
auto& translation = pose.Translation();
arr.push_back(translation.X().to<double>());
arr.push_back(translation.Y().to<double>());
arr.push_back(pose.Rotation().Degrees().to<double>());
}
if (setDefault) {
m_entry.SetDefaultDoubleArray(arr);
if (m_poses.size() < (255 / 3)) {
wpi::SmallVector<double, 9> arr;
for (auto&& pose : m_poses) {
auto& translation = pose.Translation();
arr.push_back(translation.X().to<double>());
arr.push_back(translation.Y().to<double>());
arr.push_back(pose.Rotation().Degrees().to<double>());
}
if (setDefault) {
m_entry.SetDefaultDoubleArray(arr);
} else {
m_entry.ForceSetDoubleArray(arr);
}
} else {
m_entry.SetDoubleArray(arr);
// send as raw array of doubles if too big for NT array
std::vector<char> arr;
arr.resize(m_poses.size() * 3 * 8);
char* p = arr.data();
for (auto&& pose : m_poses) {
auto& translation = pose.Translation();
wpi::support::endian::write64be(
p, wpi::DoubleToBits(translation.X().to<double>()));
p += 8;
wpi::support::endian::write64be(
p, wpi::DoubleToBits(translation.Y().to<double>()));
p += 8;
wpi::support::endian::write64be(
p, wpi::DoubleToBits(pose.Rotation().Degrees().to<double>()));
p += 8;
}
if (setDefault) {
m_entry.SetDefaultRaw(wpi::StringRef{arr.data(), arr.size()});
} else {
m_entry.ForceSetRaw(wpi::StringRef{arr.data(), arr.size()});
}
}
}
@@ -85,18 +126,45 @@ void FieldObject2d::UpdateFromEntry() const {
return;
}
auto val = m_entry.GetValue();
if (!val || !val->IsDoubleArray()) {
if (!val) {
return;
}
auto arr = val->GetDoubleArray();
auto size = arr.size();
if ((size % 3) != 0) {
return;
}
m_poses.resize(size / 3);
for (size_t i = 0; i < size / 3; ++i) {
m_poses[i] =
Pose2d{units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
if (val->IsDoubleArray()) {
auto arr = val->GetDoubleArray();
auto size = arr.size();
if ((size % 3) != 0) {
return;
}
m_poses.resize(size / 3);
for (size_t i = 0; i < size / 3; ++i) {
m_poses[i] =
Pose2d{units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
}
} else if (val->IsRaw()) {
// treat it simply as an array of doubles
wpi::StringRef data = val->GetRaw();
// must be triples of doubles
auto size = data.size();
if ((size % (3 * 8)) != 0) {
return;
}
m_poses.resize(size / (3 * 8));
const char* p = data.begin();
for (size_t i = 0; i < size / (3 * 8); ++i) {
double x = wpi::BitsToDouble(
wpi::support::endian::readNext<uint64_t, wpi::support::big,
wpi::support::unaligned>(p));
double y = wpi::BitsToDouble(
wpi::support::endian::readNext<uint64_t, wpi::support::big,
wpi::support::unaligned>(p));
double rot = wpi::BitsToDouble(
wpi::support::endian::readNext<uint64_t, wpi::support::big,
wpi::support::unaligned>(p));
m_poses[i] = Pose2d{units::meter_t{x}, units::meter_t{y},
Rotation2d{units::degree_t{rot}}};
}
}
}