Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11: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

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

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.
*