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