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

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

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();

97
wpilibc/src/main/native/cpp/romi/RomiGyro.cpp
View File

@@ -1,97 +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.
#include "frc/romi/RomiGyro.h"
#include "frc/smartdashboard/SendableBuilder.h"
using namespace frc;
RomiGyro::RomiGyro() : m_simDevice("Gyro:RomiGyro") {
if (m_simDevice) {
m_simDevice.CreateBoolean("init", hal::SimDevice::kOutput, true);
m_simRateX =
m_simDevice.CreateDouble("rate_x", hal::SimDevice::kInput, 0.0);
m_simRateY =
m_simDevice.CreateDouble("rate_y", hal::SimDevice::kInput, 0.0);
m_simRateZ =
m_simDevice.CreateDouble("rate_z", hal::SimDevice::kInput, 0.0);
m_simAngleX =
m_simDevice.CreateDouble("angle_x", hal::SimDevice::kInput, 0.0);
m_simAngleY =
m_simDevice.CreateDouble("angle_y", hal::SimDevice::kInput, 0.0);
m_simAngleZ =
m_simDevice.CreateDouble("angle_z", hal::SimDevice::kInput, 0.0);
}
}
double RomiGyro::GetAngle() const {
return GetAngleZ();
}
double RomiGyro::GetRate() const {
return GetRateZ();
}
double RomiGyro::GetRateX() const {
if (m_simRateX) {
return m_simRateX.Get();
}
return 0.0;
}
double RomiGyro::GetRateY() const {
if (m_simRateY) {
return m_simRateY.Get();
}
return 0.0;
}
double RomiGyro::GetRateZ() const {
if (m_simRateZ) {
return m_simRateZ.Get();
}
return 0.0;
}
double RomiGyro::GetAngleX() const {
if (m_simAngleX) {
return m_simAngleX.Get() - m_angleXOffset;
}
return 0.0;
}
double RomiGyro::GetAngleY() const {
if (m_simAngleY) {
return m_simAngleY.Get() - m_angleYOffset;
}
return 0.0;
}
double RomiGyro::GetAngleZ() const {
if (m_simAngleZ) {
return m_simAngleZ.Get() - m_angleZOffset;
}
return 0.0;
}
void RomiGyro::Reset() {
if (m_simAngleX) {
m_angleXOffset = m_simAngleX.Get();
m_angleYOffset = m_simAngleY.Get();
m_angleZOffset = m_simAngleZ.Get();
}
}
void RomiGyro::InitSendable(SendableBuilder& builder) {
builder.SetSmartDashboardType("Gyro");
builder.AddDoubleProperty(
"Value", [=]() { return GetAngle(); }, nullptr);
}

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