Squashed 'third_party/allwpilib_2019/' changes from e20d96ea4e..b0167e6337
32c62449be Add ArrayRef overloads to new command classes (#2216)
6190fcb237 Run wpiformat (#2218)
012d93b2bd Use an explicit stack instead of recursion when parameterizing splines (#2197)
222669dc2c Fix trapezoidal profile PID controller setpoint bug (#2210)
abe25b795b TrajectoryUtil.toPathweaverJson: Create parent directories (#2214)
354185189c Update ProjectYear to 2020 (#2212)
f14fe434a1 Add (Old) qualifier to old subsystem (#2211)
e874ba9313 Add Color classes for use with AddressableLED (#2127)
96348e835a Fix C++ SendableRegistry::AddChild() (#2207)
d91796f8d2 fix clang-format version number (#2206)
9abce8eb06 Fix subsystem LiveWindow usage (#2202)
8b4508ad53 Use default path for networktables.ini in simulation (#2205)
5b7dd186d2 Add templates for new commands for vscode plugin (#2016)
6ea13ea8f3 ntcore: Add support for local-only operation (#2204)
44bcf7fb4d Java examples: use non-static imports for constants (#2191)
c7a1dfc0bc Add SlewRateLimiter class (#2192)
a12bb447e4 Fail cmake build if python3 generate_numbers.py fails (#2203)
c4bd54ef44 Add JNI binding to suppress driver station error/warning messages (#2200)
f9a11cce5e Remove -no-module-directories flag from javadoc build (#2201)
6008671c30 Report WPILib version as part of usage reporting (#2199)
7b952d599d Add usage reporting for many new things (#2184)
93cdf68694 Add Constants.cpp for MecanumControllerCommand example (#2196)
0c6f24562f Fix bug in ULEB128 decoding (#2195)
bdc1cab013 Add support for configuring SPI Auto Stall Config (#2193)
3259cffc63 Add transform methods to Trajectory (#2187)
67b59f2b31 Minor improvements/fixes to new command framework (#2186)
1ce24a7a2f Add 2020 speed controllers (#2188)
635882a9f7 Add getter for initial pose in Trajectory (#2180)
71a22861eb Use ManagedStatic for CameraServer (#2174)
9cb69c5b46 Add a way to pass in a preconstructed value to ManagedStatic (#2175)
5e08bb28f8 Add docs and lifecycle tasks for faster dev builds (#2182)
ea4d1a39e1 Update characterization values to match real robot (#2183)
31b588d961 Fix ArmFeedforward Javadocs (#2176)
0b80d566ad Use ChipObject HMB function for LED (#2173)
f8294e689b Sim GUI: Add a bit of spacing to the analog inputs (#2170)
b78f115fcf Work around VS2019 16.4.0 bugs (#2171)
b468c51251 Change AddressableLED example to use consistent PWM port (#2168)
023c088290 Add toString() to relevant kinematics classes (#2160)
8a11d13a39 Fix C++ DutyCycleEncoder int constructor (#2166)
daa81c64a7 Minor javadoc fix in SwerveDriveKinematicsConstraint (#2167)
Change-Id: Ied6a4d039f2b95381e1d2124fcc70d52580cc165
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: b0167e6337135545e7053acb89dd5726accc7dec
diff --git a/wpilibc/build.gradle b/wpilibc/build.gradle
index 8730824..4ee5766 100644
--- a/wpilibc/build.gradle
+++ b/wpilibc/build.gradle
@@ -231,6 +231,8 @@
}
}
+apply from: "${rootDir}/shared/cppDesktopTestTask.gradle"
+
tasks.withType(RunTestExecutable) {
args "--gtest_output=xml:test_detail.xml"
outputs.dir outputDir
diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
index 728b1de..ea054ce 100644
--- a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
+++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -20,7 +20,9 @@
: m_dutyCycle{std::make_shared<DutyCycle>(
std::make_shared<DigitalInput>(channel))},
m_analogTrigger{m_dutyCycle.get()},
- m_counter{} {}
+ m_counter{} {
+ Init();
+}
DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
: m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}},
diff --git a/wpilibc/src/main/native/cpp/PWMTalonFX.cpp b/wpilibc/src/main/native/cpp/PWMTalonFX.cpp
new file mode 100644
index 0000000..09b7163
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWMTalonFX.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMTalonFX.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMTalonFX::PWMTalonFX(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_TalonFX, GetChannel() + 1);
+ SendableRegistry::GetInstance().SetName(this, "PWMTalonFX", GetChannel());
+}
diff --git a/wpilibc/src/main/native/cpp/PWMVenom.cpp b/wpilibc/src/main/native/cpp/PWMVenom.cpp
new file mode 100644
index 0000000..9fa14b7
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/PWMVenom.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/PWMVenom.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/smartdashboard/SendableRegistry.h"
+
+using namespace frc;
+
+PWMVenom::PWMVenom(int channel) : PWMSpeedController(channel) {
+ SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetSpeed(0.0);
+ SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_FusionVenom, GetChannel() + 1);
+ SendableRegistry::GetInstance().SetName(this, "PWMVenom", GetChannel());
+}
diff --git a/wpilibc/src/main/native/cpp/SPI.cpp b/wpilibc/src/main/native/cpp/SPI.cpp
index 074cc79..d51fa3b 100644
--- a/wpilibc/src/main/native/cpp/SPI.cpp
+++ b/wpilibc/src/main/native/cpp/SPI.cpp
@@ -311,6 +311,14 @@
return val;
}
+void SPI::ConfigureAutoStall(HAL_SPIPort port, int csToSclkTicks,
+ int stallTicks, int pow2BytesPerRead) {
+ int32_t status = 0;
+ HAL_ConfigureSPIAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead,
+ &status);
+ wpi_setHALError(status);
+}
+
void SPI::InitAccumulator(units::second_t period, int cmd, int xferSize,
int validMask, int validValue, int dataShift,
int dataSize, bool isSigned, bool bigEndian) {
diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
index 264859f..3abd4ba 100644
--- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
@@ -22,7 +22,7 @@
: m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
static int instances = 0;
instances++;
- HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
+ HAL_Report(HALUsageReporting::kResourceType_PIDController2, instances);
frc::SendableRegistry::GetInstance().Add(this, "PIDController", instances);
}
diff --git a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
new file mode 100644
index 0000000..a0fffc6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/controller/ProfiledPIDController.h"
+
+#include <hal/FRCUsageReporting.h>
+
+void frc::detail::ReportProfiledPIDController() {
+ static int instances = 0;
+ ++instances;
+ HAL_Report(HALUsageReporting::kResourceType_ProfiledPIDController, instances);
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index 8591da4..07a8e3b 100644
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -7,6 +7,8 @@
#include "frc/kinematics/DifferentialDriveOdometry.h"
+#include <hal/FRCUsageReporting.h>
+
using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
@@ -14,6 +16,8 @@
: m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
+ HAL_Report(HALUsageReporting::kResourceType_Odometry,
+ HALUsageReporting::kOdometry_DifferentialDrive);
}
const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index 615635a..3843483 100644
--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -7,6 +7,8 @@
#include "frc/kinematics/MecanumDriveOdometry.h"
+#include <hal/FRCUsageReporting.h>
+
using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
@@ -15,6 +17,8 @@
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
+ HAL_Report(HALUsageReporting::kResourceType_Odometry,
+ HALUsageReporting::kOdometry_MecanumDrive);
}
const Pose2d& MecanumDriveOdometry::UpdateWithTime(
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
index bce6196..9d57ad6 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
@@ -134,6 +134,12 @@
comp.subsystem = subsystem.str();
}
+void SendableRegistry::AddChild(Sendable* parent, Sendable* child) {
+ std::scoped_lock lock(m_impl->mutex);
+ auto& comp = m_impl->GetOrAdd(child);
+ comp.parent = parent;
+}
+
void SendableRegistry::AddChild(Sendable* parent, void* child) {
std::scoped_lock lock(m_impl->mutex);
auto& comp = m_impl->GetOrAdd(child);
@@ -147,6 +153,10 @@
UID compUid = it->getSecond();
m_impl->components.erase(compUid - 1);
m_impl->componentMap.erase(it);
+ // update any parent pointers
+ for (auto&& comp : m_impl->components) {
+ if (comp->parent == sendable) comp->parent = nullptr;
+ }
return true;
}
@@ -164,6 +174,10 @@
comp.builder.ClearProperties();
to->InitSendable(comp.builder);
}
+ // update any parent pointers
+ for (auto&& comp : m_impl->components) {
+ if (comp->parent == from) comp->parent = to;
+ }
}
bool SendableRegistry::Contains(const Sendable* sendable) const {
diff --git a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
index 7b2b2b6..84d4f37 100644
--- a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -107,6 +107,33 @@
(t - prevSample.t) / (sample.t - prevSample.t));
}
+Trajectory Trajectory::TransformBy(const Transform2d& transform) {
+ auto& firstState = m_states[0];
+ auto& firstPose = firstState.pose;
+
+ // Calculate the transformed first pose.
+ auto newFirstPose = firstPose + transform;
+ auto newStates = m_states;
+ newStates[0].pose = newFirstPose;
+
+ for (unsigned int i = 1; i < newStates.size(); i++) {
+ auto& state = newStates[i];
+ // We are transforming relative to the coordinate frame of the new initial
+ // pose.
+ state.pose = newFirstPose + (state.pose - firstPose);
+ }
+
+ return Trajectory(newStates);
+}
+
+Trajectory Trajectory::RelativeTo(const Pose2d& pose) {
+ auto newStates = m_states;
+ for (auto& state : newStates) {
+ state.pose = state.pose.RelativeTo(pose);
+ }
+ return Trajectory(newStates);
+}
+
void frc::to_json(wpi::json& json, const Trajectory::State& state) {
json = wpi::json{{"time", state.t.to<double>()},
{"velocity", state.velocity.to<double>()},
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
index 92c21f6..3c95472 100644
--- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -9,11 +9,16 @@
#include <utility>
+#include "frc/DriverStation.h"
#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
#include "frc/trajectory/TrajectoryParameterizer.h"
using namespace frc;
+const Trajectory TrajectoryGenerator::kDoNothingTrajectory(
+ std::vector<Trajectory::State>{Trajectory::State()});
+
Trajectory TrajectoryGenerator::GenerateTrajectory(
Spline<3>::ControlVector initial,
const std::vector<Translation2d>& interiorWaypoints,
@@ -29,9 +34,15 @@
end.y[1] *= -1;
}
- auto points =
- SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
- initial, interiorWaypoints, end));
+ std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+ try {
+ points =
+ SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
+ initial, interiorWaypoints, end));
+ } catch (SplineParameterizer::MalformedSplineException& e) {
+ DriverStation::ReportError(e.what());
+ return kDoNothingTrajectory;
+ }
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
@@ -68,8 +79,14 @@
}
}
- auto points = SplinePointsFromSplines(
- SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
+ std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+ try {
+ points = SplinePointsFromSplines(
+ SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
+ } catch (SplineParameterizer::MalformedSplineException& e) {
+ DriverStation::ReportError(e.what());
+ return kDoNothingTrajectory;
+ }
// After trajectory generation, flip theta back so it's relative to the
// field. Also fix curvature.
diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp
index 5fd824d..eaf4ddc 100644
--- a/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -36,7 +36,7 @@
return -1;
}
HAL_Report(HALUsageReporting::kResourceType_Language,
- HALUsageReporting::kLanguage_CPlusPlus);
+ HALUsageReporting::kLanguage_CPlusPlus, 0, GetWPILibVersion());
wpi::outs() << "\n********** Robot program starting **********\n";
return 0;
}
@@ -124,7 +124,11 @@
auto inst = nt::NetworkTableInstance::GetDefault();
inst.SetNetworkIdentity("Robot");
+#ifdef __FRC_ROBORIO__
inst.StartServer("/home/lvuser/networktables.ini");
+#else
+ inst.StartServer();
+#endif
SmartDashboard::init();
diff --git a/wpilibc/src/main/native/include/frc/AddressableLED.h b/wpilibc/src/main/native/include/frc/AddressableLED.h
index 2ffe0ea..5534ae4 100644
--- a/wpilibc/src/main/native/include/frc/AddressableLED.h
+++ b/wpilibc/src/main/native/include/frc/AddressableLED.h
@@ -15,6 +15,8 @@
#include <wpi/ArrayRef.h>
#include "frc/ErrorBase.h"
+#include "util/Color.h"
+#include "util/Color8Bit.h"
namespace frc {
@@ -54,6 +56,28 @@
* @param v the v value [0-255]
*/
void SetHSV(int h, int s, int v);
+
+ /*
+ * Sets a specific LED in the buffer.
+ *
+ * @param color The color of the LED
+ */
+ void SetLED(const Color& color) {
+ this->r = color.red * 255;
+ this->g = color.green * 255;
+ this->b = color.blue * 255;
+ }
+
+ /*
+ * Sets a specific LED in the buffer.
+ *
+ * @param color The color of the LED
+ */
+ void SetLED(const Color8Bit& color) {
+ this->r = color.red;
+ this->g = color.green;
+ this->b = color.blue;
+ }
};
/**
diff --git a/wpilibc/src/main/native/include/frc/PWMTalonFX.h b/wpilibc/src/main/native/include/frc/PWMTalonFX.h
new file mode 100644
index 0000000..d85c7ca
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/PWMTalonFX.h
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/PWMSpeedController.h"
+
+namespace frc {
+
+/**
+ * Cross the Road Electronics (CTRE) Talon FX Speed Controller with PWM
+ * control.
+ *
+ * Note that the Talon FX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Talon FX User
+ * Manual available from Cross The Road Electronics.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
+ */
+class PWMTalonFX : public PWMSpeedController {
+ public:
+ /**
+ * Construct a Talon FX connected via PWM.
+ *
+ * @param channel The PWM channel that the Talon FX is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+ explicit PWMTalonFX(int channel);
+
+ PWMTalonFX(PWMTalonFX&&) = default;
+ PWMTalonFX& operator=(PWMTalonFX&&) = default;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/PWMVenom.h b/wpilibc/src/main/native/include/frc/PWMVenom.h
new file mode 100644
index 0000000..189db43
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/PWMVenom.h
@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "frc/PWMSpeedController.h"
+
+namespace frc {
+
+/**
+ * Playing with Fusion Venom Smart Motor with PWM control.
+ *
+ * Note that the Venom uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
+ */
+class PWMVenom : public PWMSpeedController {
+ public:
+ /**
+ * Construct a Venom connected via PWM.
+ *
+ * @param channel The PWM channel that the Venom is attached to. 0-9 are
+ * on-board, 10-19 are on the MXP port
+ */
+ explicit PWMVenom(int channel);
+
+ PWMVenom(PWMVenom&&) = default;
+ PWMVenom& operator=(PWMVenom&&) = default;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/SPI.h b/wpilibc/src/main/native/include/frc/SPI.h
index fb4835e..8e721bc 100644
--- a/wpilibc/src/main/native/include/frc/SPI.h
+++ b/wpilibc/src/main/native/include/frc/SPI.h
@@ -270,6 +270,19 @@
int GetAutoDroppedCount();
/**
+ * Configure the Auto SPI Stall time between reads.
+ *
+ * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ * MXP.
+ * @param csToSclkTicks the number of ticks to wait before asserting the cs
+ * pin
+ * @param stallTicks the number of ticks to stall for
+ * @param pow2BytesPerRead the number of bytes to read before stalling
+ */
+ void ConfigureAutoStall(HAL_SPIPort port, int csToSclkTicks, int stallTicks,
+ int pow2BytesPerRead);
+
+ /**
* Initialize the accumulator.
*
* @param period Time between reads
diff --git a/wpilibc/src/main/native/include/frc/SlewRateLimiter.h b/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
new file mode 100644
index 0000000..7236a30
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <frc2/Timer.h>
+
+#include <algorithm>
+
+#include <units/units.h>
+
+namespace frc {
+/**
+ * A class that limits the rate of change of an input value. Useful for
+ * implementing voltage, setpoint, and/or output ramps. A slew-rate limit
+ * is most appropriate when the quantity being controlled is a velocity or
+ * a voltage; when controlling a position, consider using a TrapezoidProfile
+ * instead.
+ *
+ * @see TrapezoidProfile
+ */
+template <class Unit>
+class SlewRateLimiter {
+ using Unit_t = units::unit_t<Unit>;
+ using Rate = units::compound_unit<Unit, units::inverse<units::seconds>>;
+ using Rate_t = units::unit_t<Rate>;
+
+ public:
+ /**
+ * Creates a new SlewRateLimiter with the given rate limit and initial value.
+ *
+ * @param rateLimit The rate-of-change limit.
+ * @param initialValue The initial value of the input.
+ */
+ explicit SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue = Unit_t{0})
+ : m_rateLimit{rateLimit}, m_prevVal{initialValue} {
+ m_timer.Start();
+ }
+
+ /**
+ * Filters the input to limit its slew rate.
+ *
+ * @param input The input value whose slew rate is to be limited.
+ * @return The filtered value, which will not change faster than the slew
+ * rate.
+ */
+ Unit_t Calculate(Unit_t input) {
+ m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * m_timer.Get(),
+ m_rateLimit * m_timer.Get());
+ m_timer.Reset();
+ return m_prevVal;
+ }
+
+ /**
+ * Resets the slew rate limiter to the specified value; ignores the rate limit
+ * when doing so.
+ *
+ * @param value The value to reset to.
+ */
+ void Reset(Unit_t value) {
+ m_timer.Reset();
+ m_prevVal = value;
+ }
+
+ private:
+ frc2::Timer m_timer;
+ Rate_t m_rateLimit;
+ Unit_t m_prevVal;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
index 96fd331..079d96e 100644
--- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -21,6 +21,9 @@
#include "frc/trajectory/TrapezoidProfile.h"
namespace frc {
+namespace detail {
+void ReportProfiledPIDController();
+} // namespace detail
/**
* Implements a PID control loop whose setpoint is constrained by a trapezoid
@@ -43,7 +46,8 @@
public:
/**
* Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
- * Kd.
+ * Kd. Users should call reset() when they first start running the controller
+ * to avoid unwanted behavior.
*
* @param Kp The proportional coefficient.
* @param Ki The integral coefficient.
@@ -54,7 +58,9 @@
*/
ProfiledPIDController(double Kp, double Ki, double Kd,
Constraints constraints, units::second_t period = 20_ms)
- : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
+ : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
+ detail::ReportProfiledPIDController();
+ }
~ProfiledPIDController() override = default;
@@ -287,9 +293,34 @@
}
/**
- * Reset the previous error, the integral term, and disable the controller.
+ * Reset the previous error and the integral term.
+ *
+ * @param measurement The current measured State of the system.
*/
- void Reset() { m_controller.Reset(); }
+ void Reset(const State& measurement) {
+ m_controller.Reset();
+ m_setpoint = measurement;
+ }
+
+ /**
+ * Reset the previous error and the integral term.
+ *
+ * @param measuredPosition The current measured position of the system.
+ * @param measuredVelocity The current measured velocity of the system.
+ */
+ void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
+ Reset(State{measuredPosition, measuredVelocity});
+ }
+
+ /**
+ * Reset the previous error and the integral term.
+ *
+ * @param measuredPosition The current measured position of the system. The
+ * velocity is assumed to be zero.
+ */
+ void Reset(Distance_t measuredPosition) {
+ Reset(measuredPosition, Velocity_t(0));
+ }
void InitSendable(frc::SendableBuilder& builder) override {
builder.SetSmartDashboardType("ProfiledPIDController");
diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index cb2121f..e986029 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -7,6 +7,7 @@
#pragma once
+#include <hal/FRCUsageReporting.h>
#include <units/units.h>
#include "frc/kinematics/ChassisSpeeds.h"
@@ -31,8 +32,11 @@
* empirical value may be larger than the physical measured value due to
* scrubbing effects.
*/
- constexpr explicit DifferentialDriveKinematics(units::meter_t trackWidth)
- : trackWidth(trackWidth) {}
+ explicit DifferentialDriveKinematics(units::meter_t trackWidth)
+ : trackWidth(trackWidth) {
+ HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+ HALUsageReporting::kKinematics_DifferentialDrive);
+ }
/**
* Returns a chassis speed from left and right component velocities using
diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
index 6b0329d..47b1b34 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -9,6 +9,7 @@
#include <Eigen/Core>
#include <Eigen/QR>
+#include <hal/FRCUsageReporting.h>
#include "frc/geometry/Translation2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
@@ -63,6 +64,8 @@
SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel,
rearRightWheel);
m_forwardKinematics = m_inverseKinematics.householderQr();
+ HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+ HALUsageReporting::kKinematics_MecanumDrive);
}
MecanumDriveKinematics(const MecanumDriveKinematics&) = default;
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index 1bdbcea..f889363 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -12,6 +12,7 @@
#include <Eigen/Core>
#include <Eigen/QR>
+#include <hal/FRCUsageReporting.h>
#include <units/units.h>
#include "frc/geometry/Rotation2d.h"
@@ -71,6 +72,9 @@
}
m_forwardKinematics = m_inverseKinematics.householderQr();
+
+ HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+ HALUsageReporting::kKinematics_SwerveDrive);
}
SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 23852d0..e794388 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -7,6 +7,8 @@
#pragma once
+#include <hal/FRCUsageReporting.h>
+
namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
@@ -15,6 +17,8 @@
: m_kinematics(kinematics), m_pose(initialPose) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
+ HAL_Report(HALUsageReporting::kResourceType_Odometry,
+ HALUsageReporting::kOdometry_SwerveDrive);
}
template <size_t NumModules>
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h
index 53dbba3..78dabc6 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h
@@ -125,6 +125,15 @@
* @param parent parent object
* @param child child object
*/
+ void AddChild(Sendable* parent, Sendable* child);
+
+ /**
+ * Adds a child object to an object. Adds the child object to the registry
+ * if it's not already present.
+ *
+ * @param parent parent object
+ * @param child child object
+ */
void AddChild(Sendable* parent, void* child);
/**
diff --git a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
index 34cf026..d3a5e6e 100644
--- a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
+++ b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -33,10 +33,13 @@
#include <frc/spline/Spline.h>
+#include <stack>
+#include <string>
#include <utility>
#include <vector>
#include <units/units.h>
+#include <wpi/Twine.h>
namespace frc {
@@ -47,6 +50,11 @@
public:
using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
+ struct MalformedSplineException : public std::runtime_error {
+ explicit MalformedSplineException(const char* what_arg)
+ : runtime_error(what_arg) {}
+ };
+
/**
* Parameterizes the spline. This method breaks up the spline into various
* arcs until their dx, dy, and dtheta are within specific tolerances.
@@ -64,14 +72,48 @@
static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
double t0 = 0.0,
double t1 = 1.0) {
- std::vector<PoseWithCurvature> arr;
+ std::vector<PoseWithCurvature> splinePoints;
- // The parameterization does not add the first initial point. Let's add
- // that.
- arr.push_back(spline.GetPoint(t0));
+ // The parameterization does not add the initial point. Let's add that.
+ splinePoints.push_back(spline.GetPoint(t0));
- GetSegmentArc(spline, &arr, t0, t1);
- return arr;
+ // We use an "explicit stack" to simulate recursion, instead of a recursive
+ // function call This give us greater control, instead of a stack overflow
+ std::stack<StackContents> stack;
+ stack.emplace(StackContents{t0, t1});
+
+ StackContents current;
+ PoseWithCurvature start;
+ PoseWithCurvature end;
+ int iterations = 0;
+
+ while (!stack.empty()) {
+ current = stack.top();
+ stack.pop();
+ start = spline.GetPoint(current.t0);
+ end = spline.GetPoint(current.t1);
+
+ const auto twist = start.first.Log(end.first);
+
+ if (units::math::abs(twist.dy) > kMaxDy ||
+ units::math::abs(twist.dx) > kMaxDx ||
+ units::math::abs(twist.dtheta) > kMaxDtheta) {
+ stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
+ stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
+ } else {
+ splinePoints.push_back(spline.GetPoint(current.t1));
+ }
+
+ if (iterations++ >= kMaxIterations) {
+ throw MalformedSplineException(
+ "Could not parameterize a malformed spline. "
+ "This means that you probably had two or more adjacent "
+ "waypoints that were very close together with headings "
+ "in opposing directions.");
+ }
+ }
+
+ return splinePoints;
}
private:
@@ -80,33 +122,19 @@
static constexpr units::meter_t kMaxDy = 0.05_in;
static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
+ struct StackContents {
+ double t0;
+ double t1;
+ };
+
/**
- * Breaks up the spline into arcs until the dx, dy, and theta of each arc is
- * within tolerance.
- *
- * @param spline The spline to parameterize.
- * @param vector Pointer to vector of poses.
- * @param t0 Starting point for arc.
- * @param t1 Ending point for arc.
+ * A malformed spline does not actually explode the LIFO stack size. Instead,
+ * the stack size stays at a relatively small number (e.g. 30) and never
+ * decreases. Because of this, we must count iterations. Even long, complex
+ * paths don't usually go over 300 iterations, so hitting this maximum should
+ * definitely indicate something has gone wrong.
*/
- template <int Dim>
- static void GetSegmentArc(const Spline<Dim>& spline,
- std::vector<PoseWithCurvature>* vector, double t0,
- double t1) {
- const auto start = spline.GetPoint(t0);
- const auto end = spline.GetPoint(t1);
-
- const auto twist = start.first.Log(end.first);
-
- if (units::math::abs(twist.dy) > kMaxDy ||
- units::math::abs(twist.dx) > kMaxDx ||
- units::math::abs(twist.dtheta) > kMaxDtheta) {
- GetSegmentArc(spline, vector, t0, (t0 + t1) / 2);
- GetSegmentArc(spline, vector, (t0 + t1) / 2, t1);
- } else {
- vector->push_back(spline.GetPoint(t1));
- }
- }
+ static constexpr int kMaxIterations = 5000;
friend class CubicHermiteSplineTest;
friend class QuinticHermiteSplineTest;
diff --git a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
index 96c2c6e..13e32d6 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
@@ -12,6 +12,7 @@
#include <units/units.h>
#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Transform2d.h"
namespace wpi {
class json;
@@ -105,6 +106,34 @@
*/
State Sample(units::second_t t) const;
+ /**
+ * Transforms all poses in the trajectory by the given transform. This is
+ * useful for converting a robot-relative trajectory into a field-relative
+ * trajectory. This works with respect to the first pose in the trajectory.
+ *
+ * @param transform The transform to transform the trajectory by.
+ * @return The transformed trajectory.
+ */
+ Trajectory TransformBy(const Transform2d& transform);
+
+ /**
+ * Transforms all poses in the trajectory so that they are relative to the
+ * given pose. This is useful for converting a field-relative trajectory
+ * into a robot-relative trajectory.
+ *
+ * @param pose The pose that is the origin of the coordinate frame that
+ * the current trajectory will be transformed into.
+ * @return The transformed trajectory.
+ */
+ Trajectory RelativeTo(const Pose2d& pose);
+
+ /**
+ * Returns the initial pose of the trajectory.
+ *
+ * @return The initial pose of the trajectory.
+ */
+ Pose2d InitialPose() const { return Sample(0_s).pose; }
+
private:
std::vector<State> m_states;
units::second_t m_totalTime;
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
index 4d6eff6..40e7e7b 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -113,5 +113,8 @@
}
return splinePoints;
}
+
+ private:
+ static const Trajectory kDoNothingTrajectory;
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index 0479cfe..575088e 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -7,6 +7,7 @@
#pragma once
+#include <hal/FRCUsageReporting.h>
#include <units/units.h>
namespace frc {
@@ -53,6 +54,13 @@
public:
class Constraints {
public:
+ Constraints() {
+ HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
+ }
+ Constraints(Velocity_t maxVelocity_, Acceleration_t maxAcceleration_)
+ : maxVelocity{maxVelocity_}, maxAcceleration{maxAcceleration_} {
+ HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
+ }
Velocity_t maxVelocity{0};
Acceleration_t maxAcceleration{0};
};
diff --git a/wpilibc/src/main/native/include/frc/util/Color.h b/wpilibc/src/main/native/include/frc/util/Color.h
new file mode 100644
index 0000000..2538d60
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/util/Color.h
@@ -0,0 +1,949 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+
+namespace frc {
+
+/**
+ * Represents colors that can be used with Addressable LEDs.
+ *
+ * Limited to 12 bits of precision.
+ */
+class Color {
+ public:
+ /*
+ * FIRST Colors
+ */
+
+ /**
+ * #1560BD.
+ */
+ static const Color kDenim;
+
+ /**
+ * #0066B3.
+ */
+ static const Color kFirstBlue;
+
+ /**
+ * #ED1C24.
+ */
+ static const Color kFirstRed;
+
+ /*
+ * Standard Colors
+ */
+
+ /**
+ * #F0F8FF.
+ */
+ static const Color kAliceBlue;
+
+ /**
+ * #FAEBD7.
+ */
+ static const Color kAntiqueWhite;
+
+ /**
+ * #00FFFF.
+ */
+ static const Color kAqua;
+
+ /**
+ * #7FFFD4.
+ */
+ static const Color kAquamarine;
+
+ /**
+ * #F0FFFF.
+ */
+ static const Color kAzure;
+
+ /**
+ * #F5F5DC.
+ */
+ static const Color kBeige;
+
+ /**
+ * #FFE4C4.
+ */
+ static const Color kBisque;
+
+ /**
+ * #000000.
+ */
+ static const Color kBlack;
+
+ /**
+ * #FFEBCD.
+ */
+ static const Color kBlanchedAlmond;
+
+ /**
+ * #0000FF.
+ */
+ static const Color kBlue;
+
+ /**
+ * #8A2BE2.
+ */
+ static const Color kBlueViolet;
+
+ /**
+ * #A52A2A.
+ */
+ static const Color kBrown;
+
+ /**
+ * #DEB887.
+ */
+ static const Color kBurlywood;
+
+ /**
+ * #5F9EA0.
+ */
+ static const Color kCadetBlue;
+
+ /**
+ * #7FFF00.
+ */
+ static const Color kChartreuse;
+
+ /**
+ * #D2691E.
+ */
+ static const Color kChocolate;
+
+ /**
+ * #FF7F50.
+ */
+ static const Color kCoral;
+
+ /**
+ * #6495ED.
+ */
+ static const Color kCornflowerBlue;
+
+ /**
+ * #FFF8DC.
+ */
+ static const Color kCornsilk;
+
+ /**
+ * #DC143C.
+ */
+ static const Color kCrimson;
+
+ /**
+ * #00FFFF.
+ */
+ static const Color kCyan;
+
+ /**
+ * #00008B.
+ */
+ static const Color kDarkBlue;
+
+ /**
+ * #008B8B.
+ */
+ static const Color kDarkCyan;
+
+ /**
+ * #B8860B.
+ */
+ static const Color kDarkGoldenrod;
+
+ /**
+ * #A9A9A9.
+ */
+ static const Color kDarkGray;
+
+ /**
+ * #006400.
+ */
+ static const Color kDarkGreen;
+
+ /**
+ * #BDB76B.
+ */
+ static const Color kDarkKhaki;
+
+ /**
+ * #8B008B.
+ */
+ static const Color kDarkMagenta;
+
+ /**
+ * #556B2F.
+ */
+ static const Color kDarkOliveGreen;
+
+ /**
+ * #FF8C00.
+ */
+ static const Color kDarkOrange;
+
+ /**
+ * #9932CC.
+ */
+ static const Color kDarkOrchid;
+
+ /**
+ * #8B0000.
+ */
+ static const Color kDarkRed;
+
+ /**
+ * #E9967A.
+ */
+ static const Color kDarkSalmon;
+
+ /**
+ * #8FBC8F.
+ */
+ static const Color kDarkSeaGreen;
+
+ /**
+ * #483D8B.
+ */
+ static const Color kDarkSlateBlue;
+
+ /**
+ * #2F4F4F.
+ */
+ static const Color kDarkSlateGray;
+
+ /**
+ * #00CED1.
+ */
+ static const Color kDarkTurquoise;
+
+ /**
+ * #9400D3.
+ */
+ static const Color kDarkViolet;
+
+ /**
+ * #FF1493.
+ */
+ static const Color kDeepPink;
+
+ /**
+ * #00BFFF.
+ */
+ static const Color kDeepSkyBlue;
+
+ /**
+ * #696969.
+ */
+ static const Color kDimGray;
+
+ /**
+ * #1E90FF.
+ */
+ static const Color kDodgerBlue;
+
+ /**
+ * #B22222.
+ */
+ static const Color kFirebrick;
+
+ /**
+ * #FFFAF0.
+ */
+ static const Color kFloralWhite;
+
+ /**
+ * #228B22.
+ */
+ static const Color kForestGreen;
+
+ /**
+ * #FF00FF.
+ */
+ static const Color kFuchsia;
+
+ /**
+ * #DCDCDC.
+ */
+ static const Color kGainsboro;
+
+ /**
+ * #F8F8FF.
+ */
+ static const Color kGhostWhite;
+
+ /**
+ * #FFD700.
+ */
+ static const Color kGold;
+
+ /**
+ * #DAA520.
+ */
+ static const Color kGoldenrod;
+
+ /**
+ * #808080.
+ */
+ static const Color kGray;
+
+ /**
+ * #008000.
+ */
+ static const Color kGreen;
+
+ /**
+ * #ADFF2F.
+ */
+ static const Color kGreenYellow;
+
+ /**
+ * #F0FFF0.
+ */
+ static const Color kHoneydew;
+
+ /**
+ * #FF69B4.
+ */
+ static const Color kHotPink;
+
+ /**
+ * #CD5C5C.
+ */
+ static const Color kIndianRed;
+
+ /**
+ * #4B0082.
+ */
+ static const Color kIndigo;
+
+ /**
+ * #FFFFF0.
+ */
+ static const Color kIvory;
+
+ /**
+ * #F0E68C.
+ */
+ static const Color kKhaki;
+
+ /**
+ * #E6E6FA.
+ */
+ static const Color kLavender;
+
+ /**
+ * #FFF0F5.
+ */
+ static const Color kLavenderBlush;
+
+ /**
+ * #7CFC00.
+ */
+ static const Color kLawnGreen;
+
+ /**
+ * #FFFACD.
+ */
+ static const Color kLemonChiffon;
+
+ /**
+ * #ADD8E6.
+ */
+ static const Color kLightBlue;
+
+ /**
+ * #F08080.
+ */
+ static const Color kLightCoral;
+
+ /**
+ * #E0FFFF.
+ */
+ static const Color kLightCyan;
+
+ /**
+ * #FAFAD2.
+ */
+ static const Color kLightGoldenrodYellow;
+
+ /**
+ * #D3D3D3.
+ */
+ static const Color kLightGray;
+
+ /**
+ * #90EE90.
+ */
+ static const Color kLightGreen;
+
+ /**
+ * #FFB6C1.
+ */
+ static const Color kLightPink;
+
+ /**
+ * #FFA07A.
+ */
+ static const Color kLightSalmon;
+
+ /**
+ * #20B2AA.
+ */
+ static const Color kLightSeagGeen;
+
+ /**
+ * #87CEFA.
+ */
+ static const Color kLightSkyBlue;
+
+ /**
+ * #778899.
+ */
+ static const Color kLightSlateGray;
+
+ /**
+ * #B0C4DE.
+ */
+ static const Color kLightSteellue;
+
+ /**
+ * #FFFFE0.
+ */
+ static const Color kLightYellow;
+
+ /**
+ * #00FF00.
+ */
+ static const Color kLime;
+
+ /**
+ * #32CD32.
+ */
+ static const Color kLimeGreen;
+
+ /**
+ * #FAF0E6.
+ */
+ static const Color kLinen;
+
+ /**
+ * #FF00FF.
+ */
+ static const Color kMagenta;
+
+ /**
+ * #800000.
+ */
+ static const Color kMaroon;
+
+ /**
+ * #66CDAA.
+ */
+ static const Color kMediumAquamarine;
+
+ /**
+ * #0000CD.
+ */
+ static const Color kMediumBlue;
+
+ /**
+ * #BA55D3.
+ */
+ static const Color kMediumOrchid;
+
+ /**
+ * #9370DB.
+ */
+ static const Color kMediumPurple;
+
+ /**
+ * #3CB371.
+ */
+ static const Color kMediumSeaGreen;
+
+ /**
+ * #7B68EE.
+ */
+ static const Color kMediumSlateBlue;
+
+ /**
+ * #00FA9A.
+ */
+ static const Color kMediumSpringGreen;
+
+ /**
+ * #48D1CC.
+ */
+ static const Color kMediumTurquoise;
+
+ /**
+ * #C71585.
+ */
+ static const Color kMediumVioletRed;
+
+ /**
+ * #191970.
+ */
+ static const Color kMidnightBlue;
+
+ /**
+ * #F5FFFA.
+ */
+ static const Color kMintcream;
+
+ /**
+ * #FFE4E1.
+ */
+ static const Color kMistyRose;
+
+ /**
+ * #FFE4B5.
+ */
+ static const Color kMoccasin;
+
+ /**
+ * #FFDEAD.
+ */
+ static const Color kNavajoWhite;
+
+ /**
+ * #000080.
+ */
+ static const Color kNavy;
+
+ /**
+ * #FDF5E6.
+ */
+ static const Color kOldLace;
+
+ /**
+ * #808000.
+ */
+ static const Color kOlive;
+
+ /**
+ * #6B8E23.
+ */
+ static const Color kOliveDrab;
+
+ /**
+ * #FFA500.
+ */
+ static const Color kOrange;
+
+ /**
+ * #FF4500.
+ */
+ static const Color kOrangeRed;
+
+ /**
+ * #DA70D6.
+ */
+ static const Color kOrchid;
+
+ /**
+ * #EEE8AA.
+ */
+ static const Color kPaleGoldenrod;
+
+ /**
+ * #98FB98.
+ */
+ static const Color kPaleGreen;
+
+ /**
+ * #AFEEEE.
+ */
+ static const Color kPaleTurquoise;
+
+ /**
+ * #DB7093.
+ */
+ static const Color kPaleVioletRed;
+
+ /**
+ * #FFEFD5.
+ */
+ static const Color kPapayaWhip;
+
+ /**
+ * #FFDAB9.
+ */
+ static const Color kPeachPuff;
+
+ /**
+ * #CD853F.
+ */
+ static const Color kPeru;
+
+ /**
+ * #FFC0CB.
+ */
+ static const Color kPink;
+
+ /**
+ * #DDA0DD.
+ */
+ static const Color kPlum;
+
+ /**
+ * #B0E0E6.
+ */
+ static const Color kPowderBlue;
+
+ /**
+ * #800080.
+ */
+ static const Color kPurple;
+
+ /**
+ * #FF0000.
+ */
+ static const Color kRed;
+
+ /**
+ * #BC8F8F.
+ */
+ static const Color kRosyBrown;
+
+ /**
+ * #4169E1.
+ */
+ static const Color kRoyalBlue;
+
+ /**
+ * #8B4513.
+ */
+ static const Color kSaddleBrown;
+
+ /**
+ * #FA8072.
+ */
+ static const Color kSalmon;
+
+ /**
+ * #F4A460.
+ */
+ static const Color kSandyBrown;
+
+ /**
+ * #2E8B57.
+ */
+ static const Color kSeaGreen;
+
+ /**
+ * #FFF5EE.
+ */
+ static const Color kSeashell;
+
+ /**
+ * #A0522D.
+ */
+ static const Color kSienna;
+
+ /**
+ * #C0C0C0.
+ */
+ static const Color kSilver;
+
+ /**
+ * #87CEEB.
+ */
+ static const Color kSkyBlue;
+
+ /**
+ * #6A5ACD.
+ */
+ static const Color kSlateBlue;
+
+ /**
+ * #708090.
+ */
+ static const Color kSlateGray;
+
+ /**
+ * #FFFAFA.
+ */
+ static const Color kSnow;
+
+ /**
+ * #00FF7F.
+ */
+ static const Color kSpringGreen;
+
+ /**
+ * #4682B4.
+ */
+ static const Color kSteelBlue;
+
+ /**
+ * #D2B48C.
+ */
+ static const Color kTan;
+
+ /**
+ * #008080.
+ */
+ static const Color kTeal;
+
+ /**
+ * #D8BFD8.
+ */
+ static const Color kThistle;
+
+ /**
+ * #FF6347.
+ */
+ static const Color kTomato;
+
+ /**
+ * #40E0D0.
+ */
+ static const Color kTurquoise;
+
+ /**
+ * #EE82EE.
+ */
+ static const Color kViolet;
+
+ /**
+ * #F5DEB3.
+ */
+ static const Color kWheat;
+
+ /**
+ * #FFFFFF.
+ */
+ static const Color kWhite;
+
+ /**
+ * #F5F5F5.
+ */
+ static const Color kWhiteSmoke;
+
+ /**
+ * #FFFF00.
+ */
+ static const Color kYellow;
+
+ /**
+ * #9ACD32.
+ */
+ static const Color kYellowGreen;
+
+ constexpr Color() = default;
+
+ /**
+ * Constructs a Color.
+ *
+ * @param red Red value (0-1)
+ * @param green Green value (0-1)
+ * @param blue Blue value (0-1)
+ */
+ constexpr Color(double r, double g, double b)
+ : red(roundAndClamp(r)),
+ green(roundAndClamp(g)),
+ blue(roundAndClamp(b)) {}
+
+ double red = 0.0;
+ double green = 0.0;
+ double blue = 0.0;
+
+ private:
+ static constexpr double kPrecision = 1.0 / (1 << 12);
+
+ static constexpr double roundAndClamp(double value) {
+ const auto rounded =
+ (static_cast<int>(value / kPrecision) + 0.5) * kPrecision;
+ return std::clamp(rounded, 0.0, 1.0);
+ }
+};
+
+inline bool operator==(const Color& c1, const Color& c2) {
+ return c1.red == c2.red && c1.green == c2.green && c1.blue == c2.blue;
+}
+
+/*
+ * FIRST Colors
+ */
+inline constexpr Color Color::kDenim{0.0823529412, 0.376470589, 0.7411764706};
+inline constexpr Color Color::kFirstBlue{0.0, 0.4, 0.7019607844};
+inline constexpr Color Color::kFirstRed{0.9294117648, 0.1098039216,
+ 0.1411764706};
+
+/*
+ * Standard Colors
+ */
+inline constexpr Color Color::kAliceBlue{0.9411765f, 0.972549f, 1.0f};
+inline constexpr Color Color::kAntiqueWhite{0.98039216f, 0.92156863f,
+ 0.84313726f};
+inline constexpr Color Color::kAqua{0.0f, 1.0f, 1.0f};
+inline constexpr Color Color::kAquamarine{0.49803922f, 1.0f, 0.83137256f};
+inline constexpr Color Color::kAzure{0.9411765f, 1.0f, 1.0f};
+inline constexpr Color Color::kBeige{0.9607843f, 0.9607843f, 0.8627451f};
+inline constexpr Color Color::kBisque{1.0f, 0.89411765f, 0.76862746f};
+inline constexpr Color Color::kBlack{0.0f, 0.0f, 0.0f};
+inline constexpr Color Color::kBlanchedAlmond{1.0f, 0.92156863f, 0.8039216f};
+inline constexpr Color Color::kBlue{0.0f, 0.0f, 1.0f};
+inline constexpr Color Color::kBlueViolet{0.5411765f, 0.16862746f, 0.8862745f};
+inline constexpr Color Color::kBrown{0.64705884f, 0.16470589f, 0.16470589f};
+inline constexpr Color Color::kBurlywood{0.87058824f, 0.72156864f, 0.5294118f};
+inline constexpr Color Color::kCadetBlue{0.37254903f, 0.61960787f, 0.627451f};
+inline constexpr Color Color::kChartreuse{0.49803922f, 1.0f, 0.0f};
+inline constexpr Color Color::kChocolate{0.8235294f, 0.4117647f, 0.11764706f};
+inline constexpr Color Color::kCoral{1.0f, 0.49803922f, 0.3137255f};
+inline constexpr Color Color::kCornflowerBlue{0.39215687f, 0.58431375f,
+ 0.92941177f};
+inline constexpr Color Color::kCornsilk{1.0f, 0.972549f, 0.8627451f};
+inline constexpr Color Color::kCrimson{0.8627451f, 0.078431375f, 0.23529412f};
+inline constexpr Color Color::kCyan{0.0f, 1.0f, 1.0f};
+inline constexpr Color Color::kDarkBlue{0.0f, 0.0f, 0.54509807f};
+inline constexpr Color Color::kDarkCyan{0.0f, 0.54509807f, 0.54509807f};
+inline constexpr Color Color::kDarkGoldenrod{0.72156864f, 0.5254902f,
+ 0.043137256f};
+inline constexpr Color Color::kDarkGray{0.6627451f, 0.6627451f, 0.6627451f};
+inline constexpr Color Color::kDarkGreen{0.0f, 0.39215687f, 0.0f};
+inline constexpr Color Color::kDarkKhaki{0.7411765f, 0.7176471f, 0.41960785f};
+inline constexpr Color Color::kDarkMagenta{0.54509807f, 0.0f, 0.54509807f};
+inline constexpr Color Color::kDarkOliveGreen{0.33333334f, 0.41960785f,
+ 0.18431373f};
+inline constexpr Color Color::kDarkOrange{1.0f, 0.54901963f, 0.0f};
+inline constexpr Color Color::kDarkOrchid{0.6f, 0.19607843f, 0.8f};
+inline constexpr Color Color::kDarkRed{0.54509807f, 0.0f, 0.0f};
+inline constexpr Color Color::kDarkSalmon{0.9137255f, 0.5882353f, 0.47843137f};
+inline constexpr Color Color::kDarkSeaGreen{0.56078434f, 0.7372549f,
+ 0.56078434f};
+inline constexpr Color Color::kDarkSlateBlue{0.28235295f, 0.23921569f,
+ 0.54509807f};
+inline constexpr Color Color::kDarkSlateGray{0.18431373f, 0.30980393f,
+ 0.30980393f};
+inline constexpr Color Color::kDarkTurquoise{0.0f, 0.80784315f, 0.81960785f};
+inline constexpr Color Color::kDarkViolet{0.5803922f, 0.0f, 0.827451f};
+inline constexpr Color Color::kDeepPink{1.0f, 0.078431375f, 0.5764706f};
+inline constexpr Color Color::kDeepSkyBlue{0.0f, 0.7490196f, 1.0f};
+inline constexpr Color Color::kDimGray{0.4117647f, 0.4117647f, 0.4117647f};
+inline constexpr Color Color::kDodgerBlue{0.11764706f, 0.5647059f, 1.0f};
+inline constexpr Color Color::kFirebrick{0.69803923f, 0.13333334f, 0.13333334f};
+inline constexpr Color Color::kFloralWhite{1.0f, 0.98039216f, 0.9411765f};
+inline constexpr Color Color::kForestGreen{0.13333334f, 0.54509807f,
+ 0.13333334f};
+inline constexpr Color Color::kFuchsia{1.0f, 0.0f, 1.0f};
+inline constexpr Color Color::kGainsboro{0.8627451f, 0.8627451f, 0.8627451f};
+inline constexpr Color Color::kGhostWhite{0.972549f, 0.972549f, 1.0f};
+inline constexpr Color Color::kGold{1.0f, 0.84313726f, 0.0f};
+inline constexpr Color Color::kGoldenrod{0.85490197f, 0.64705884f, 0.1254902f};
+inline constexpr Color Color::kGray{0.5019608f, 0.5019608f, 0.5019608f};
+inline constexpr Color Color::kGreen{0.0f, 0.5019608f, 0.0f};
+inline constexpr Color Color::kGreenYellow{0.6784314f, 1.0f, 0.18431373f};
+inline constexpr Color Color::kHoneydew{0.9411765f, 1.0f, 0.9411765f};
+inline constexpr Color Color::kHotPink{1.0f, 0.4117647f, 0.7058824f};
+inline constexpr Color Color::kIndianRed{0.8039216f, 0.36078432f, 0.36078432f};
+inline constexpr Color Color::kIndigo{0.29411766f, 0.0f, 0.50980395f};
+inline constexpr Color Color::kIvory{1.0f, 1.0f, 0.9411765f};
+inline constexpr Color Color::kKhaki{0.9411765f, 0.9019608f, 0.54901963f};
+inline constexpr Color Color::kLavender{0.9019608f, 0.9019608f, 0.98039216f};
+inline constexpr Color Color::kLavenderBlush{1.0f, 0.9411765f, 0.9607843f};
+inline constexpr Color Color::kLawnGreen{0.4862745f, 0.9882353f, 0.0f};
+inline constexpr Color Color::kLemonChiffon{1.0f, 0.98039216f, 0.8039216f};
+inline constexpr Color Color::kLightBlue{0.6784314f, 0.84705883f, 0.9019608f};
+inline constexpr Color Color::kLightCoral{0.9411765f, 0.5019608f, 0.5019608f};
+inline constexpr Color Color::kLightCyan{0.8784314f, 1.0f, 1.0f};
+inline constexpr Color Color::kLightGoldenrodYellow{0.98039216f, 0.98039216f,
+ 0.8235294f};
+inline constexpr Color Color::kLightGray{0.827451f, 0.827451f, 0.827451f};
+inline constexpr Color Color::kLightGreen{0.5647059f, 0.93333334f, 0.5647059f};
+inline constexpr Color Color::kLightPink{1.0f, 0.7137255f, 0.75686276f};
+inline constexpr Color Color::kLightSalmon{1.0f, 0.627451f, 0.47843137f};
+inline constexpr Color Color::kLightSeagGeen{0.1254902f, 0.69803923f,
+ 0.6666667f};
+inline constexpr Color Color::kLightSkyBlue{0.5294118f, 0.80784315f,
+ 0.98039216f};
+inline constexpr Color Color::kLightSlateGray{0.46666667f, 0.53333336f, 0.6f};
+inline constexpr Color Color::kLightSteellue{0.6901961f, 0.76862746f,
+ 0.87058824f};
+inline constexpr Color Color::kLightYellow{1.0f, 1.0f, 0.8784314f};
+inline constexpr Color Color::kLime{0.0f, 1.0f, 0.0f};
+inline constexpr Color Color::kLimeGreen{0.19607843f, 0.8039216f, 0.19607843f};
+inline constexpr Color Color::kLinen{0.98039216f, 0.9411765f, 0.9019608f};
+inline constexpr Color Color::kMagenta{1.0f, 0.0f, 1.0f};
+inline constexpr Color Color::kMaroon{0.5019608f, 0.0f, 0.0f};
+inline constexpr Color Color::kMediumAquamarine{0.4f, 0.8039216f, 0.6666667f};
+inline constexpr Color Color::kMediumBlue{0.0f, 0.0f, 0.8039216f};
+inline constexpr Color Color::kMediumOrchid{0.7294118f, 0.33333334f, 0.827451f};
+inline constexpr Color Color::kMediumPurple{0.5764706f, 0.4392157f,
+ 0.85882354f};
+inline constexpr Color Color::kMediumSeaGreen{0.23529412f, 0.7019608f,
+ 0.44313726f};
+inline constexpr Color Color::kMediumSlateBlue{0.48235294f, 0.40784314f,
+ 0.93333334f};
+inline constexpr Color Color::kMediumSpringGreen{0.0f, 0.98039216f, 0.6039216f};
+inline constexpr Color Color::kMediumTurquoise{0.28235295f, 0.81960785f, 0.8f};
+inline constexpr Color Color::kMediumVioletRed{0.78039217f, 0.08235294f,
+ 0.52156866f};
+inline constexpr Color Color::kMidnightBlue{0.09803922f, 0.09803922f,
+ 0.4392157f};
+inline constexpr Color Color::kMintcream{0.9607843f, 1.0f, 0.98039216f};
+inline constexpr Color Color::kMistyRose{1.0f, 0.89411765f, 0.88235295f};
+inline constexpr Color Color::kMoccasin{1.0f, 0.89411765f, 0.70980394f};
+inline constexpr Color Color::kNavajoWhite{1.0f, 0.87058824f, 0.6784314f};
+inline constexpr Color Color::kNavy{0.0f, 0.0f, 0.5019608f};
+inline constexpr Color Color::kOldLace{0.99215686f, 0.9607843f, 0.9019608f};
+inline constexpr Color Color::kOlive{0.5019608f, 0.5019608f, 0.0f};
+inline constexpr Color Color::kOliveDrab{0.41960785f, 0.5568628f, 0.13725491f};
+inline constexpr Color Color::kOrange{1.0f, 0.64705884f, 0.0f};
+inline constexpr Color Color::kOrangeRed{1.0f, 0.27058825f, 0.0f};
+inline constexpr Color Color::kOrchid{0.85490197f, 0.4392157f, 0.8392157f};
+inline constexpr Color Color::kPaleGoldenrod{0.93333334f, 0.9098039f,
+ 0.6666667f};
+inline constexpr Color Color::kPaleGreen{0.59607846f, 0.9843137f, 0.59607846f};
+inline constexpr Color Color::kPaleTurquoise{0.6862745f, 0.93333334f,
+ 0.93333334f};
+inline constexpr Color Color::kPaleVioletRed{0.85882354f, 0.4392157f,
+ 0.5764706f};
+inline constexpr Color Color::kPapayaWhip{1.0f, 0.9372549f, 0.8352941f};
+inline constexpr Color Color::kPeachPuff{1.0f, 0.85490197f, 0.7254902f};
+inline constexpr Color Color::kPeru{0.8039216f, 0.52156866f, 0.24705882f};
+inline constexpr Color Color::kPink{1.0f, 0.7529412f, 0.79607844f};
+inline constexpr Color Color::kPlum{0.8666667f, 0.627451f, 0.8666667f};
+inline constexpr Color Color::kPowderBlue{0.6901961f, 0.8784314f, 0.9019608f};
+inline constexpr Color Color::kPurple{0.5019608f, 0.0f, 0.5019608f};
+inline constexpr Color Color::kRed{1.0f, 0.0f, 0.0f};
+inline constexpr Color Color::kRosyBrown{0.7372549f, 0.56078434f, 0.56078434f};
+inline constexpr Color Color::kRoyalBlue{0.25490198f, 0.4117647f, 0.88235295f};
+inline constexpr Color Color::kSaddleBrown{0.54509807f, 0.27058825f,
+ 0.07450981f};
+inline constexpr Color Color::kSalmon{0.98039216f, 0.5019608f, 0.44705883f};
+inline constexpr Color Color::kSandyBrown{0.95686275f, 0.6431373f, 0.3764706f};
+inline constexpr Color Color::kSeaGreen{0.18039216f, 0.54509807f, 0.34117648f};
+inline constexpr Color Color::kSeashell{1.0f, 0.9607843f, 0.93333334f};
+inline constexpr Color Color::kSienna{0.627451f, 0.32156864f, 0.1764706f};
+inline constexpr Color Color::kSilver{0.7529412f, 0.7529412f, 0.7529412f};
+inline constexpr Color Color::kSkyBlue{0.5294118f, 0.80784315f, 0.92156863f};
+inline constexpr Color Color::kSlateBlue{0.41568628f, 0.3529412f, 0.8039216f};
+inline constexpr Color Color::kSlateGray{0.4392157f, 0.5019608f, 0.5647059f};
+inline constexpr Color Color::kSnow{1.0f, 0.98039216f, 0.98039216f};
+inline constexpr Color Color::kSpringGreen{0.0f, 1.0f, 0.49803922f};
+inline constexpr Color Color::kSteelBlue{0.27450982f, 0.50980395f, 0.7058824f};
+inline constexpr Color Color::kTan{0.8235294f, 0.7058824f, 0.54901963f};
+inline constexpr Color Color::kTeal{0.0f, 0.5019608f, 0.5019608f};
+inline constexpr Color Color::kThistle{0.84705883f, 0.7490196f, 0.84705883f};
+inline constexpr Color Color::kTomato{1.0f, 0.3882353f, 0.2784314f};
+inline constexpr Color Color::kTurquoise{0.2509804f, 0.8784314f, 0.8156863f};
+inline constexpr Color Color::kViolet{0.93333334f, 0.50980395f, 0.93333334f};
+inline constexpr Color Color::kWheat{0.9607843f, 0.87058824f, 0.7019608f};
+inline constexpr Color Color::kWhite{1.0f, 1.0f, 1.0f};
+inline constexpr Color Color::kWhiteSmoke{0.9607843f, 0.9607843f, 0.9607843f};
+inline constexpr Color Color::kYellow{1.0f, 1.0f, 0.0f};
+inline constexpr Color Color::kYellowGreen{0.6039216f, 0.8039216f, 0.19607843f};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/util/Color8Bit.h b/wpilibc/src/main/native/include/frc/util/Color8Bit.h
new file mode 100644
index 0000000..d5dba07
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/util/Color8Bit.h
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+
+#include "Color.h"
+
+namespace frc {
+
+/**
+ * Represents colors that can be used with Addressable LEDs.
+ */
+class Color8Bit {
+ public:
+ constexpr Color8Bit() = default;
+
+ /**
+ * Constructs a Color8Bit.
+ *
+ * @param red Red value (0-255)
+ * @param green Green value (0-255)
+ * @param blue Blue value (0-255)
+ */
+ constexpr Color8Bit(int r, int g, int b)
+ : red(std::clamp(r, 0, 255)),
+ green(std::clamp(g, 0, 255)),
+ blue(std::clamp(b, 0, 255)) {}
+
+ /**
+ * Constructs a Color8Bit from a Color.
+ *
+ * @param color The color
+ */
+ constexpr Color8Bit(const Color& color)
+ : red(color.red * 255),
+ green(color.green * 255),
+ blue(color.blue * 255) {}
+
+ constexpr operator Color() const {
+ return Color(red / 255.0, green / 255.0, blue / 255.0);
+ }
+
+ int red = 0;
+ int green = 0;
+ int blue = 0;
+};
+
+inline bool operator==(const Color8Bit& c1, const Color8Bit& c2) {
+ return c1.red == c2.red && c1.green == c2.green && c1.blue == c2.blue;
+}
+
+} // namespace frc
diff --git a/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp b/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
new file mode 100644
index 0000000..ae253a7
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <thread>
+
+#include "frc/SlewRateLimiter.h"
+#include "gtest/gtest.h"
+
+TEST(SlewRateLimiterTest, SlewRateLimitTest) {
+ frc::SlewRateLimiter<units::meters> limiter(1_mps);
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+ EXPECT_TRUE(limiter.Calculate(2_m) < 2_m);
+}
+
+TEST(SlewRateLimiterTest, SlewRateNoLimitTest) {
+ frc::SlewRateLimiter<units::meters> limiter(1_mps);
+
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+ EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
+}
diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index 319be79..f2f0a38 100644
--- a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -120,3 +120,14 @@
Pose2d end{4_m, 0_m, 0_rad};
Run(start, waypoints, end);
}
+
+TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) {
+ EXPECT_THROW(
+ Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector<Translation2d>{},
+ Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
+ SplineParameterizer::MalformedSplineException);
+ EXPECT_THROW(
+ Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
+ Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
+ SplineParameterizer::MalformedSplineException);
+}
diff --git a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
index cc10c6c..8a87053 100644
--- a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -85,3 +85,12 @@
Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
}
+
+TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) {
+ EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)),
+ Pose2d(1_m, 0_m, Rotation2d(180_deg))),
+ SplineParameterizer::MalformedSplineException);
+ EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)),
+ Pose2d(10_m, 11_m, Rotation2d(-90_deg))),
+ SplineParameterizer::MalformedSplineException);
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
index 7e47e11..90046c5 100644
--- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -33,3 +33,13 @@
12_fps_sq + 0.01_fps_sq);
}
}
+
+TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
+ const auto t = TrajectoryGenerator::GenerateTrajectory(
+ std::vector<Pose2d>{Pose2d(0_m, 0_m, Rotation2d(0_deg)),
+ Pose2d(1_m, 0_m, Rotation2d(180_deg))},
+ TrajectoryConfig(12_fps, 12_fps_sq));
+
+ ASSERT_EQ(t.States().size(), 1u);
+ ASSERT_EQ(t.TotalTime(), 0_s);
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
new file mode 100644
index 0000000..af69b58
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
+ std::vector<frc::Trajectory::State> statesB) {
+ for (unsigned int i = 0; i < statesA.size() - 1; i++) {
+ auto a1 = statesA[i].pose;
+ auto a2 = statesA[i + 1].pose;
+
+ auto b1 = statesB[i].pose;
+ auto b2 = statesB[i + 1].pose;
+
+ auto a = a2.RelativeTo(a1);
+ auto b = b2.RelativeTo(b1);
+
+ EXPECT_NEAR(a.Translation().X().to<double>(),
+ b.Translation().X().to<double>(), 1E-9);
+ EXPECT_NEAR(a.Translation().Y().to<double>(),
+ b.Translation().Y().to<double>(), 1E-9);
+ EXPECT_NEAR(a.Rotation().Radians().to<double>(),
+ b.Rotation().Radians().to<double>(), 1E-9);
+ }
+}
+
+TEST(TrajectoryTransforms, TransformBy) {
+ frc::TrajectoryConfig config{3_mps, 3_mps_sq};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, frc::Rotation2d(90_deg)},
+ config);
+
+ auto transformedTrajectory =
+ trajectory.TransformBy({{1_m, 2_m}, frc::Rotation2d(30_deg)});
+
+ auto firstPose = transformedTrajectory.Sample(0_s).pose;
+
+ EXPECT_NEAR(firstPose.Translation().X().to<double>(), 1.0, 1E-9);
+ EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 2.0, 1E-9);
+ EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 30.0, 1E-9);
+
+ TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
+}
+
+TEST(TrajectoryTransforms, RelativeTo) {
+ frc::TrajectoryConfig config{3_mps, 3_mps_sq};
+ auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+ frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
+ frc::Pose2d{5_m, 7_m, frc::Rotation2d(90_deg)}, config);
+
+ auto transformedTrajectory =
+ trajectory.RelativeTo({1_m, 2_m, frc::Rotation2d(30_deg)});
+
+ auto firstPose = transformedTrajectory.Sample(0_s).pose;
+
+ EXPECT_NEAR(firstPose.Translation().X().to<double>(), 0, 1E-9);
+ EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 0, 1E-9);
+ EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 0, 1E-9);
+
+ TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
+}