Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +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

1
glass/.styleguide
View File

@@ -21,6 +21,7 @@ repoRootNameOverride {
includeOtherLibs {
^GLFW
^cscore
^frc/
^imgui
^ntcore
^wpi/

1055
glass/src/lib/native/cpp/other/Field2D.cpp
View File

File diff suppressed because it is too large Load Diff

11
glass/src/lib/native/cpp/other/Log.cpp
View File

@@ -23,7 +23,7 @@ void LogData::Append(const wpi::Twine& msg) {
}
size_t oldSize = m_buf.size();
wpi::raw_vector_ostream os{m_buf};
wpi::raw_string_ostream os{m_buf};
msg.print(os);
for (size_t newSize = m_buf.size(); oldSize < newSize; ++oldSize) {
if (m_buf[oldSize] == '\n') {
@@ -32,6 +32,10 @@ void LogData::Append(const wpi::Twine& msg) {
}
}
const std::string& LogData::GetBuffer() {
return m_buf;
}
void glass::DisplayLog(LogData* data, bool autoScroll) {
if (data->m_buf.empty()) {
return;
@@ -65,6 +69,11 @@ void LogView::Display() {
if (ImGui::Selectable("Clear")) {
m_data->Clear();
}
const auto& buf = m_data->GetBuffer();
if (ImGui::Selectable("Copy to Clipboard", false,
buf.empty() ? ImGuiSelectableFlags_Disabled : 0)) {
ImGui::SetClipboardText(buf.c_str());
}
ImGui::EndPopup();
}

29
glass/src/lib/native/cpp/other/Plot.cpp
View File

@@ -174,6 +174,7 @@ PlotSeries::PlotSeries(wpi::StringRef id) : m_id(id) {
PlotSeries::PlotSeries(DataSource* source, int yAxis) : m_yAxis(yAxis) {
SetSource(source);
m_id = source->GetId();
}
void PlotSeries::CheckSource() {
@@ -191,7 +192,6 @@ void PlotSeries::CheckSource() {
void PlotSeries::SetSource(DataSource* source) {
m_source = source;
m_id = source->GetId();
// add initial value
m_data[m_size++] = ImPlotPoint{wpi::Now() * 1.0e-6, source->GetValue()};
@@ -374,9 +374,15 @@ PlotSeries::Action PlotSeries::EmitPlot(PlotView& view, double now, size_t i,
ImPlot::EndLegendDragDropSource();
}
// Show full source name tooltip
if (!m_name.empty() && ImPlot::IsLegendEntryHovered(label)) {
ImGui::SetTooltip("%s", m_id.c_str());
}
// Edit settings via popup
Action rv = kNone;
if (ImPlot::BeginLegendPopup(label)) {
ImGui::TextUnformatted(m_id.c_str());
if (ImGui::Button("Close")) {
ImGui::CloseCurrentPopup();
}
@@ -983,10 +989,25 @@ void PlotProvider::DisplayMenu() {
}
if (ImGui::MenuItem("New Plot Window")) {
// this is an inefficient algorithm, but the number of windows is small
char id[32];
std::snprintf(id, sizeof(id), "Plot <%d>",
static_cast<int>(m_windows.size()));
AddWindow(id, std::make_unique<PlotView>(this));
size_t numWindows = m_windows.size();
for (size_t i = 0; i <= numWindows; ++i) {
std::snprintf(id, sizeof(id), "Plot <%d>", static_cast<int>(i));
bool match = false;
for (size_t j = i; j < numWindows; ++j) {
if (m_windows[j]->GetId() == id) {
match = true;
break;
}
}
if (!match) {
break;
}
}
if (auto win = AddWindow(id, std::make_unique<PlotView>(this))) {
win->SetDefaultSize(700, 400);
}
}
}

2
glass/src/lib/native/cpp/other/StringChooser.cpp
View File

@@ -38,6 +38,4 @@ void glass::DisplayStringChooser(StringChooserModel* model) {
}
ImGui::EndCombo();
}
ImGui::SameLine();
}

31
glass/src/lib/native/include/glass/other/Field2D.h
View File

@@ -4,38 +4,37 @@
#pragma once
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/geometry/Translation2d.h>
#include <imgui.h>
#include <wpi/ArrayRef.h>
#include <wpi/STLExtras.h>
#include <wpi/StringRef.h>
#include <wpi/Twine.h>
#include "glass/Model.h"
#include "glass/View.h"
namespace glass {
class DataSource;
class FieldObjectModel : public Model {
public:
virtual DataSource* GetXData() = 0;
virtual DataSource* GetYData() = 0;
virtual DataSource* GetRotationData() = 0;
virtual const char* GetName() const = 0;
virtual void SetPose(double x, double y, double rot) = 0;
virtual void SetPosition(double x, double y) = 0;
virtual void SetRotation(double rot) = 0;
};
class FieldObjectGroupModel : public Model {
public:
virtual void ForEachFieldObject(
wpi::function_ref<void(FieldObjectModel& model)> func) = 0;
virtual wpi::ArrayRef<frc::Pose2d> GetPoses() = 0;
virtual void SetPoses(wpi::ArrayRef<frc::Pose2d> poses) = 0;
virtual void SetPose(size_t i, frc::Pose2d pose) = 0;
virtual void SetPosition(size_t i, frc::Translation2d pos) = 0;
virtual void SetRotation(size_t i, frc::Rotation2d rot) = 0;
};
class Field2DModel : public Model {
public:
virtual void ForEachFieldObjectGroup(
wpi::function_ref<void(FieldObjectGroupModel& model, wpi::StringRef name)>
virtual FieldObjectModel* AddFieldObject(const wpi::Twine& name) = 0;
virtual void RemoveFieldObject(const wpi::Twine& name) = 0;
virtual void ForEachFieldObject(
wpi::function_ref<void(FieldObjectModel& model, wpi::StringRef name)>
func) = 0;
};

4
glass/src/lib/native/include/glass/other/Log.h
View File

@@ -4,6 +4,7 @@
#pragma once
#include <string>
#include <vector>
#include <wpi/Twine.h>
@@ -20,10 +21,11 @@ class LogData {
void Clear();
void Append(const wpi::Twine& msg);
const std::string& GetBuffer();
private:
size_t m_maxLines;
std::vector<char> m_buf;
std::string m_buf;
std::vector<size_t> m_lineOffsets{0};
};

278
glass/src/libnt/native/cpp/NTField2D.cpp
View File

@@ -5,20 +5,22 @@
#include "glass/networktables/NTField2D.h"
#include <algorithm>
#include <vector>
#include <ntcore_cpp.h>
#include <wpi/Endian.h>
#include <wpi/MathExtras.h>
#include <wpi/SmallString.h>
#include <wpi/SmallVector.h>
#include "glass/DataSource.h"
using namespace glass;
class NTField2DModel::GroupModel : public FieldObjectGroupModel {
class NTField2DModel::ObjectModel : public FieldObjectModel {
public:
GroupModel(wpi::StringRef name, NT_Entry entry)
ObjectModel(wpi::StringRef name, NT_Entry entry)
: m_name{name}, m_entry{entry} {}
wpi::StringRef GetName() const { return m_name; }
const char* GetName() const override { return m_name.c_str(); }
NT_Entry GetEntry() const { return m_entry; }
void NTUpdate(const nt::Value& value);
@@ -31,146 +33,118 @@ class NTField2DModel::GroupModel : public FieldObjectGroupModel {
bool Exists() override { return nt::GetEntryType(m_entry) != NT_UNASSIGNED; }
bool IsReadOnly() override { return false; }
void ForEachFieldObject(
wpi::function_ref<void(FieldObjectModel& model)> func) override;
wpi::ArrayRef<frc::Pose2d> GetPoses() override { return m_poses; }
void SetPoses(wpi::ArrayRef<frc::Pose2d> poses) override;
void SetPose(size_t i, frc::Pose2d pose) override;
void SetPosition(size_t i, frc::Translation2d pos) override;
void SetRotation(size_t i, frc::Rotation2d rot) override;
private:
void UpdateNT();
std::string m_name;
NT_Entry m_entry;
// keep count of objects rather than resizing vector, as there is a fair
// amount of overhead associated with the latter (DataSource record keeping)
size_t m_count = 0;
class ObjectModel;
std::vector<std::unique_ptr<ObjectModel>> m_objects;
std::vector<frc::Pose2d> m_poses;
};
class NTField2DModel::GroupModel::ObjectModel : public FieldObjectModel {
public:
ObjectModel(wpi::StringRef name, NT_Entry entry, int index)
: m_entry{entry},
m_index{index},
m_x{name + "[" + wpi::Twine{index} + "]/x"},
m_y{name + "[" + wpi::Twine{index} + "]/y"},
m_rot{name + "[" + wpi::Twine{index} + "]/rot"} {}
void NTField2DModel::ObjectModel::NTUpdate(const nt::Value& value) {
if (value.IsDoubleArray()) {
auto arr = value.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] = frc::Pose2d{
units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
frc::Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
}
} else if (value.IsRaw()) {
// treat it simply as an array of doubles
wpi::StringRef data = value.GetRaw();
void SetExists(bool exists) { m_exists = exists; }
void Update() override {}
bool Exists() override { return m_exists; }
bool IsReadOnly() override { return false; }
DataSource* GetXData() override { return &m_x; }
DataSource* GetYData() override { return &m_y; }
DataSource* GetRotationData() override { return &m_rot; }
void SetPose(double x, double y, double rot) override;
void SetPosition(double x, double y) override;
void SetRotation(double rot) override;
private:
void SetPoseImpl(double x, double y, double rot, bool setX, bool setY,
bool setRot);
NT_Entry m_entry;
int m_index;
bool m_exists = true;
public:
DataSource m_x;
DataSource m_y;
DataSource m_rot;
};
void NTField2DModel::GroupModel::NTUpdate(const nt::Value& value) {
if (!value.IsDoubleArray()) {
m_count = 0;
return;
}
auto arr = value.GetDoubleArray();
// must be triples
if ((arr.size() % 3) != 0) {
m_count = 0;
return;
}
m_count = arr.size() / 3;
if (m_count > m_objects.size()) {
m_objects.reserve(m_count);
for (size_t i = m_objects.size(); i < m_count; ++i) {
m_objects.emplace_back(std::make_unique<ObjectModel>(m_name, m_entry, i));
// 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] = frc::Pose2d{units::meter_t{x}, units::meter_t{y},
frc::Rotation2d{units::degree_t{rot}}};
}
}
if (m_count < m_objects.size()) {
for (size_t i = m_count; i < m_objects.size(); ++i) {
m_objects[i]->SetExists(false);
}
void NTField2DModel::ObjectModel::UpdateNT() {
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>());
}
}
for (size_t i = 0; i < m_count; ++i) {
auto& obj = m_objects[i];
obj->SetExists(true);
obj->m_x.SetValue(arr[i * 3], value.last_change());
obj->m_y.SetValue(arr[i * 3 + 1], value.last_change());
obj->m_rot.SetValue(arr[i * 3 + 2], value.last_change());
nt::SetEntryTypeValue(m_entry, nt::Value::MakeDoubleArray(arr));
} else {
// 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;
}
nt::SetEntryTypeValue(
m_entry, nt::Value::MakeRaw(wpi::StringRef{arr.data(), arr.size()}));
}
}
void NTField2DModel::GroupModel::ForEachFieldObject(
wpi::function_ref<void(FieldObjectModel& model)> func) {
for (size_t i = 0; i < m_count; ++i) {
func(*m_objects[i]);
void NTField2DModel::ObjectModel::SetPoses(wpi::ArrayRef<frc::Pose2d> poses) {
m_poses = poses;
UpdateNT();
}
void NTField2DModel::ObjectModel::SetPose(size_t i, frc::Pose2d pose) {
if (i < m_poses.size()) {
m_poses[i] = pose;
UpdateNT();
}
}
void NTField2DModel::GroupModel::ObjectModel::SetPose(double x, double y,
double rot) {
SetPoseImpl(x, y, rot, true, true, true);
void NTField2DModel::ObjectModel::SetPosition(size_t i,
frc::Translation2d pos) {
if (i < m_poses.size()) {
m_poses[i] = frc::Pose2d{pos, m_poses[i].Rotation()};
UpdateNT();
}
}
void NTField2DModel::GroupModel::ObjectModel::SetPosition(double x, double y) {
SetPoseImpl(x, y, 0, true, true, false);
}
void NTField2DModel::GroupModel::ObjectModel::SetRotation(double rot) {
SetPoseImpl(0, 0, rot, false, false, true);
}
void NTField2DModel::GroupModel::ObjectModel::SetPoseImpl(double x, double y,
double rot, bool setX,
bool setY,
bool setRot) {
// get from NT, validate type and size
auto value = nt::GetEntryValue(m_entry);
if (!value || !value->IsDoubleArray()) {
return;
void NTField2DModel::ObjectModel::SetRotation(size_t i, frc::Rotation2d rot) {
if (i < m_poses.size()) {
m_poses[i] = frc::Pose2d{m_poses[i].Translation(), rot};
UpdateNT();
}
auto origArr = value->GetDoubleArray();
if (static_cast<int>(origArr.size()) < ((m_index + 1) * 3)) {
return;
}
// copy existing array
wpi::SmallVector<double, 8> arr;
arr.reserve(origArr.size());
for (auto&& elem : origArr) {
arr.emplace_back(elem);
}
// update value
if (setX) {
arr[m_index * 3 + 0] = x;
}
if (setY) {
arr[m_index * 3 + 1] = y;
}
if (setRot) {
arr[m_index * 3 + 2] = rot;
}
// set back to NT
nt::SetEntryValue(m_entry, nt::Value::MakeDoubleArray(arr));
}
NTField2DModel::NTField2DModel(wpi::StringRef path)
@@ -199,9 +173,9 @@ void NTField2DModel::Update() {
// common case: update of existing entry; search by entry
if (event.flags & NT_NOTIFY_UPDATE) {
auto it = std::find_if(
m_groups.begin(), m_groups.end(),
m_objects.begin(), m_objects.end(),
[&](const auto& e) { return e->GetEntry() == event.entry; });
if (it != m_groups.end()) {
if (it != m_objects.end()) {
(*it)->NTUpdate(*event.value);
continue;
}
@@ -213,20 +187,16 @@ void NTField2DModel::Update() {
if (name.empty() || name[0] == '.') {
continue;
}
auto it = std::lower_bound(m_groups.begin(), m_groups.end(), event.name,
[](const auto& e, wpi::StringRef name) {
return e->GetName() < name;
});
bool match = (it != m_groups.end() && (*it)->GetName() == event.name);
auto [it, match] = Find(event.name);
if (event.flags & NT_NOTIFY_DELETE) {
if (match) {
m_groups.erase(it);
m_objects.erase(it);
}
continue;
} else if (event.flags & NT_NOTIFY_NEW) {
if (!match) {
it = m_groups.emplace(
it, std::make_unique<GroupModel>(event.name, event.entry));
it = m_objects.emplace(
it, std::make_unique<ObjectModel>(event.name, event.entry));
}
} else if (!match) {
continue;
@@ -246,12 +216,40 @@ bool NTField2DModel::IsReadOnly() {
return false;
}
void NTField2DModel::ForEachFieldObjectGroup(
wpi::function_ref<void(FieldObjectGroupModel& model, wpi::StringRef name)>
FieldObjectModel* NTField2DModel::AddFieldObject(const wpi::Twine& name) {
wpi::SmallString<128> fullNameBuf;
wpi::StringRef fullName = (m_path + name).toStringRef(fullNameBuf);
auto [it, match] = Find(fullName);
if (!match) {
it = m_objects.emplace(
it, std::make_unique<ObjectModel>(fullName, m_nt.GetEntry(fullName)));
}
return it->get();
}
void NTField2DModel::RemoveFieldObject(const wpi::Twine& name) {
wpi::SmallString<128> fullNameBuf;
auto [it, match] = Find((m_path + name).toStringRef(fullNameBuf));
if (match) {
nt::DeleteEntry((*it)->GetEntry());
m_objects.erase(it);
}
}
void NTField2DModel::ForEachFieldObject(
wpi::function_ref<void(FieldObjectModel& model, wpi::StringRef name)>
func) {
for (auto&& group : m_groups) {
if (group->Exists()) {
func(*group, wpi::StringRef{group->GetName()}.drop_front(m_path.size()));
for (auto&& obj : m_objects) {
if (obj->Exists()) {
func(*obj, wpi::StringRef{obj->GetName()}.drop_front(m_path.size()));
}
}
}
std::pair<NTField2DModel::Objects::iterator, bool> NTField2DModel::Find(
wpi::StringRef fullName) {
auto it = std::lower_bound(
m_objects.begin(), m_objects.end(), fullName,
[](const auto& e, wpi::StringRef name) { return e->GetName() < name; });
return {it, it != m_objects.end() && (*it)->GetName() == fullName};
}

35
glass/src/libnt/native/cpp/NTStringChooser.cpp
View File

@@ -39,17 +39,30 @@ void NTStringChooserModel::SetOptions(wpi::ArrayRef<std::string> val) {
void NTStringChooserModel::Update() {
for (auto&& event : m_nt.PollListener()) {
if (event.entry == m_default && event.value && event.value->IsString()) {
m_defaultValue = event.value->GetString();
} else if (event.entry == m_selected && event.value &&
event.value->IsString()) {
m_selectedValue = event.value->GetString();
} else if (event.entry == m_active && event.value &&
event.value->IsString()) {
m_activeValue = event.value->GetString();
} else if (event.entry == m_options && event.value &&
event.value->IsStringArray()) {
m_optionsValue = event.value->GetStringArray();
if (event.entry == m_default) {
if ((event.flags & NT_NOTIFY_DELETE) != 0) {
m_defaultValue.clear();
} else if (event.value && event.value->IsString()) {
m_defaultValue = event.value->GetString();
}
} else if (event.entry == m_selected) {
if ((event.flags & NT_NOTIFY_DELETE) != 0) {
m_selectedValue.clear();
} else if (event.value && event.value->IsString()) {
m_selectedValue = event.value->GetString();
}
} else if (event.entry == m_active) {
if ((event.flags & NT_NOTIFY_DELETE) != 0) {
m_activeValue.clear();
} else if (event.value && event.value->IsString()) {
m_activeValue = event.value->GetString();
}
} else if (event.entry == m_options) {
if ((event.flags & NT_NOTIFY_DELETE) != 0) {
m_optionsValue.clear();
} else if (event.value && event.value->IsStringArray()) {
m_optionsValue = event.value->GetStringArray();
}
}
}
}

14
glass/src/libnt/native/include/glass/networktables/NTField2D.h
View File

@@ -6,6 +6,7 @@
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <ntcore_cpp.h>
@@ -32,8 +33,10 @@ class NTField2DModel : public Field2DModel {
bool Exists() override;
bool IsReadOnly() override;
void ForEachFieldObjectGroup(
wpi::function_ref<void(FieldObjectGroupModel& model, wpi::StringRef name)>
FieldObjectModel* AddFieldObject(const wpi::Twine& name) override;
void RemoveFieldObject(const wpi::Twine& name) override;
void ForEachFieldObject(
wpi::function_ref<void(FieldObjectModel& model, wpi::StringRef name)>
func) override;
private:
@@ -42,8 +45,11 @@ class NTField2DModel : public Field2DModel {
NT_Entry m_name;
std::string m_nameValue;
class GroupModel;
std::vector<std::unique_ptr<GroupModel>> m_groups;
class ObjectModel;
using Objects = std::vector<std::unique_ptr<ObjectModel>>;
Objects m_objects;
std::pair<Objects::iterator, bool> Find(wpi::StringRef fullName);
};
} // namespace glass

3
glass/src/libnt/native/include/glass/networktables/NetworkTablesHelper.h
View File

@@ -28,7 +28,8 @@ class NetworkTablesHelper {
}
static constexpr int kDefaultListenerFlags =
NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE | NT_NOTIFY_IMMEDIATE;
NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE | NT_NOTIFY_DELETE |
NT_NOTIFY_IMMEDIATE;
NT_EntryListener AddListener(NT_Entry entry,
unsigned int flags = kDefaultListenerFlags) {

18
wpigui/src/main/native/cpp/wpigui.cpp
View File

@@ -213,7 +213,23 @@ bool gui::Initialize(const char* title, int width, int height) {
// Update window settings
if (gContext->xPos != -1 && gContext->yPos != -1) {
glfwSetWindowPos(gContext->window, gContext->xPos, gContext->yPos);
// check to make sure the position isn't off-screen
bool found = false;
int monCount;
GLFWmonitor** monitors = glfwGetMonitors(&monCount);
for (int i = 0; i < monCount; ++i) {
int monXPos, monYPos, monWidth, monHeight;
glfwGetMonitorWorkarea(monitors[i], &monXPos, &monYPos, &monWidth,
&monHeight);
if (gContext->xPos >= monXPos && gContext->xPos < (monXPos + monWidth) &&
gContext->yPos >= monYPos && gContext->yPos < (monYPos + monHeight)) {
found = true;
break;
}
}
if (found) {
glfwSetWindowPos(gContext->window, gContext->xPos, gContext->yPos);
}
glfwShowWindow(gContext->window);
}

3
wpilibc/src/main/native/cpp/AnalogEncoder.cpp
View File

@@ -12,6 +12,9 @@
using namespace frc;
AnalogEncoder::AnalogEncoder(int channel)
: AnalogEncoder(std::make_shared<AnalogInput>(channel)) {}
AnalogEncoder::AnalogEncoder(AnalogInput& analogInput)
: m_analogInput{&analogInput, NullDeleter<AnalogInput>{}},
m_analogTrigger{m_analogInput.get()},

12
wpilibc/src/main/native/cpp/Compressor.cpp
View File

@@ -125,14 +125,10 @@ int Compressor::GetModule() const {
void Compressor::InitSendable(SendableBuilder& builder) {
builder.SetSmartDashboardType("Compressor");
builder.AddBooleanProperty(
"Enabled", [=]() { return Enabled(); },
[=](bool value) {
if (value) {
Start();
} else {
Stop();
}
});
"Closed Loop Control", [=]() { return GetClosedLoopControl(); },
[=](bool value) { SetClosedLoopControl(value); });
builder.AddBooleanProperty(
"Enabled", [=] { return Enabled(); }, nullptr);
builder.AddBooleanProperty(
"Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr);
}

7
wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
View File

@@ -34,6 +34,13 @@ void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
ChassisSpeeds HolonomicDriveController::Calculate(
const Pose2d& currentPose, const Pose2d& poseRef,
units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
// If this is the first run, then we need to reset the theta controller to the
// current pose's heading.
if (m_firstRun) {
m_thetaController.Reset(currentPose.Rotation().Radians());
m_firstRun = false;
}
// Calculate feedforward velocities (field-relative)
auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
auto yFF = linearVelocityRef * poseRef.Rotation().Sin();

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}}};
}
}
}

7
wpilibc/src/main/native/include/frc/AnalogEncoder.h
View File

@@ -23,6 +23,13 @@ class AnalogInput;
*/
class AnalogEncoder : public Sendable, public SendableHelper<AnalogEncoder> {
public:
/**
* Construct a new AnalogEncoder attached to a specific AnalogIn channel.
*
* @param channel the analog input channel to attach to
*/
explicit AnalogEncoder(int channel);
/**
* Construct a new AnalogEncoder attached to a specific AnalogInput.
*

2
wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
View File

@@ -105,5 +105,7 @@ class HolonomicDriveController {
frc2::PIDController m_xController;
frc2::PIDController m_yController;
ProfiledPIDController<units::radian> m_thetaController;
bool m_firstRun = true;
};
} // namespace frc

105
wpilibc/src/main/native/include/frc/romi/RomiGyro.h
View File

@@ -1,105 +0,0 @@
// 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.
#pragma once
#include <hal/SimDevice.h>
#include "frc/interfaces/Gyro.h"
#include "frc/smartdashboard/Sendable.h"
#include "frc/smartdashboard/SendableHelper.h"
namespace frc {
/**
* Use a rate gyro to return the robots heading relative to a starting position.
*
* This class is for the Romi onboard gyro, and will only work in
* simulation/Romi mode. Only one instance of a RomiGyro is supported.
*/
class RomiGyro : public Gyro, public Sendable, public SendableHelper<RomiGyro> {
public:
RomiGyro();
/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on integration of the returned rate form the gyro.
* The angle is continuous, that is, it will continue from 360->361 degrees.
* This allows algorithms that wouldn't want to see a discontinuity in the
* gyro output as it sweeps from 360 to 0 on the second time around.
*
* @return the current heading of the robot in degrees.
*/
double GetAngle() const override;
/**
* Return the rate of rotation of the gyro
*
* The rate is based on the most recent reading of the gyro.
*
* @return the current rate in degrees per second
*/
double GetRate() const override;
/**
* Initialize the gyro.
*
* NOTE: This function is a no-op. The Romi gyro should be calibrated via
* the web UI.
*/
void Calibrate() final {}
/**
* Gets the rate of turn in degrees-per-second around the X-axis
*/
double GetRateX() const;
/**
* Gets the rate of turn in degrees-per-second around the Y-axis
*/
double GetRateY() const;
/**
* Gets the rate of turn in degrees-per-second around the Z-axis
*/
double GetRateZ() const;
/**
* Gets the currently reported angle around the X-axis
*/
double GetAngleX() const;
/**
* Gets the currently reported angle around the X-axis
*/
double GetAngleY() const;
/**
* Gets the currently reported angle around the X-axis
*/
double GetAngleZ() const;
/**
* Resets the gyro
*/
void Reset() override;
void InitSendable(SendableBuilder& builder) override;
private:
hal::SimDevice m_simDevice;
hal::SimDouble m_simRateX;
hal::SimDouble m_simRateY;
hal::SimDouble m_simRateZ;
hal::SimDouble m_simAngleX;
hal::SimDouble m_simAngleY;
hal::SimDouble m_simAngleZ;
double m_angleXOffset = 0;
double m_angleYOffset = 0;
double m_angleZOffset = 0;
};
} // namespace frc

4
wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
View File

@@ -18,7 +18,7 @@ namespace frc::sim {
class FlywheelSim : public LinearSystemSim<1, 1, 1> {
public:
/**
* Creates a simulated flywhel mechanism.
* Creates a simulated flywheel mechanism.
*
* @param plant The linear system representing the flywheel.
* @param gearbox The type of and number of motors in the flywheel
@@ -32,7 +32,7 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
const std::array<double, 1>& measurementStdDevs = {0.0});
/**
* Creates a simulated flywhel mechanism.
* Creates a simulated flywheel mechanism.
*
* @param gearbox The type of and number of motors in the flywheel
* gearbox.

8
wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h
View File

@@ -21,6 +21,7 @@
namespace frc {
class Field2d;
class Trajectory;
/**
* Game field object on a Field2d.
@@ -76,6 +77,13 @@ class FieldObject2d {
*/
void SetPoses(std::initializer_list<Pose2d> poses);
/**
* Sets poses from a trajectory.
*
* @param trajectory The trajectory from which poses should be added.
*/
void SetTrajectory(const Trajectory& trajectory);
/**
* Get multiple poses.
*

14
wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
View File

@@ -47,3 +47,17 @@ TEST(HolonomicDriveControllerTest, ReachesReference) {
EXPECT_NEAR_UNITS(frc::AngleModulus(robotPose.Rotation().Radians()), 0_rad,
kAngularTolerance);
}
TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
frc::HolonomicDriveController controller{
frc2::PIDController{1, 0, 0}, frc2::PIDController{1, 0, 0},
frc::ProfiledPIDController<units::radian>{
1, 0, 0,
frc::TrapezoidProfile<units::radian>::Constraints{
4_rad_per_s, 2_rad_per_s / 1_s}}};
frc::ChassisSpeeds speeds = controller.Calculate(
frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad);
EXPECT_EQ(0, speeds.omega.to<double>());
}

32
wpilibc/src/main/native/cpp/romi/RomiGyro.cpp → wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp
View File

@@ -2,11 +2,7 @@
// 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/romi/RomiGyro.h"
#include "frc/smartdashboard/SendableBuilder.h"
using namespace frc;
#include "sensors/RomiGyro.h"
RomiGyro::RomiGyro() : m_simDevice("Gyro:RomiGyro") {
if (m_simDevice) {
@@ -26,15 +22,7 @@ RomiGyro::RomiGyro() : m_simDevice("Gyro:RomiGyro") {
}
}
double RomiGyro::GetAngle() const {
return GetAngleZ();
}
double RomiGyro::GetRate() const {
return GetRateZ();
}
double RomiGyro::GetRateX() const {
double RomiGyro::GetRateX() {
if (m_simRateX) {
return m_simRateX.Get();
}
@@ -42,7 +30,7 @@ double RomiGyro::GetRateX() const {
return 0.0;
}
double RomiGyro::GetRateY() const {
double RomiGyro::GetRateY() {
if (m_simRateY) {
return m_simRateY.Get();
}
@@ -50,7 +38,7 @@ double RomiGyro::GetRateY() const {
return 0.0;
}
double RomiGyro::GetRateZ() const {
double RomiGyro::GetRateZ() {
if (m_simRateZ) {
return m_simRateZ.Get();
}
@@ -58,7 +46,7 @@ double RomiGyro::GetRateZ() const {
return 0.0;
}
double RomiGyro::GetAngleX() const {
double RomiGyro::GetAngleX() {
if (m_simAngleX) {
return m_simAngleX.Get() - m_angleXOffset;
}
@@ -66,7 +54,7 @@ double RomiGyro::GetAngleX() const {
return 0.0;
}
double RomiGyro::GetAngleY() const {
double RomiGyro::GetAngleY() {
if (m_simAngleY) {
return m_simAngleY.Get() - m_angleYOffset;
}
@@ -74,7 +62,7 @@ double RomiGyro::GetAngleY() const {
return 0.0;
}
double RomiGyro::GetAngleZ() const {
double RomiGyro::GetAngleZ() {
if (m_simAngleZ) {
return m_simAngleZ.Get() - m_angleZOffset;
}
@@ -89,9 +77,3 @@ void RomiGyro::Reset() {
m_angleZOffset = m_simAngleZ.Get();
}
}
void RomiGyro::InitSendable(SendableBuilder& builder) {
builder.SetSmartDashboardType("Gyro");
builder.AddDoubleProperty(
"Value", [=]() { return GetAngle(); }, nullptr);
}

60
wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h Normal file
View File

@@ -0,0 +1,60 @@
// 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.
#pragma once
#include <hal/SimDevice.h>
class RomiGyro {
public:
RomiGyro();
/**
* Gets the rate of turn in degrees-per-second around the X-axis
*/
double GetRateX();
/**
* Gets the rate of turn in degrees-per-second around the Y-axis
*/
double GetRateY();
/**
* Gets the rate of turn in degrees-per-second around the Z-axis
*/
double GetRateZ();
/**
* Gets the currently reported angle around the X-axis
*/
double GetAngleX();
/**
* Gets the currently reported angle around the X-axis
*/
double GetAngleY();
/**
* Gets the currently reported angle around the X-axis
*/
double GetAngleZ();
/**
* Resets the gyro
*/
void Reset();
private:
hal::SimDevice m_simDevice;
hal::SimDouble m_simRateX;
hal::SimDouble m_simRateY;
hal::SimDouble m_simRateZ;
hal::SimDouble m_simAngleX;
hal::SimDouble m_simAngleY;
hal::SimDouble m_simAngleZ;
double m_angleXOffset = 0;
double m_angleYOffset = 0;
double m_angleZOffset = 0;
};

5
wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h
View File

@@ -8,10 +8,11 @@
#include <frc/Encoder.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/motorcontrol/Spark.h>
#include <frc/romi/RomiGyro.h>
#include <frc2/command/SubsystemBase.h>
#include <units/length.h>
#include "sensors/RomiGyro.h"
class Drivetrain : public frc2::SubsystemBase {
public:
static constexpr double kCountsPerRevolution = 1440.0;
@@ -116,6 +117,6 @@ class Drivetrain : public frc2::SubsystemBase {
frc::DifferentialDrive m_drive{m_leftMotor, m_rightMotor};
frc::RomiGyro m_gyro;
RomiGyro m_gyro;
frc::BuiltInAccelerometer m_accelerometer;
};

9
wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
View File

@@ -23,6 +23,15 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
protected SimDevice m_simDevice;
protected SimDouble m_simPosition;
/**
* Construct a new AnalogEncoder attached to a specific AnalogIn channel.
*
* @param channel the analog input channel to attach to
*/
public AnalogEncoder(int channel) {
this(new AnalogInput(channel));
}
/**
* Construct a new AnalogEncoder attached to a specific AnalogInput.
*

11
wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
View File

@@ -205,15 +205,8 @@ public class Compressor implements Sendable, AutoCloseable {
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Compressor");
builder.addBooleanProperty(
"Enabled",
this::enabled,
value -> {
if (value) {
start();
} else {
stop();
}
});
"Closed Loop Control", this::getClosedLoopControl, this::setClosedLoopControl);
builder.addBooleanProperty("Enabled", this::enabled, null);
builder.addBooleanProperty("Pressure switch", this::getPressureSwitchValue, null);
}
}

9
wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java
View File

@@ -31,6 +31,8 @@ public class HolonomicDriveController {
private final PIDController m_yController;
private final ProfiledPIDController m_thetaController;
private boolean m_firstRun = true;
/**
* Constructs a holonomic drive controller.
*
@@ -82,6 +84,13 @@ public class HolonomicDriveController {
@SuppressWarnings("LocalVariableName")
public ChassisSpeeds calculate(
Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters, Rotation2d angleRef) {
// If this is the first run, then we need to reset the theta controller to the current pose's
// heading.
if (m_firstRun) {
m_thetaController.reset(currentPose.getRotation().getRadians());
m_firstRun = false;
}
// Calculate feedforward velocities (field-relative).
double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();

96
wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
View File

@@ -8,6 +8,9 @@ import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.ArrayList;
import java.util.List;
@@ -82,6 +85,19 @@ public class FieldObject2d {
updateEntry();
}
/**
* Sets poses from a trajectory.
*
* @param trajectory The trajectory from which the poses should be added.
*/
public synchronized void setTrajectory(Trajectory trajectory) {
m_poses.clear();
for (Trajectory.State state : trajectory.getStates()) {
m_poses.add(state.poseMeters);
}
updateEntry();
}
/**
* Get multiple poses.
*
@@ -101,20 +117,39 @@ public class FieldObject2d {
return;
}
double[] arr = new double[m_poses.size() * 3];
int ndx = 0;
for (Pose2d pose : m_poses) {
Translation2d translation = pose.getTranslation();
arr[ndx + 0] = translation.getX();
arr[ndx + 1] = translation.getY();
arr[ndx + 2] = pose.getRotation().getDegrees();
ndx += 3;
}
if (m_poses.size() < (255 / 3)) {
double[] arr = new double[m_poses.size() * 3];
int ndx = 0;
for (Pose2d pose : m_poses) {
Translation2d translation = pose.getTranslation();
arr[ndx + 0] = translation.getX();
arr[ndx + 1] = translation.getY();
arr[ndx + 2] = pose.getRotation().getDegrees();
ndx += 3;
}
if (setDefault) {
m_entry.setDefaultDoubleArray(arr);
if (setDefault) {
m_entry.setDefaultDoubleArray(arr);
} else {
m_entry.setDoubleArray(arr);
}
} else {
m_entry.setDoubleArray(arr);
// send as raw array of doubles if too big for NT array
ByteBuffer output = ByteBuffer.allocate(m_poses.size() * 3 * 8);
output.order(ByteOrder.BIG_ENDIAN);
for (Pose2d pose : m_poses) {
Translation2d translation = pose.getTranslation();
output.putDouble(translation.getX());
output.putDouble(translation.getY());
output.putDouble(pose.getRotation().getDegrees());
}
if (setDefault) {
m_entry.setDefaultRaw(output.array());
} else {
m_entry.forceSetRaw(output.array());
}
}
}
@@ -125,17 +160,36 @@ public class FieldObject2d {
}
double[] arr = m_entry.getDoubleArray((double[]) null);
if (arr == null) {
return;
}
if (arr != null) {
if ((arr.length % 3) != 0) {
return;
}
if ((arr.length % 3) != 0) {
return;
}
m_poses.clear();
for (int i = 0; i < arr.length; i += 3) {
m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
}
} else {
// read as raw array of doubles
byte[] data = m_entry.getRaw((byte[]) null);
if (data == null) {
return;
}
m_poses.clear();
for (int i = 0; i < arr.length; i += 3) {
m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
// must be triples of doubles
if ((data.length % (3 * 8)) != 0) {
return;
}
ByteBuffer input = ByteBuffer.wrap(data);
input.order(ByteOrder.BIG_ENDIAN);
m_poses.clear();
for (int i = 0; i < (data.length / (3 * 8)); i++) {
double x = input.getDouble();
double y = input.getDouble();
double rot = input.getDouble();
m_poses.add(new Pose2d(x, y, Rotation2d.fromDegrees(rot)));
}
}
}

15
wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java
View File

@@ -72,4 +72,19 @@ class HolonomicDriveControllerTest {
MathUtil.angleModulus(finalRobotPose.getRotation().getRadians()),
kAngularTolerance));
}
@Test
void testDoesNotRotateUnnecessarily() {
var controller =
new HolonomicDriveController(
new PIDController(1, 0, 0),
new PIDController(1, 0, 0),
new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(4, 2)));
ChassisSpeeds speeds =
controller.calculate(
new Pose2d(0, 0, new Rotation2d(1.57)), new Pose2d(), 0, new Rotation2d(1.57));
assertEquals(0.0, speeds.omegaRadiansPerSecond);
}
}

55
wpilibj/src/main/java/edu/wpi/first/wpilibj/romi/RomiGyro.java → wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java
View File

@@ -2,17 +2,13 @@
// 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.
package edu.wpi.first.wpilibj.romi;
package edu.wpi.first.wpilibj.examples.romireference.sensors;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.Sendable;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
public class RomiGyro implements Gyro, Sendable {
private final SimDevice m_simDevice;
public class RomiGyro {
private SimDouble m_simRateX;
private SimDouble m_simRateY;
private SimDouble m_simRateZ;
@@ -26,16 +22,16 @@ public class RomiGyro implements Gyro, Sendable {
/** Create a new RomiGyro. */
public RomiGyro() {
m_simDevice = SimDevice.create("Gyro:RomiGyro");
if (m_simDevice != null) {
m_simDevice.createBoolean("init", Direction.kOutput, true);
m_simRateX = m_simDevice.createDouble("rate_x", Direction.kInput, 0.0);
m_simRateY = m_simDevice.createDouble("rate_y", Direction.kInput, 0.0);
m_simRateZ = m_simDevice.createDouble("rate_z", Direction.kInput, 0.0);
SimDevice gyroSimDevice = SimDevice.create("Gyro:RomiGyro");
if (gyroSimDevice != null) {
gyroSimDevice.createBoolean("init", Direction.kOutput, true);
m_simRateX = gyroSimDevice.createDouble("rate_x", Direction.kInput, 0.0);
m_simRateY = gyroSimDevice.createDouble("rate_y", Direction.kInput, 0.0);
m_simRateZ = gyroSimDevice.createDouble("rate_z", Direction.kInput, 0.0);
m_simAngleX = m_simDevice.createDouble("angle_x", Direction.kInput, 0.0);
m_simAngleY = m_simDevice.createDouble("angle_y", Direction.kInput, 0.0);
m_simAngleZ = m_simDevice.createDouble("angle_z", Direction.kInput, 0.0);
m_simAngleX = gyroSimDevice.createDouble("angle_x", Direction.kInput, 0.0);
m_simAngleY = gyroSimDevice.createDouble("angle_y", Direction.kInput, 0.0);
m_simAngleZ = gyroSimDevice.createDouble("angle_z", Direction.kInput, 0.0);
}
}
@@ -118,7 +114,6 @@ public class RomiGyro implements Gyro, Sendable {
}
/** Reset the gyro angles to 0. */
@Override
public void reset() {
if (m_simAngleX != null) {
m_angleXOffset = m_simAngleX.get();
@@ -126,32 +121,4 @@ public class RomiGyro implements Gyro, Sendable {
m_angleZOffset = m_simAngleZ.get();
}
}
@Override
public void calibrate() {
// no-op - Romi Gyro calibration is done via web UI
}
@Override
public double getAngle() {
return getAngleZ();
}
@Override
public double getRate() {
return getRateZ();
}
@Override
public void close() throws Exception {
if (m_simDevice != null) {
m_simDevice.close();
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
View File

@@ -7,8 +7,8 @@ package edu.wpi.first.wpilibj.examples.romireference.subsystems;
import edu.wpi.first.wpilibj.BuiltInAccelerometer;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.examples.romireference.sensors.RomiGyro;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.romi.RomiGyro;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Drivetrain extends SubsystemBase {

32
wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
View File

@@ -91,6 +91,38 @@ public final class WPIMathJNI {
*/
public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
/**
* Loads a Pathweaver JSON.
*
* @param path The path to the JSON.
* @return A double array with the trajectory states from the JSON.
*/
public static native double[] fromPathweaverJson(String path) throws IOException;
/**
* Converts a trajectory into a Pathweaver JSON and saves it.
*
* @param elements The elements of the trajectory.
* @param path The location to save the JSON to.
*/
public static native void toPathweaverJson(double[] elements, String path) throws IOException;
/**
* Deserializes a trajectory JSON into a double[] of trajectory elements.
*
* @param json The JSON containing the serialized trajectory.
* @return A double array with the trajectory states.
*/
public static native double[] deserializeTrajectory(String json);
/**
* Serializes the trajectory into a JSON string.
*
* @param elements The elements of the trajectory.
* @return A JSON containing the serialized trajectory.
*/
public static native String serializeTrajectory(double[] elements);
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);

6
wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
View File

@@ -74,6 +74,9 @@ public class LinearFilter {
* Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
* gain = e^(-dt / T), T is the time constant in seconds.
*
* <p>Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency above which the
* input starts to attenuate.
*
* <p>This filter is stable for time constants greater than zero.
*
* @param timeConstant The discrete-time time constant in seconds.
@@ -91,6 +94,9 @@ public class LinearFilter {
* Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
* gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
*
* <p>Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency below which the
* input starts to attenuate.
*
* <p>This filter is stable for time constants greater than zero.
*
* @param timeConstant The discrete-time time constant in seconds.

20
wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
View File

@@ -80,6 +80,26 @@ public class CubicHermiteSpline extends Spline {
*/
private SimpleMatrix makeHermiteBasis() {
if (hermiteBasis == null) {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
// the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P(i+1) = P(1) = a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
//
// [ P(i) ] = [ 0 0 0 1 ][ a3 ]
// [ P'(i) ] = [ 0 0 1 0 ][ a2 ]
// [ P(i+1) ] = [ 1 1 1 1 ][ a1 ]
// [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
//
// To solve for the coefficients, we can invert the 4x4 matrix and move it
// to the other side of the equation.
//
// [ a3 ] = [ 2 1 -2 1 ][ P(i) ]
// [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ]
// [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
// [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
hermiteBasis =
new SimpleMatrix(
4,

29
wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
View File

@@ -80,13 +80,40 @@ public class QuinticHermiteSpline extends Spline {
*/
private SimpleMatrix makeHermiteBasis() {
if (hermiteBasis == null) {
// Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
// vectors, we want to find the coefficients of the spline
// P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P''(i) = P''(0) = 2 * a2
// P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
// P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
//
// [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ]
// [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ]
// [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ]
// [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ]
// [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ]
// [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ]
//
// To solve for the coefficients, we can invert the 6x6 matrix and move it
// to the other side of the equation.
//
// [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ]
// [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ]
// [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ]
// [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ]
// [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ]
// [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ]
hermiteBasis =
new SimpleMatrix(
6,
6,
true,
new double[] {
-06.0, -03.0, -00.5, +06.0, -03.0, +00.5, +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
-06.0, -03.0, -00.5, +06.0, -03.0, +00.5, +15.0, +08.0, +01.5, -15.0, +07.0, -01.0,
-10.0, -06.0, -01.5, +10.0, -04.0, +00.5, +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +00.0
});

112
wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
View File

@@ -4,73 +4,117 @@
package edu.wpi.first.wpilibj.trajectory;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.databind.ObjectReader;
import com.fasterxml.jackson.databind.ObjectWriter;
import java.io.BufferedReader;
import java.io.BufferedWriter;
import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.Arrays;
import java.util.ArrayList;
import java.util.List;
public final class TrajectoryUtil {
private static final ObjectReader READER = new ObjectMapper().readerFor(Trajectory.State[].class);
private static final ObjectWriter WRITER = new ObjectMapper().writerFor(Trajectory.State[].class);
private TrajectoryUtil() {
throw new UnsupportedOperationException("This is a utility class!");
}
/**
* Creates a trajectory from a double[] of elements.
*
* @param elements A double[] containing the raw elements of the trajectory.
* @return A trajectory created from the elements.
*/
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
private static Trajectory createTrajectoryFromElements(double[] elements) {
// Make sure that the elements have the correct length.
if (elements.length % 7 != 0) {
throw new TrajectorySerializationException(
"An error occurred when converting trajectory elements into a trajectory.");
}
// Create a list of states from the elements.
List<Trajectory.State> states = new ArrayList<>();
for (int i = 0; i < elements.length; i += 7) {
states.add(
new Trajectory.State(
elements[i],
elements[i + 1],
elements[i + 2],
new Pose2d(elements[i + 3], elements[i + 4], new Rotation2d(elements[i + 5])),
elements[i + 6]));
}
return new Trajectory(states);
}
/**
* Returns a double[] of elements from the given trajectory.
*
* @param trajectory The trajectory to retrieve raw elements from.
* @return A double[] of elements from the given trajectory.
*/
private static double[] getElementsFromTrajectory(Trajectory trajectory) {
// Create a double[] of elements and fill it from the trajectory states.
double[] elements = new double[trajectory.getStates().size() * 7];
for (int i = 0; i < trajectory.getStates().size() * 7; i += 7) {
var state = trajectory.getStates().get(i / 7);
elements[i] = state.timeSeconds;
elements[i + 1] = state.velocityMetersPerSecond;
elements[i + 2] = state.accelerationMetersPerSecondSq;
elements[i + 3] = state.poseMeters.getX();
elements[i + 4] = state.poseMeters.getY();
elements[i + 5] = state.poseMeters.getRotation().getRadians();
elements[i + 6] = state.curvatureRadPerMeter;
}
return elements;
}
/**
* Imports a Trajectory from a PathWeaver-style JSON file.
*
* @param path the path of the json file to import from
* @param path The path of the json file to import from
* @return The trajectory represented by the file.
* @throws IOException if reading from the file fails
* @throws IOException if reading from the file fails.
*/
public static Trajectory fromPathweaverJson(Path path) throws IOException {
try (BufferedReader reader = Files.newBufferedReader(path)) {
Trajectory.State[] state = READER.readValue(reader);
return new Trajectory(Arrays.asList(state));
}
return createTrajectoryFromElements(WPIMathJNI.fromPathweaverJson(path.toString()));
}
/**
* Exports a Trajectory to a PathWeaver-style JSON file.
*
* @param trajectory the trajectory to export
* @param path the path of the file to export to
* @throws IOException if writing to the file fails
* @param trajectory The trajectory to export
* @param path The path of the file to export to
* @throws IOException if writing to the file fails.
*/
public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
Files.createDirectories(path.getParent());
try (BufferedWriter writer = Files.newBufferedWriter(path)) {
WRITER.writeValue(writer, trajectory.getStates().toArray(new Trajectory.State[0]));
}
WPIMathJNI.toPathweaverJson(getElementsFromTrajectory(trajectory), path.toString());
}
/**
* Deserializes a Trajectory from PathWeaver-style JSON.
*
* @param json the string containing the serialized JSON
* @param json The string containing the serialized JSON
* @return the trajectory represented by the JSON
* @throws JsonProcessingException if deserializing the JSON fails
* @throws TrajectorySerializationException if deserialization of the string fails.
*/
public static Trajectory deserializeTrajectory(String json) throws JsonProcessingException {
Trajectory.State[] state = READER.readValue(json);
return new Trajectory(Arrays.asList(state));
public static Trajectory deserializeTrajectory(String json) {
return createTrajectoryFromElements(WPIMathJNI.deserializeTrajectory(json));
}
/**
* Serializes a Trajectory to PathWeaver-style JSON.
*
* @param trajectory the trajectory to export
* @return the string containing the serialized JSON
* @throws JsonProcessingException if serializing the Trajectory fails
* @param trajectory The trajectory to export
* @return The string containing the serialized JSON
* @throws TrajectorySerializationException if serialization of the trajectory fails.
*/
public static String serializeTrajectory(Trajectory trajectory) throws JsonProcessingException {
return WRITER.writeValueAsString(trajectory.getStates().toArray(new Trajectory.State[0]));
public static String serializeTrajectory(Trajectory trajectory) {
return WPIMathJNI.serializeTrajectory(getElementsFromTrajectory(trajectory));
}
public static class TrajectorySerializationException extends RuntimeException {
public TrajectorySerializationException(String message) {
super(message);
}
}
}

136
wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
View File

@@ -13,6 +13,7 @@
#include "Eigen/QR"
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "frc/trajectory/TrajectoryUtil.h"
#include "unsupported/Eigen/MatrixFunctions"
using namespace wpi::java;
@@ -43,6 +44,46 @@ bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
return true;
}
std::vector<double> GetElementsFromTrajectory(
const frc::Trajectory& trajectory) {
std::vector<double> elements;
elements.reserve(trajectory.States().size() * 7);
for (auto&& state : trajectory.States()) {
elements.push_back(state.t.to<double>());
elements.push_back(state.velocity.to<double>());
elements.push_back(state.acceleration.to<double>());
elements.push_back(state.pose.X().to<double>());
elements.push_back(state.pose.Y().to<double>());
elements.push_back(state.pose.Rotation().Radians().to<double>());
elements.push_back(state.curvature.to<double>());
}
return elements;
}
frc::Trajectory CreateTrajectoryFromElements(wpi::ArrayRef<double> elements) {
// Make sure that the elements have the correct length.
assert(elements.size() % 7 == 0);
// Create a vector of states from the elements.
std::vector<frc::Trajectory::State> states;
states.reserve(elements.size() / 7);
for (size_t i = 0; i < elements.size(); i += 7) {
states.emplace_back(frc::Trajectory::State{
units::second_t{elements[i]},
units::meters_per_second_t{elements[i + 1]},
units::meters_per_second_squared_t{elements[i + 2]},
frc::Pose2d{units::meter_t{elements[i + 3]},
units::meter_t{elements[i + 4]},
units::radian_t{elements[i + 5]}},
units::curvature_t{elements[i + 6]}});
}
return frc::Trajectory(states);
}
extern "C" {
/*
@@ -163,4 +204,99 @@ Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
return isStabilizable;
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: fromPathweaverJson
* Signature: (Ljava/lang/String;)[D
*/
JNIEXPORT jdoubleArray JNICALL
Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
(JNIEnv* env, jclass, jstring path)
{
try {
auto trajectory =
frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
std::vector<double> elements = GetElementsFromTrajectory(trajectory);
return MakeJDoubleArray(env, elements);
} catch (std::exception& e) {
jclass cls = env->FindClass("java/lang/IOException");
if (cls) {
env->ThrowNew(cls, e.what());
}
return nullptr;
}
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: toPathweaverJson
* Signature: ([DLjava/lang/String;)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
(JNIEnv* env, jclass, jdoubleArray elements, jstring path)
{
try {
auto trajectory =
CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
frc::TrajectoryUtil::ToPathweaverJson(trajectory,
JStringRef{env, path}.c_str());
} catch (std::exception& e) {
jclass cls = env->FindClass("java/lang/IOException");
if (cls) {
env->ThrowNew(cls, e.what());
}
}
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: deserializeTrajectory
* Signature: (Ljava/lang/String;)[D
*/
JNIEXPORT jdoubleArray JNICALL
Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
(JNIEnv* env, jclass, jstring json)
{
try {
auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
JStringRef{env, json}.c_str());
std::vector<double> elements = GetElementsFromTrajectory(trajectory);
return MakeJDoubleArray(env, elements);
} catch (std::exception& e) {
jclass cls = env->FindClass(
"edu/wpi/first/wpilibj/trajectory/TrajectoryUtil$"
"TrajectorySerializationException");
if (cls) {
env->ThrowNew(cls, e.what());
}
return nullptr;
}
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: serializeTrajectory
* Signature: ([D)Ljava/lang/String;
*/
JNIEXPORT jstring JNICALL
Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
(JNIEnv* env, jclass, jdoubleArray elements)
{
try {
auto trajectory =
CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
return MakeJString(env,
frc::TrajectoryUtil::SerializeTrajectory(trajectory));
} catch (std::exception& e) {
jclass cls = env->FindClass(
"edu/wpi/first/wpilibj/trajectory/TrajectoryUtil$"
"TrajectorySerializationException");
if (cls) {
env->ThrowNew(cls, e.what());
}
return nullptr;
}
}
} // extern "C"

6
wpimath/src/main/native/include/frc/LinearFilter.h
View File

@@ -104,6 +104,9 @@ class LinearFilter {
* y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
* where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
*
* Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency
* above which the input starts to attenuate.
*
* This filter is stable for time constants greater than zero.
*
* @param timeConstant The discrete-time time constant in seconds.
@@ -121,6 +124,9 @@ class LinearFilter {
* y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
* where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
*
* Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency
* below which the input starts to attenuate.
*
* This filter is stable for time constants greater than zero.
*
* @param timeConstant The discrete-time time constant in seconds.

21
wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
View File

@@ -52,6 +52,27 @@ class CubicHermiteSpline : public Spline<3> {
* @return The hermite basis matrix for cubic hermite spline interpolation.
*/
static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
// the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P(i+1) = P(1) = a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
//
// [ P(i) ] = [ 0 0 0 1 ][ a3 ]
// [ P'(i) ] = [ 0 0 1 0 ][ a2 ]
// [ P(i+1) ] = [ 1 1 1 1 ][ a1 ]
// [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
//
// To solve for the coefficients, we can invert the 4x4 matrix and move it
// to the other side of the equation.
//
// [ a3 ] = [ 2 1 -2 1 ][ P(i) ]
// [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ]
// [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
// [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
// clang-format off
static auto basis = (Eigen::Matrix<double, 4, 4>() <<
+2.0, +1.0, -2.0, +1.0,

30
wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
View File

@@ -52,10 +52,38 @@ class QuinticHermiteSpline : public Spline<5> {
* @return The hermite basis matrix for quintic hermite spline interpolation.
*/
static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
// Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
// vectors, we want to find the coefficients of the spline
// P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P''(i) = P''(0) = 2 * a2
// P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
// P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
//
// [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ]
// [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ]
// [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ]
// [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ]
// [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ]
// [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ]
//
// To solve for the coefficients, we can invert the 6x6 matrix and move it
// to the other side of the equation.
//
// [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ]
// [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ]
// [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ]
// [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ]
// [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ]
// [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ]
// clang-format off
static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
-06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
+15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
+15.0, +08.0, +01.5, -15.0, +07.0, -01.0,
-10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
+00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+00.0, +01.0, +00.0, +00.0, +00.0, +00.0,

16
wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
View File

@@ -36,19 +36,19 @@ class TrajectoryUtil {
static Trajectory FromPathweaverJson(const wpi::Twine& path);
/**
* Deserializes a Trajectory from PathWeaver-style JSON.
* @param json the string containing the serialized JSON
* @return the trajectory represented by the JSON
*/
* Deserializes a Trajectory from PathWeaver-style JSON.
*
* @param json the string containing the serialized JSON
*
* @return the trajectory represented by the JSON
*/
static std::string SerializeTrajectory(const Trajectory& trajectory);
/**
* Serializes a Trajectory to PathWeaver-style JSON.
*
* @param trajectory the trajectory to export
*
* @return the string containing the serialized JSON
*/
static Trajectory DeserializeTrajectory(wpi::StringRef json_str);

2
wpiutil/src/main/native/include/wpi/circular_buffer.inc
View File

@@ -11,7 +11,7 @@
namespace wpi {
template <class T>
circular_buffer<T>::circular_buffer(size_t size) : m_data(size, T{0}) {}
circular_buffer<T>::circular_buffer(size_t size) : m_data(size, T{}) {}
/**
* Returns number of elements in buffer

7
wpiutil/src/main/native/unix/StackTrace.cpp
View File

@@ -26,8 +26,11 @@ std::string GetStackTrace(int offset) {
// extract just function name from "pathToExe(functionName+offset)"
StringRef sym{mangledSymbols[i]};
sym = sym.split('(').second;
sym = sym.split('+').first;
trace << "\tat " << Demangle(sym) << "\n";
StringRef offset;
std::tie(sym, offset) = sym.split('+');
StringRef addr;
std::tie(offset, addr) = offset.split(')');
trace << "\tat " << Demangle(sym) << " + " << offset << addr << "\n";
}
}