Squashed 'third_party/allwpilib/' changes from f1a82828fe..ce550705d7
ce550705d7 [ntcore] Fix client "received unknown id -1" (#6186)
3989617bde [ntcore] NetworkTable::GetStruct: Add I template param (#6183)
f1836e1321 [fieldImages] Fix 2024 field json (#6179)
d05f179a9a [build] Fix running apriltagsvision Java example (#6173)
b1b03bed85 [wpilib] Update MotorControllerGroup deprecation message (#6171)
fa63fbf446 LICENSE.md: Bump year to 2024 (#6169)
4809f3d0fc [apriltag] Add 2024 AprilTag locations (#6168)
dd90965362 [wpiutil] Fix RawFrame.setInfo() NPE (#6167)
8659372d08 [fieldImages] Add 2024 field image (#6166)
a2e4d0b15d [docs] Fix docs for SysID routine (#6164)
0a46a3a618 [wpilib] Make ADXL345 default I2C address public (#6163)
7c26bc70ab [sysid] Load DataLog files directly for analysis (#6103)
f94e3d81b9 [docs] Fix SysId routine JavaDoc warnings (#6159)
6bed82a18e [wpilibc] Clean up C++ SysId routine (#6160)
4595f84719 [wpilib] Report LiveWindow-enabled-in-test (#6158)
707cb06105 [wpilib] Add SysIdRoutine logging utility and command factory (#6033)
3e40b9e5da [wpilib] Correct SmartDashboard usage reporting (#6157)
106518c3f8 [docs] Fix wpilibj JavaDoc warnings (#6154)
19cb2a8eb4 [wpilibj] Make class variables private to match C++ (#6153)
13f4460e00 [docs] Add missing docs to enum fields (NFC) (#6150)
4210f5635d [docs] Fix warnings about undocumented default constructors (#6151)
0f060afb55 [ntcore] Disable WebSocket fragmentation (#6149)
f29a7d2e50 [docs] Add missing JavaDocs (#6146)
6e58db398d [commands] Make Java fields private (#6148)
4ac0720385 [build] Clean up CMake files (#6141)
44db3e0ac0 [sysid] Make constexpr variables outside class scope inline (#6145)
73c7d87db7 [glass] NTStringChooser: Properly set retained (#6144)
25636b712f [build] Remove unnecessary native dependencies in wpilibjExamples (#6143)
01fb98baaa [docs] Add Missing JNI docs from C++ (NFC) (#6139)
5c424248c4 [wpilibj] Remove unused AnalogTriggerException (#6142)
c486972c55 [wpimath] Make ExponentialProfile.State mutable (#6138)
783acb9b72 [wpilibj] Store long preferences as integers (#6136)
99ab836894 [wpiutil] Add missing JavaDocs (NFC) (#6132)
ad0859a8c9 [docs] Add missing JavaDocs (#6125)
5579219716 [docs] Exclude quickbuf files and proto/struct packages from doclint (#6128)
98f06911c7 [sysid] Use eigenvector component instead of eigenvalue for fit quality check (#6131)
e1d49b975c [wpimath] Add LinearFilter reset() overload to initialize input and output buffers (#6133)
8a0bf2b7a4 [hal] Add CANAPITypes to java (#6121)
91d8837c11 [wpilib] Make protected fields in accelerometers/gyros private (#6134)
e7c9f27683 [wpilib] Add functional interface equivalents to MotorController (#6053)
8aca706217 [glass] Add type information to SmartDashboard menu (#6117)
7d3e4ddba9 [docs] Add warning about using user button to docs (NFC) (#6129)
ec3cb3dcba [build] Disable clang-tidy warning about test case names (#6127)
495585b25d [examples] Update april tag family to 36h11 (#6126)
f9aabc5ab2 [wpilib] Throw early when EventLoop is modified while running (#6115)
c16946c0ec [hal] Add CANJNI docs (NFC) (#6120)
b7f4eb2811 [doc] Update maven artifacts for units and apriltags (NFC) (#6123)
f419a62b38 [doc] Update maintainers.md (NFC) (#6124)
938bf45fd9 [wpiutil] Remove type param from ProtobufSerializable and StructSerializable (#6122)
c34debe012 [docs] Link to external OpenCV docs (#6119)
07183765de [hal] Fix formatting of HAL_ENUM enums (NFC) (#6114)
af46034b7f [wpilib] Document only first party controllers are guaranteed to have correct mapping (#6112)
636ef58d94 [hal] Properly error check readCANStreamSession (#6108)
cc631d2a69 [build] Fix generated source set location in the HAL (#6113)
09f76b32c2 [wpimath] Compile with UTF-8 encoding (#6111)
47c5fd8620 [sysid] Check data quality before OLS (#6110)
24a76be694 [hal] Add method to detect if the CAN Stream has overflowed (#6105)
9333951736 [hal] Allocate CANStreamMessage in JNI if null (#6107)
6a2d3c30a6 [wpiutil] Struct: Add info template parameter pack (#6086)
e07de37e64 [commands] Mark ParallelDeadlineGroup.setDeadline() final (#6102)
141241d2d6 [wpilib] Fix usage reporting for static classes (#6090)
f2c2bab7dc [sysid] Fix adjusted R² calculation (#6101)
5659038443 [wpiutil,cscore,apriltag] Fix RawFrame (#6098)
8aeee03626 [commands] Improve error message when composing commands twice in same composition (#6091)
55508706ff [wpiutil,cscore] Move VideoMode.PixelFormat to wpiutil (#6097)
ab78b930e9 [wpilib] ADIS16470: Add access to all 3 axes (#6074)
795d4be9fd [wpilib] Fix precision issue in Color round-and-clamp (#6100)
7aa9ad44b8 [commands] Deprecate C++ TransferOwnership() (#6095)
92c81d0791 [ci] Update pregenerate workflow to actions/checkout@v4 (#6094)
1ce617be07 [ci] Update artifact actions to v4 (#6092)
2441b57156 [wpilib] Add PWMSparkFlex MotorController (#6089)
21d1972d7a [wpiutil] DataLog: Ensure file is written on shutdown (#6087)
c29e8c66cf [wpiutil] DataLog: Fix UB in AppendImpl (#6088)
ab309e34ef [glass] Fix order of loading window settings (#6056)
22a322c9f3 [wpimath] Report error on negative PID gains (#6055)
1dba26c937 [wpilib] Add method to get breaker fault at a specific channel in PowerDistribution[Sticky]Faults (#5521)
ef1cb3f41e [commands] Fix compose-while-scheduled issue and test all compositions (#5581)
aeb1a4aa33 [wpiutil] Add serializable marker interfaces (#6060)
c1178d5add [wpilib] Add StadiaController and command wrapper (#6083)
4e4a468d4d [wpimath] Make feedforward classes throw exceptions for negative Kv or Ka (#6084)
d1793f077d [build] cmake: Add NO_WERROR option to disable -Werror (#6071)
43fb6e9f87 [glass] Add Profiled PID controller support & IZone Support (#5959)
bcef6c5398 [apriltag] Fix Java generation functions (#6063)
4059e0cd9f [hal,wpilib] Add function to control "Radio" LED (#6073)
0b2cfb3abc [dlt] Change datalogtool default folder to logs folder (#6079)
df5e439b0c [wpilib] PS4Controller: enable usage reporting (#6081)
0ff7478968 [cscore] Fix RawFrame class not being loaded in JNI (#6077)
6f23d32fe1 [wpilib] AddressableLED: Update warning about single driver (NFC) (#6069)
35a1c52788 [build] Upgrade quickbuf to 1.3.3 (#6072)
e4e2bafdb1 [sysid] Document timestamp units (#6065)
3d201c71f7 [ntcore] Fix overlapping subscriber handling (#6067)
f02984159f [glass] Check for null entries when updating struct/proto (#6059)
a004c9e05f [commands] SubsystemBase: allow setting name in constructor (#6052)
0b4c6a1546 [wpimath] Add more docs to SimulatedAnnealing (NFC) (#6054)
ab15dae887 [wpilib] ArcadeDrive: Fix max output handling (#6051)
9599c1f56f [hal] Add usage reporting ids from 2024v2 image (#6041)
f87c64af8a [wpimath] MecanumDriveWheelSpeeds: Fix desaturate() (#6040)
8798700cec [wpilibcExamples] Add inline specifier to constexpr constants (#6049)
85c9ae6eff [wpilib] Fix PS5 Controller mappings (#6050)
7c8b7a97ad [wpiutil] Zero out roborio system timestamp (#6042)
d9b504bc84 [wpilib] DataLogManager: Change sim location to logs subdir (#6039)
906b810136 [build] cmake: Fix ntcore generated header install (#6038)
56e5b404d1 Update to final 2024 V2 image (#6034)
8723ee5c39 [ntcore] Add cached topic property (#5494)
192a28af47 Fix JDK 21 warnings (#6028)
d40bdd70ba [build] Upgrade to spotbugs Gradle plugin 6.0.2 (#6027)
7bfadf32e5 [wpilibj] Joystick: make remainder of get axis methods final (#6024)
a770110438 [commands] CommandCompositionError: Include stacktrace of original composition (#5984)
54a55b8b53 [wpiutil,hal] Update image; init Rio Now() HMB with a FPGA session (#6016)
7d4e515a6b [wpimath] Simplify calculation of C for DARE precondition (#6022)
5200316c14 [ntcore] Update transmit period on topic add/remove (#6021)
ddf79a25d4 [wpiunits] Overload Measure.per(Time) to return Measure<Velocity> (#6018)
a71adef316 [wpiutil] Clean up circular_buffer iterator syntax (#6020)
39a0bf4b98 [examples] Call resetOdometry() when controller command is executed (#5905)
f5fc101fda [build] cmake: Export jars and clean up jar installs (#6014)
38bf024c96 [build] Update to Gradle 8.5 (#6007)
9d11544c18 [wpimath] Rotate traveling salesman solution so input and solution have same initial pose (#6015)
28deba20f5 [wpimath] Commit generated quickbuf Java files (#5994)
c2971c0bb3 [build] cmake: Export apriltag and wpimath (#6012)
41cfc961e4 gitattributes: Add linguist-generated locations (#6004)
14c3ade155 [wpimath] Struct cleanup (#6011)
90757b9e90 [wpilib] Make Color::HexString() constexpr (#5985)
2676b77873 Fix compilation issues that occur when building with bazel (#6008)
d32c10487c [examples] Update C++ examples to use CommandPtr (#5988)
9bc5fcf886 [build] cmake: Default WITH_JAVA_SOURCE to WITH_JAVA (#6005)
d431abba3b [upstream_utils] Fix GCEM namespace usage and add hypot(x, y, z) (#6002)
2bb1409b82 Clean up Java style (#5990)
66172ab288 Remove submodule (#6003)
e8f8c0ceb0 [upstream_utils] Update to latest Eigen HEAD (#5996)
890992a849 [hal] Commit generated usage reporting files (#5993)
a583ca01e1 [wpiutil] Change Struct to allow non-constexpr implementation (#5992)
ca272de400 [build] Fix Gradle compile_commands.json and clang-tidy warnings (#5977)
76ae090570 [wpiutil] type_traits: Add is_constexpr() (#5997)
5172ab8fd0 [commands] C++ CommandPtr: Prevent null initialization (#5991)
96914143ba [build] Bump native-utils to fix compile_commands.json (#5989)
464e6121ef [ci] Report failed status to Azure on failed tests (#2654)
5dad46cd45 [wpimath] Commit generated files (#5986)
54ab65a63a [ntcore] Commit generated files (#5962)
7ed900ae3a [wpilib] Add hex string constructor to Color and Color8Bit (#5063)
74b85b76a9 [wpimath] Make gcem call std functions if not constant-evaluated (#5983)
30816111db [wpimath] Fix TimeInterpolatableBuffer crash (#5972)
5cc923de33 [wpilib] DataLogManager: Use logs subdirectory on USB drives (#5975)
1144115da0 [commands] Add GetName to Subsystem, use in Scheduler tracer epochs (#5836)
ac7d726ac3 [wpimath] Add simulated annealing (#5961)
e09be72ee0 [wpimath] Remove unused SimpleMatrixUtils class (#5979)
0f9ebe92d9 [wpimath] Add generic circular buffer class to Java (#5969)
9fa28eb07a [ci] Bump actions/checkout to v4 (#5736)
ca684ac207 [hal] Add capability to read power distribution data as a stream (#4983)
51eecef2bd [wpimath] Optimize 2nd derivative of quintic splines (#3292)
4fcf0b25a1 [build] Apply a formatter for CMake files (#5973)
9b8011aa67 [build] Pin wpiformat version (#5982)
e00a0e84c1 [build] cmake: fix protobuf dependency finding for certain distributions (#5981)
23dd591394 [upstream_utils] Remove libuv patch that adjusts whitespace (#5976)
b0719942f0 [wpiutil] Timestamp: Report errors on Rio HMB init failure (#5974)
7bc89c4322 [wpilib] Update getAlliance() docs (NFC) (#5971)
841ea682d1 [upstream_utils] Upgrade to LLVM 17.0.5 (#5970)
a74db52dae [cameraserver] Add getVideo() pixelFormat overload (#5966)
a7eb422662 [build] Update native utils for new compile commands files (#5968)
544b231d4d [sysid] Add missing cassert include (#5967)
31cd015970 [wpimath] Add SysId doc links to LinearSystemId in C++ (NFC) (#5960)
9280054eab Revert "[build] Export wpimath protobuf symbols (#5952)"
2aba97c610 Export pb files from wpimath
c80b2d2017 [build] Export wpimath protobuf symbols (#5952)
3c0652c18a [cscore] Replace CS_PixelFormat with WPI_PixelFormat (#5954)
95716eb0cb [wpiunits] Documentation improvements (#5932)
423fd75fa8 [wpilib] Default LiveWindowEnabledInTest to false (#5950)
dfdea9c992 [wpimath] Make KalmanFilter variant for asymmetric updates (#5951)
ca81ced409 [wpiutil] Move RawFrame to wpiutil; add generation of RawFrame for AprilTags (#5923)
437cc91af5 [cscore] CvSink: Allow specifying output PixelFormat (#5943)
25b7dca46b [build] Remove CMake flat install option (#5944)
bb05e20247 [wpimath] Add protobuf/struct for trivial types (#5935)
35744a036e [wpimath] Move struct/proto classes to separate files (#5918)
80d7ad58ea [build] Declare platform launcher dependency explicitly (#5909)
f8d983b154 [ntcore] Protobuf/Struct: Use atomic_bool instead of atomic_flag (#5946)
4a44210ee3 [ntcore] NetworkTableInstance: Suppress unused lambda capture warning (#5947)
bdc8620d55 [upstream_utils] Fix fmt compilation errors on Windows (#5948)
0ca1e9b5f9 [wpimath] Add basic wpiunits support (#5821)
cc30824409 [ntcore] Increase client meta-topic decoding limit (#5934)
b1fad062f7 [wpilib] Use RKDP in DifferentialDrivetrainSim (#5931)
ead9ae5a69 [build] Add generateProto dependency to test and dev (#5933)
cfbff32185 [wpiutil] timestamp: Fix startup race on Rio (#5930)
7d90d0bcc3 [wpimath] Clean up StateSpaceUtil (#5891)
7755e45aac [build] Add generated protobuf headers to C++ test include path (#5926)
3985c031da [ntcore] ProtobufSubscriber: Fix typos (#5928)
7a87fe4b60 [ntcore] ProtobufSubscriber: Make mutex and msg mutable (#5927)
09f3ed6a5f [commands] Add static Trigger factories for robot mode changes (#5902)
79dd795bc0 [wpimath] Clean up VecBuilder and MatBuilder (#5906)
e117274a67 [wpilib] Change default Rio log dir from /home/lvuser to /home/lvuser/logs (#5899)
a8b80ca256 [upstream_utils] Update to libuv 1.47.0 (#5889)
b3a9c3e96b [build] Bump macOS deployment target to 12 (#5890)
0f8129677b [build] Distribute wpimath protobuf headers (#5925)
d105f9e3e9 [wpiutil] ProtobufBuffer: Fix buffer reallocation (#5924)
c5f2f6a0fb [fieldImages] Fix typo in field images artifact name (#5922)
c1a57e422a [commands] Clean up make_vector.h (#5917)
78ebc6e9ec [wpimath] change G to gearing in LinearSystemId factories (#5834)
9ada181866 [hal] DriverStation.h: Add stddef.h include (#5897)
95fa5ec72f [wpilibc,ntcoreffi] DataLogManager: join on Stop() call (#5910)
b6f2d3cc14 [build] Remove usage of Version.parse (#5911)
cc2cbeb04c [examples] Replace gyro rotation with poseEstimator rotation (#5900)
fa6b171e1c [wpiutil] Suppress protobuf warning false positives on GCC 13 (#5907)
d504639bbe [apriltag] Improve AprilTag docs (#5895)
3a1194be40 Replace static_cast<void>() with [[maybe_unused]] attribute (#5892)
70392cbbcb [build] cmake: Add protobuf dependency to wpiutil-config (#5886)
17c1bd5a83 [ntcore] Use json_fwd (#5881)
e69a9efeba [wpilibcExamples] Match array parameter bounds (#5880)
14dcd0d26f Use char instead of uint8_t for json::parse (#5877)
ec1d261984 [hal] Fix garbage data for match info before DS connection (#5879)
63dbf5c614 [wpiutil] MemoryBuffer: Fix normal read and file type check (#5875)
b2e7be9250 [ntcore] Only datalog meta-topics if specifically requested (#5873)
201a42a3cd [wpimath] Reorder TrapezoidProfile.calculate() arguments (#5874)
04a781b4d7 [apriltag] Add GetTags to C++ version of AprilTagFieldLayout (#5872)
87a8a1ced4 [docs] Exclude eigen and protobuf from doxygen (#5871)
git-subtree-dir: third_party/allwpilib
git-subtree-split: ce550705d7cdab117c0153a202973fc026a81274
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ic8645d0551d62b411b0a816c493f0f33291896a1
diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
index 14fd37f..4ce4197 100644
--- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
+++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
@@ -679,8 +679,13 @@
return compAngle;
}
-int ADIS16448_IMU::ConfigDecRate(uint16_t DecimationSetting) {
- uint16_t writeValue = DecimationSetting;
+int ADIS16448_IMU::ConfigDecRate(uint16_t decimationRate) {
+ // Switches the active SPI port to standard SPI mode, writes a new value to
+ // the DECIMATE register in the IMU, and re-enables auto SPI.
+ //
+ // This function enters standard SPI mode, writes a new DECIMATE setting to
+ // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
+ uint16_t writeValue = decimationRate;
uint16_t readbackValue;
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
@@ -688,14 +693,14 @@
}
/* Check max */
- if (DecimationSetting > 9) {
+ if (decimationRate > 9) {
REPORT_ERROR(
"Attempted to write an invalid decimation value. Capping at 9");
- DecimationSetting = 9;
+ decimationRate = 9;
}
/* Shift decimation setting to correct position and select internal sync */
- writeValue = (DecimationSetting << 8) | 0x1;
+ writeValue = (decimationRate << 8) | 0x1;
/* Apply to IMU */
WriteRegister(SMPL_PRD, writeValue);
diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
index 05dac78..1c5c7a1 100644
--- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
+++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
@@ -63,14 +63,35 @@
* Constructor.
*/
ADIS16470_IMU::ADIS16470_IMU()
- : ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {}
+ : ADIS16470_IMU(kZ, kY, kX, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {}
-ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
+ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
+ IMUAxis roll_axis)
+ : ADIS16470_IMU(yaw_axis, pitch_axis, roll_axis, SPI::Port::kOnboardCS0,
+ CalibrationTime::_4s) {}
+
+ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
+ IMUAxis roll_axis, SPI::Port port,
CalibrationTime cal_time)
: m_yaw_axis(yaw_axis),
+ m_pitch_axis(pitch_axis),
+ m_roll_axis(roll_axis),
m_spi_port(port),
m_calibration_time(static_cast<uint16_t>(cal_time)),
m_simDevice("Gyro:ADIS16470", port) {
+ if (yaw_axis == kYaw || yaw_axis == kPitch || yaw_axis == kRoll ||
+ pitch_axis == kYaw || pitch_axis == kPitch || pitch_axis == kRoll ||
+ roll_axis == kYaw || roll_axis == kPitch || roll_axis == kRoll) {
+ REPORT_ERROR(
+ "ADIS16470 constructor only allows IMUAxis.kX, IMUAxis.kY or "
+ "IMUAxis.kZ as arguments.");
+ REPORT_ERROR(
+ "Constructing ADIS with default axes. (IMUAxis.kZ is defined as Yaw)");
+ yaw_axis = kZ;
+ pitch_axis = kY;
+ roll_axis = kX;
+ }
+
if (m_simDevice) {
m_connected =
m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true);
@@ -266,17 +287,8 @@
m_auto_configured = true;
}
// Do we need to change auto SPI settings?
- switch (m_yaw_axis) {
- case kX:
- m_spi->SetAutoTransmitData(m_autospi_x_packet, 2);
- break;
- case kY:
- m_spi->SetAutoTransmitData(m_autospi_y_packet, 2);
- break;
- default:
- m_spi->SetAutoTransmitData(m_autospi_z_packet, 2);
- break;
- }
+ m_spi->SetAutoTransmitData(m_autospi_allangle_packet, 2);
+
// Configure auto stall time
m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
// Kick off DMA SPI (Note: Device configuration impossible after SPI DMA is
@@ -336,31 +348,22 @@
return 0;
}
-/**
- * @brief Switches the active SPI port to standard SPI mode, writes a new value
- *to the DECIMATE register in the IMU, and re-enables auto SPI.
- *
- * @param reg Decimation value to be set.
- *
- * @return An int indicating the success or failure of writing the new DECIMATE
- *setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
- *Failure
- *
- * This function enters standard SPI mode, writes a new DECIMATE setting to the
- *IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
- **/
-int ADIS16470_IMU::ConfigDecRate(uint16_t reg) {
- uint16_t m_reg = reg;
+int ADIS16470_IMU::ConfigDecRate(uint16_t decimationRate) {
+ // Switches the active SPI port to standard SPI mode, writes a new value to
+ // the DECIMATE register in the IMU, and re-enables auto SPI.
+ //
+ // This function enters standard SPI mode, writes a new DECIMATE setting to
+ // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
return 2;
}
- if (m_reg > 1999) {
+ if (decimationRate > 1999) {
REPORT_ERROR("Attempted to write an invalid decimation value.");
- m_reg = 1999;
+ decimationRate = 1999;
}
- m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
- WriteRegister(DEC_RATE, m_reg);
+ m_scaled_sample_rate = (((decimationRate + 1.0) / 2000.0) * 1000000.0);
+ WriteRegister(DEC_RATE, decimationRate);
if (!SwitchToAutoSPI()) {
REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
return 2;
@@ -445,7 +448,9 @@
**/
void ADIS16470_IMU::Reset() {
std::scoped_lock sync(m_mutex);
- m_integ_angle = 0.0;
+ m_integ_angle_x = 0.0;
+ m_integ_angle_y = 0.0;
+ m_integ_angle_z = 0.0;
}
void ADIS16470_IMU::Close() {
@@ -502,7 +507,7 @@
**/
void ADIS16470_IMU::Acquire() {
// Set data packet length
- const int dataset_len = 19; // 18 data points + timestamp
+ const int dataset_len = 27; // 26 data points + timestamp
/* Fixed buffer size */
const int BUFFER_SIZE = 4000;
@@ -513,7 +518,9 @@
int data_remainder = 0;
int data_to_read = 0;
uint32_t previous_timestamp = 0;
- double delta_angle = 0.0;
+ double delta_angle_x = 0.0;
+ double delta_angle_y = 0.0;
+ double delta_angle_z = 0.0;
double gyro_rate_x = 0.0;
double gyro_rate_y = 0.0;
double gyro_rate_z = 0.0;
@@ -562,14 +569,22 @@
m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
/* Get delta angle value for selected yaw axis and scale by the elapsed
* time (based on timestamp) */
- delta_angle = (ToInt(&buffer[i + 3]) * delta_angle_sf) /
- (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
- gyro_rate_x = (BuffToShort(&buffer[i + 7]) / 10.0);
- gyro_rate_y = (BuffToShort(&buffer[i + 9]) / 10.0);
- gyro_rate_z = (BuffToShort(&buffer[i + 11]) / 10.0);
- accel_x = (BuffToShort(&buffer[i + 13]) / 800.0);
- accel_y = (BuffToShort(&buffer[i + 15]) / 800.0);
- accel_z = (BuffToShort(&buffer[i + 17]) / 800.0);
+ delta_angle_x =
+ (ToInt(&buffer[i + 3]) * delta_angle_sf) /
+ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+ delta_angle_y =
+ (ToInt(&buffer[i + 7]) * delta_angle_sf) /
+ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+ delta_angle_z =
+ (ToInt(&buffer[i + 11]) * delta_angle_sf) /
+ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+
+ gyro_rate_x = (BuffToShort(&buffer[i + 15]) / 10.0);
+ gyro_rate_y = (BuffToShort(&buffer[i + 17]) / 10.0);
+ gyro_rate_z = (BuffToShort(&buffer[i + 19]) / 10.0);
+ accel_x = (BuffToShort(&buffer[i + 21]) / 800.0);
+ accel_y = (BuffToShort(&buffer[i + 23]) / 800.0);
+ accel_z = (BuffToShort(&buffer[i + 25]) / 800.0);
// Convert scaled sensor data to SI units
gyro_rate_x_si = gyro_rate_x * deg_to_rad;
@@ -611,9 +626,13 @@
if (m_first_run) {
/* Don't accumulate first run. previous_timestamp will be "very" old
* and the integration will end up way off */
- m_integ_angle = 0.0;
+ m_integ_angle_x = 0.0;
+ m_integ_angle_y = 0.0;
+ m_integ_angle_z = 0.0;
} else {
- m_integ_angle += delta_angle;
+ m_integ_angle_x += delta_angle_x;
+ m_integ_angle_y += delta_angle_y;
+ m_integ_angle_z += delta_angle_z;
}
m_gyro_rate_x = gyro_rate_x;
m_gyro_rate_y = gyro_rate_y;
@@ -634,7 +653,9 @@
data_remainder = 0;
data_to_read = 0;
previous_timestamp = 0.0;
- delta_angle = 0.0;
+ delta_angle_x = 0.0;
+ delta_angle_y = 0.0;
+ delta_angle_z = 0.0;
gyro_rate_x = 0.0;
gyro_rate_y = 0.0;
gyro_rate_z = 0.0;
@@ -696,50 +717,143 @@
return compAngle;
}
-units::degree_t ADIS16470_IMU::GetAngle() const {
- switch (m_yaw_axis) {
+void ADIS16470_IMU::SetGyroAngle(IMUAxis axis, units::degree_t angle) {
+ switch (axis) {
+ case kYaw:
+ axis = m_yaw_axis;
+ break;
+ case kPitch:
+ axis = m_pitch_axis;
+ break;
+ case kRoll:
+ axis = m_roll_axis;
+ break;
+ default:
+ break;
+ }
+
+ switch (axis) {
+ case kX:
+ SetGyroAngleX(angle);
+ break;
+ case kY:
+ SetGyroAngleY(angle);
+ break;
+ case kZ:
+ SetGyroAngleZ(angle);
+ break;
+ default:
+ break;
+ }
+}
+
+void ADIS16470_IMU::SetGyroAngleX(units::degree_t angle) {
+ std::scoped_lock sync(m_mutex);
+ m_integ_angle_x = angle.value();
+}
+
+void ADIS16470_IMU::SetGyroAngleY(units::degree_t angle) {
+ std::scoped_lock sync(m_mutex);
+ m_integ_angle_y = angle.value();
+}
+
+void ADIS16470_IMU::SetGyroAngleZ(units::degree_t angle) {
+ std::scoped_lock sync(m_mutex);
+ m_integ_angle_z = angle.value();
+}
+
+units::degree_t ADIS16470_IMU::GetAngle(IMUAxis axis) const {
+ switch (axis) {
+ case kYaw:
+ axis = m_yaw_axis;
+ break;
+ case kPitch:
+ axis = m_pitch_axis;
+ break;
+ case kRoll:
+ axis = m_roll_axis;
+ break;
+ default:
+ break;
+ }
+
+ switch (axis) {
case kX:
if (m_simGyroAngleX) {
return units::degree_t{m_simGyroAngleX.Get()};
}
- break;
+ {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_integ_angle_x};
+ }
case kY:
if (m_simGyroAngleY) {
return units::degree_t{m_simGyroAngleY.Get()};
}
- break;
+ {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_integ_angle_y};
+ }
case kZ:
if (m_simGyroAngleZ) {
return units::degree_t{m_simGyroAngleZ.Get()};
}
+ {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_integ_angle_z};
+ }
+ default:
break;
}
- std::scoped_lock sync(m_mutex);
- return units::degree_t{m_integ_angle};
+
+ return units::degree_t{0.0};
}
-units::degrees_per_second_t ADIS16470_IMU::GetRate() const {
- if (m_yaw_axis == kX) {
- if (m_simGyroRateX) {
- return units::degrees_per_second_t{m_simGyroRateX.Get()};
- }
- std::scoped_lock sync(m_mutex);
- return units::degrees_per_second_t{m_gyro_rate_x};
- } else if (m_yaw_axis == kY) {
- if (m_simGyroRateY) {
- return units::degrees_per_second_t{m_simGyroRateY.Get()};
- }
- std::scoped_lock sync(m_mutex);
- return units::degrees_per_second_t{m_gyro_rate_y};
- } else if (m_yaw_axis == kZ) {
- if (m_simGyroRateZ) {
- return units::degrees_per_second_t{m_simGyroRateZ.Get()};
- }
- std::scoped_lock sync(m_mutex);
- return units::degrees_per_second_t{m_gyro_rate_z};
- } else {
- return 0_deg_per_s;
+units::degrees_per_second_t ADIS16470_IMU::GetRate(IMUAxis axis) const {
+ switch (axis) {
+ case kYaw:
+ axis = m_yaw_axis;
+ break;
+ case kPitch:
+ axis = m_pitch_axis;
+ break;
+ case kRoll:
+ axis = m_roll_axis;
+ break;
+ default:
+ break;
}
+
+ switch (axis) {
+ case kX:
+ if (m_simGyroRateX) {
+ return units::degrees_per_second_t{m_simGyroRateX.Get()};
+ }
+ {
+ std::scoped_lock sync(m_mutex);
+ return units::degrees_per_second_t{m_gyro_rate_x};
+ }
+ case kY:
+ if (m_simGyroRateY) {
+ return units::degrees_per_second_t{m_simGyroRateY.Get()};
+ }
+ {
+ std::scoped_lock sync(m_mutex);
+ return units::degrees_per_second_t{m_gyro_rate_y};
+ }
+ case kZ:
+ if (m_simGyroRateZ) {
+ return units::degrees_per_second_t{m_simGyroRateZ.Get()};
+ }
+ {
+ std::scoped_lock sync(m_mutex);
+ return units::degrees_per_second_t{m_gyro_rate_z};
+ }
+ default:
+ break;
+ }
+
+ return 0_deg_per_s;
}
units::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const {
@@ -790,20 +904,12 @@
return m_yaw_axis;
}
-int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) {
- if (m_yaw_axis == yaw_axis) {
- return 1;
- }
- if (!SwitchToStandardSPI()) {
- REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
- return 2;
- }
- m_yaw_axis = yaw_axis;
- if (!SwitchToAutoSPI()) {
- REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
- return 2;
- }
- return 0;
+ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetPitchAxis() const {
+ return m_pitch_axis;
+}
+
+ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetRollAxis() const {
+ return m_roll_axis;
}
int ADIS16470_IMU::GetPort() const {
@@ -819,5 +925,5 @@
void ADIS16470_IMU::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("ADIS16470 IMU");
builder.AddDoubleProperty(
- "Yaw Angle", [=, this] { return GetAngle().value(); }, nullptr);
+ "Yaw Angle", [=, this] { return GetAngle(kYaw).value(); }, nullptr);
}
diff --git a/wpilibc/src/main/native/cpp/DMA.cpp b/wpilibc/src/main/native/cpp/DMA.cpp
index ce6c532..da45aa3 100644
--- a/wpilibc/src/main/native/cpp/DMA.cpp
+++ b/wpilibc/src/main/native/cpp/DMA.cpp
@@ -40,9 +40,9 @@
FRC_CheckErrorStatus(status, "SetPause");
}
-void DMA::SetTimedTrigger(units::second_t seconds) {
+void DMA::SetTimedTrigger(units::second_t period) {
int32_t status = 0;
- HAL_SetDMATimedTrigger(dmaHandle, seconds.value(), &status);
+ HAL_SetDMATimedTrigger(dmaHandle, period.value(), &status);
FRC_CheckErrorStatus(status, "SetTimedTrigger");
}
diff --git a/wpilibc/src/main/native/cpp/DataLogManager.cpp b/wpilibc/src/main/native/cpp/DataLogManager.cpp
index afe9330..09ac077 100644
--- a/wpilibc/src/main/native/cpp/DataLogManager.cpp
+++ b/wpilibc/src/main/native/cpp/DataLogManager.cpp
@@ -64,20 +64,26 @@
}
#ifdef __FRC_ROBORIO__
// prefer a mounted USB drive if one is accessible
- constexpr std::string_view usbDir{"/u"};
std::error_code ec;
- auto s = fs::status(usbDir, ec);
+ auto s = fs::status("/u", ec);
if (!ec && fs::is_directory(s) &&
(s.permissions() & fs::perms::others_write) != fs::perms::none) {
- return std::string{usbDir};
+ fs::create_directory("/u/logs", ec);
+ return "/u/logs";
}
if (RobotBase::GetRuntimeType() == kRoboRIO) {
FRC_ReportError(warn::Warning,
"DataLogManager: Logging to RoboRIO 1 internal storage is "
"not recommended! Plug in a FAT32 formatted flash drive!");
}
+ fs::create_directory("/home/lvuser/logs", ec);
+ return "/home/lvuser/logs";
+#else
+ std::string logDir = filesystem::GetOperatingDirectory() + "/logs";
+ std::error_code ec;
+ fs::create_directory(logDir, ec);
+ return logDir;
#endif
- return filesystem::GetOperatingDirectory();
}
static std::string MakeLogFilename(std::string_view filenameOverride) {
@@ -328,7 +334,7 @@
void DataLogManager::Stop() {
auto& inst = GetInstance();
inst.owner.GetThread()->m_log.Stop();
- inst.owner.Stop();
+ inst.owner.Join();
}
void DataLogManager::Log(std::string_view message) {
diff --git a/wpilibc/src/main/native/cpp/I2C.cpp b/wpilibc/src/main/native/cpp/I2C.cpp
index da26c0c..232a47f 100644
--- a/wpilibc/src/main/native/cpp/I2C.cpp
+++ b/wpilibc/src/main/native/cpp/I2C.cpp
@@ -4,6 +4,7 @@
#include "frc/I2C.h"
+#include <algorithm>
#include <utility>
#include <hal/FRCUsageReporting.h>
@@ -96,7 +97,7 @@
uint8_t deviceData[4];
for (int i = 0, curRegisterAddress = registerAddress; i < count;
i += 4, curRegisterAddress += 4) {
- int toRead = count - i < 4 ? count - i : 4;
+ int toRead = std::min(count - i, 4);
// Read the chunk of data. Return false if the sensor does not respond.
if (Read(curRegisterAddress, toRead, deviceData)) {
return false;
diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
index ee941b6..3669bce 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
@@ -8,6 +8,7 @@
#include <fmt/format.h>
#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
#include <networktables/NetworkTableInstance.h>
#include "frc/DSControlWord.h"
@@ -97,10 +98,16 @@
}
void IterativeRobotBase::EnableLiveWindowInTest(bool testLW) {
+ static bool hasReported;
if (IsTestEnabled()) {
throw FRC_MakeError(err::IncompatibleMode,
"Can't configure test mode while in test mode!");
}
+ if (!hasReported && testLW) {
+ HAL_Report(HALUsageReporting::kResourceType_SmartDashboard,
+ HALUsageReporting::kSmartDashboard_LiveWindow);
+ hasReported = true;
+ }
m_lwEnabledInTest = testLW;
}
diff --git a/wpilibc/src/main/native/cpp/PS4Controller.cpp b/wpilibc/src/main/native/cpp/PS4Controller.cpp
index 5ac3420..e59e18c 100644
--- a/wpilibc/src/main/native/cpp/PS4Controller.cpp
+++ b/wpilibc/src/main/native/cpp/PS4Controller.cpp
@@ -11,8 +11,7 @@
using namespace frc;
PS4Controller::PS4Controller(int port) : GenericHID(port) {
- // re-enable when PS4Controller is added to Usage Reporting
- // HAL_Report(HALUsageReporting::kResourceType_PS4Controller, port + 1);
+ HAL_Report(HALUsageReporting::kResourceType_PS4Controller, port + 1);
}
double PS4Controller::GetLeftX() const {
diff --git a/wpilibc/src/main/native/cpp/PneumaticHub.cpp b/wpilibc/src/main/native/cpp/PneumaticHub.cpp
index bcbbd55..9101473 100644
--- a/wpilibc/src/main/native/cpp/PneumaticHub.cpp
+++ b/wpilibc/src/main/native/cpp/PneumaticHub.cpp
@@ -23,15 +23,15 @@
/** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
units::pounds_per_square_inch_t VoltsToPSI(units::volt_t sensorVoltage,
units::volt_t supplyVoltage) {
- auto pressure = 250 * (sensorVoltage.value() / supplyVoltage.value()) - 25;
- return units::pounds_per_square_inch_t{pressure};
+ return units::pounds_per_square_inch_t{
+ 250 * (sensorVoltage.value() / supplyVoltage.value()) - 25};
}
/** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
units::volt_t PSIToVolts(units::pounds_per_square_inch_t pressure,
units::volt_t supplyVoltage) {
- auto voltage = supplyVoltage.value() * (0.004 * pressure.value() + 0.1);
- return units::volt_t{voltage};
+ return units::volt_t{supplyVoltage.value() *
+ (0.004 * pressure.value() + 0.1)};
}
wpi::mutex PneumaticHub::m_handleLock;
@@ -339,6 +339,46 @@
return stickyFaults;
}
+bool PneumaticHub::Faults::GetChannelFault(int channel) const {
+ switch (channel) {
+ case 0:
+ return Channel0Fault != 0;
+ case 1:
+ return Channel1Fault != 0;
+ case 2:
+ return Channel2Fault != 0;
+ case 3:
+ return Channel3Fault != 0;
+ case 4:
+ return Channel4Fault != 0;
+ case 5:
+ return Channel5Fault != 0;
+ case 6:
+ return Channel6Fault != 0;
+ case 7:
+ return Channel7Fault != 0;
+ case 8:
+ return Channel8Fault != 0;
+ case 9:
+ return Channel9Fault != 0;
+ case 10:
+ return Channel10Fault != 0;
+ case 11:
+ return Channel11Fault != 0;
+ case 12:
+ return Channel12Fault != 0;
+ case 13:
+ return Channel13Fault != 0;
+ case 14:
+ return Channel14Fault != 0;
+ case 15:
+ return Channel15Fault != 0;
+ default:
+ throw FRC_MakeError(err::ChannelIndexOutOfRange,
+ "Pneumatics fault channel out of bounds!");
+ }
+}
+
void PneumaticHub::ClearStickyFaults() {
int32_t status = 0;
HAL_ClearREVPHStickyFaults(m_handle, &status);
diff --git a/wpilibc/src/main/native/cpp/PowerDistribution.cpp b/wpilibc/src/main/native/cpp/PowerDistribution.cpp
index ba4cb38..d645178 100644
--- a/wpilibc/src/main/native/cpp/PowerDistribution.cpp
+++ b/wpilibc/src/main/native/cpp/PowerDistribution.cpp
@@ -172,6 +172,118 @@
return faults;
}
+bool PowerDistribution::Faults::GetBreakerFault(int channel) const {
+ switch (channel) {
+ case 0:
+ return Channel0BreakerFault != 0;
+ case 1:
+ return Channel1BreakerFault != 0;
+ case 2:
+ return Channel2BreakerFault != 0;
+ case 3:
+ return Channel3BreakerFault != 0;
+ case 4:
+ return Channel4BreakerFault != 0;
+ case 5:
+ return Channel5BreakerFault != 0;
+ case 6:
+ return Channel6BreakerFault != 0;
+ case 7:
+ return Channel7BreakerFault != 0;
+ case 8:
+ return Channel8BreakerFault != 0;
+ case 9:
+ return Channel9BreakerFault != 0;
+ case 10:
+ return Channel10BreakerFault != 0;
+ case 11:
+ return Channel11BreakerFault != 0;
+ case 12:
+ return Channel12BreakerFault != 0;
+ case 13:
+ return Channel13BreakerFault != 0;
+ case 14:
+ return Channel14BreakerFault != 0;
+ case 15:
+ return Channel15BreakerFault != 0;
+ case 16:
+ return Channel16BreakerFault != 0;
+ case 17:
+ return Channel17BreakerFault != 0;
+ case 18:
+ return Channel18BreakerFault != 0;
+ case 19:
+ return Channel19BreakerFault != 0;
+ case 20:
+ return Channel20BreakerFault != 0;
+ case 21:
+ return Channel21BreakerFault != 0;
+ case 22:
+ return Channel22BreakerFault != 0;
+ case 23:
+ return Channel23BreakerFault != 0;
+ default:
+ throw FRC_MakeError(err::ChannelIndexOutOfRange,
+ "Power distribution fault channel out of bounds!");
+ }
+}
+
+bool PowerDistribution::StickyFaults::GetBreakerFault(int channel) const {
+ switch (channel) {
+ case 0:
+ return Channel0BreakerFault != 0;
+ case 1:
+ return Channel1BreakerFault != 0;
+ case 2:
+ return Channel2BreakerFault != 0;
+ case 3:
+ return Channel3BreakerFault != 0;
+ case 4:
+ return Channel4BreakerFault != 0;
+ case 5:
+ return Channel5BreakerFault != 0;
+ case 6:
+ return Channel6BreakerFault != 0;
+ case 7:
+ return Channel7BreakerFault != 0;
+ case 8:
+ return Channel8BreakerFault != 0;
+ case 9:
+ return Channel9BreakerFault != 0;
+ case 10:
+ return Channel10BreakerFault != 0;
+ case 11:
+ return Channel11BreakerFault != 0;
+ case 12:
+ return Channel12BreakerFault != 0;
+ case 13:
+ return Channel13BreakerFault != 0;
+ case 14:
+ return Channel14BreakerFault != 0;
+ case 15:
+ return Channel15BreakerFault != 0;
+ case 16:
+ return Channel16BreakerFault != 0;
+ case 17:
+ return Channel17BreakerFault != 0;
+ case 18:
+ return Channel18BreakerFault != 0;
+ case 19:
+ return Channel19BreakerFault != 0;
+ case 20:
+ return Channel20BreakerFault != 0;
+ case 21:
+ return Channel21BreakerFault != 0;
+ case 22:
+ return Channel22BreakerFault != 0;
+ case 23:
+ return Channel23BreakerFault != 0;
+ default:
+ throw FRC_MakeError(err::ChannelIndexOutOfRange,
+ "Power distribution fault channel out of bounds!");
+ }
+}
+
PowerDistribution::StickyFaults PowerDistribution::GetStickyFaults() const {
int32_t status = 0;
HAL_PowerDistributionStickyFaults halStickyFaults;
diff --git a/wpilibc/src/main/native/cpp/RobotController.cpp b/wpilibc/src/main/native/cpp/RobotController.cpp
index 8b2b496..2414ed7 100644
--- a/wpilibc/src/main/native/cpp/RobotController.cpp
+++ b/wpilibc/src/main/native/cpp/RobotController.cpp
@@ -8,6 +8,7 @@
#include <hal/CAN.h>
#include <hal/HALBase.h>
+#include <hal/LEDs.h>
#include <hal/Power.h>
#include "frc/Errors.h"
@@ -230,6 +231,30 @@
return units::celsius_t{retVal};
}
+static_assert(RadioLEDState::kOff ==
+ static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kOff));
+static_assert(
+ RadioLEDState::kGreen ==
+ static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kGreen));
+static_assert(RadioLEDState::kRed ==
+ static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kRed));
+static_assert(
+ RadioLEDState::kOrange ==
+ static_cast<RadioLEDState>(HAL_RadioLEDState::HAL_RadioLED_kOrange));
+
+void RobotController::SetRadioLEDState(RadioLEDState state) {
+ int32_t status = 0;
+ HAL_SetRadioLEDState(static_cast<HAL_RadioLEDState>(state), &status);
+ FRC_CheckErrorStatus(status, "SetRadioLEDState");
+}
+
+RadioLEDState RobotController::GetRadioLEDState() {
+ int32_t status = 0;
+ auto retVal = static_cast<RadioLEDState>(HAL_GetRadioLEDState(&status));
+ FRC_CheckErrorStatus(status, "GetRadioLEDState");
+ return retVal;
+}
+
CANStatus RobotController::GetCANStatus() {
int32_t status = 0;
float percentBusUtilization = 0;
diff --git a/wpilibc/src/main/native/cpp/StadiaController.cpp b/wpilibc/src/main/native/cpp/StadiaController.cpp
new file mode 100644
index 0000000..4f8d666
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/StadiaController.cpp
@@ -0,0 +1,272 @@
+// 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/StadiaController.h"
+
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/event/BooleanEvent.h"
+
+using namespace frc;
+
+StadiaController::StadiaController(int port) : GenericHID(port) {
+ // re-enable when StadiaController is added to Usage Reporting
+ // HAL_Report(HALUsageReporting::kResourceType_StadiaController, port + 1);
+}
+
+double StadiaController::GetLeftX() const {
+ return GetRawAxis(Axis::kLeftX);
+}
+
+double StadiaController::GetRightX() const {
+ return GetRawAxis(Axis::kRightX);
+}
+
+double StadiaController::GetLeftY() const {
+ return GetRawAxis(Axis::kLeftY);
+}
+
+double StadiaController::GetRightY() const {
+ return GetRawAxis(Axis::kRightY);
+}
+
+bool StadiaController::GetLeftBumper() const {
+ return GetRawButton(Button::kLeftBumper);
+}
+
+bool StadiaController::GetRightBumper() const {
+ return GetRawButton(Button::kRightBumper);
+}
+
+bool StadiaController::GetLeftBumperPressed() {
+ return GetRawButtonPressed(Button::kLeftBumper);
+}
+
+bool StadiaController::GetRightBumperPressed() {
+ return GetRawButtonPressed(Button::kRightBumper);
+}
+
+bool StadiaController::GetLeftBumperReleased() {
+ return GetRawButtonReleased(Button::kLeftBumper);
+}
+
+bool StadiaController::GetRightBumperReleased() {
+ return GetRawButtonReleased(Button::kRightBumper);
+}
+
+BooleanEvent StadiaController::LeftBumper(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetLeftBumper(); });
+}
+
+BooleanEvent StadiaController::RightBumper(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetRightBumper(); });
+}
+
+bool StadiaController::GetLeftStickButton() const {
+ return GetRawButton(Button::kLeftStick);
+}
+
+bool StadiaController::GetRightStickButton() const {
+ return GetRawButton(Button::kRightStick);
+}
+
+bool StadiaController::GetLeftStickButtonPressed() {
+ return GetRawButtonPressed(Button::kLeftStick);
+}
+
+bool StadiaController::GetRightStickButtonPressed() {
+ return GetRawButtonPressed(Button::kRightStick);
+}
+
+bool StadiaController::GetLeftStickButtonReleased() {
+ return GetRawButtonReleased(Button::kLeftStick);
+}
+
+bool StadiaController::GetRightStickButtonReleased() {
+ return GetRawButtonReleased(Button::kRightStick);
+}
+
+BooleanEvent StadiaController::LeftStick(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetLeftStickButton(); });
+}
+
+BooleanEvent StadiaController::RightStick(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetRightStickButton(); });
+}
+
+bool StadiaController::GetAButton() const {
+ return GetRawButton(Button::kA);
+}
+
+bool StadiaController::GetAButtonPressed() {
+ return GetRawButtonPressed(Button::kA);
+}
+
+bool StadiaController::GetAButtonReleased() {
+ return GetRawButtonReleased(Button::kA);
+}
+
+BooleanEvent StadiaController::A(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetAButton(); });
+}
+
+bool StadiaController::GetBButton() const {
+ return GetRawButton(Button::kB);
+}
+
+bool StadiaController::GetBButtonPressed() {
+ return GetRawButtonPressed(Button::kB);
+}
+
+bool StadiaController::GetBButtonReleased() {
+ return GetRawButtonReleased(Button::kB);
+}
+
+BooleanEvent StadiaController::B(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetBButton(); });
+}
+
+bool StadiaController::GetXButton() const {
+ return GetRawButton(Button::kX);
+}
+
+bool StadiaController::GetXButtonPressed() {
+ return GetRawButtonPressed(Button::kX);
+}
+
+bool StadiaController::GetXButtonReleased() {
+ return GetRawButtonReleased(Button::kX);
+}
+
+BooleanEvent StadiaController::X(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetXButton(); });
+}
+
+bool StadiaController::GetYButton() const {
+ return GetRawButton(Button::kY);
+}
+
+bool StadiaController::GetYButtonPressed() {
+ return GetRawButtonPressed(Button::kY);
+}
+
+bool StadiaController::GetYButtonReleased() {
+ return GetRawButtonReleased(Button::kY);
+}
+
+BooleanEvent StadiaController::Y(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetYButton(); });
+}
+
+bool StadiaController::GetEllipsesButton() const {
+ return GetRawButton(Button::kEllipses);
+}
+
+bool StadiaController::GetEllipsesButtonPressed() {
+ return GetRawButtonPressed(Button::kEllipses);
+}
+
+bool StadiaController::GetEllipsesButtonReleased() {
+ return GetRawButtonReleased(Button::kEllipses);
+}
+
+BooleanEvent StadiaController::Ellipses(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetEllipsesButton(); });
+}
+
+bool StadiaController::GetHamburgerButton() const {
+ return GetRawButton(Button::kHamburger);
+}
+
+bool StadiaController::GetHamburgerButtonPressed() {
+ return GetRawButtonPressed(Button::kHamburger);
+}
+
+bool StadiaController::GetHamburgerButtonReleased() {
+ return GetRawButtonReleased(Button::kHamburger);
+}
+
+BooleanEvent StadiaController::Hamburger(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetHamburgerButton(); });
+}
+
+bool StadiaController::GetStadiaButton() const {
+ return GetRawButton(Button::kStadia);
+}
+
+bool StadiaController::GetStadiaButtonPressed() {
+ return GetRawButtonPressed(Button::kStadia);
+}
+
+bool StadiaController::GetStadiaButtonReleased() {
+ return GetRawButtonReleased(Button::kStadia);
+}
+
+BooleanEvent StadiaController::Stadia(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetStadiaButton(); });
+}
+
+bool StadiaController::GetGoogleButton() const {
+ return GetRawButton(Button::kGoogle);
+}
+
+bool StadiaController::GetGoogleButtonPressed() {
+ return GetRawButtonPressed(Button::kGoogle);
+}
+
+bool StadiaController::GetGoogleButtonReleased() {
+ return GetRawButtonReleased(Button::kGoogle);
+}
+
+BooleanEvent StadiaController::Google(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetGoogleButton(); });
+}
+
+bool StadiaController::GetFrameButton() const {
+ return GetRawButton(Button::kFrame);
+}
+
+bool StadiaController::GetFrameButtonPressed() {
+ return GetRawButtonPressed(Button::kFrame);
+}
+
+bool StadiaController::GetFrameButtonReleased() {
+ return GetRawButtonReleased(Button::kFrame);
+}
+
+BooleanEvent StadiaController::Frame(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetFrameButton(); });
+}
+
+bool StadiaController::GetLeftTriggerButton() const {
+ return GetRawButton(Button::kLeftTrigger);
+}
+
+bool StadiaController::GetLeftTriggerButtonPressed() {
+ return GetRawButtonPressed(Button::kLeftTrigger);
+}
+
+bool StadiaController::GetLeftTriggerButtonReleased() {
+ return GetRawButtonReleased(Button::kLeftTrigger);
+}
+
+BooleanEvent StadiaController::LeftTrigger(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetLeftTriggerButton(); });
+}
+
+bool StadiaController::GetRightTriggerButton() const {
+ return GetRawButton(Button::kRightTrigger);
+}
+
+bool StadiaController::GetRightTriggerButtonPressed() {
+ return GetRawButtonPressed(Button::kRightTrigger);
+}
+
+bool StadiaController::GetRightTriggerButtonReleased() {
+ return GetRawButtonReleased(Button::kRightTrigger);
+}
+
+BooleanEvent StadiaController::RightTrigger(EventLoop* loop) const {
+ return BooleanEvent(loop, [this]() { return this->GetRightTriggerButton(); });
+}
diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index 8cce62e..e3a0a8c 100644
--- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -16,11 +16,21 @@
using namespace frc;
+WPI_IGNORE_DEPRECATED
+
DifferentialDrive::DifferentialDrive(MotorController& leftMotor,
MotorController& rightMotor)
- : m_leftMotor(&leftMotor), m_rightMotor(&rightMotor) {
- wpi::SendableRegistry::AddChild(this, m_leftMotor);
- wpi::SendableRegistry::AddChild(this, m_rightMotor);
+ : DifferentialDrive{[&](double output) { leftMotor.Set(output); },
+ [&](double output) { rightMotor.Set(output); }} {
+ wpi::SendableRegistry::AddChild(this, &leftMotor);
+ wpi::SendableRegistry::AddChild(this, &rightMotor);
+}
+
+WPI_UNIGNORE_DEPRECATED
+
+DifferentialDrive::DifferentialDrive(std::function<void(double)> leftMotor,
+ std::function<void(double)> rightMotor)
+ : m_leftMotor{std::move(leftMotor)}, m_rightMotor{std::move(rightMotor)} {
static int instances = 0;
++instances;
wpi::SendableRegistry::AddLW(this, "DifferentialDrive", instances);
@@ -40,8 +50,11 @@
auto [left, right] = ArcadeDriveIK(xSpeed, zRotation, squareInputs);
- m_leftMotor->Set(left);
- m_rightMotor->Set(right);
+ m_leftOutput = left * m_maxOutput;
+ m_rightOutput = right * m_maxOutput;
+
+ m_leftMotor(m_leftOutput);
+ m_rightMotor(m_rightOutput);
Feed();
}
@@ -60,8 +73,11 @@
auto [left, right] = CurvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
- m_leftMotor->Set(left * m_maxOutput);
- m_rightMotor->Set(right * m_maxOutput);
+ m_leftOutput = left * m_maxOutput;
+ m_rightOutput = right * m_maxOutput;
+
+ m_leftMotor(m_leftOutput);
+ m_rightMotor(m_rightOutput);
Feed();
}
@@ -80,8 +96,11 @@
auto [left, right] = TankDriveIK(leftSpeed, rightSpeed, squareInputs);
- m_leftMotor->Set(left * m_maxOutput);
- m_rightMotor->Set(right * m_maxOutput);
+ m_leftOutput = left * m_maxOutput;
+ m_rightOutput = right * m_maxOutput;
+
+ m_leftMotor(m_leftOutput);
+ m_rightMotor(m_rightOutput);
Feed();
}
@@ -157,8 +176,12 @@
}
void DifferentialDrive::StopMotor() {
- m_leftMotor->StopMotor();
- m_rightMotor->StopMotor();
+ m_leftOutput = 0.0;
+ m_rightOutput = 0.0;
+
+ m_leftMotor(0.0);
+ m_rightMotor(0.0);
+
Feed();
}
@@ -171,9 +194,7 @@
builder.SetActuator(true);
builder.SetSafeState([=, this] { StopMotor(); });
builder.AddDoubleProperty(
- "Left Motor Speed", [=, this] { return m_leftMotor->Get(); },
- [=, this](double value) { m_leftMotor->Set(value); });
+ "Left Motor Speed", [&] { return m_leftOutput; }, m_leftMotor);
builder.AddDoubleProperty(
- "Right Motor Speed", [=, this] { return m_rightMotor->Get(); },
- [=, this](double value) { m_rightMotor->Set(value); });
+ "Right Motor Speed", [&] { return m_rightOutput; }, m_rightMotor);
}
diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index 2bf6a3f..aeec27d 100644
--- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -16,18 +16,32 @@
using namespace frc;
+WPI_IGNORE_DEPRECATED
+
MecanumDrive::MecanumDrive(MotorController& frontLeftMotor,
MotorController& rearLeftMotor,
MotorController& frontRightMotor,
MotorController& rearRightMotor)
- : m_frontLeftMotor(&frontLeftMotor),
- m_rearLeftMotor(&rearLeftMotor),
- m_frontRightMotor(&frontRightMotor),
- m_rearRightMotor(&rearRightMotor) {
- wpi::SendableRegistry::AddChild(this, m_frontLeftMotor);
- wpi::SendableRegistry::AddChild(this, m_rearLeftMotor);
- wpi::SendableRegistry::AddChild(this, m_frontRightMotor);
- wpi::SendableRegistry::AddChild(this, m_rearRightMotor);
+ : MecanumDrive{[&](double output) { frontLeftMotor.Set(output); },
+ [&](double output) { rearLeftMotor.Set(output); },
+ [&](double output) { frontRightMotor.Set(output); },
+ [&](double output) { rearRightMotor.Set(output); }} {
+ wpi::SendableRegistry::AddChild(this, &frontLeftMotor);
+ wpi::SendableRegistry::AddChild(this, &rearLeftMotor);
+ wpi::SendableRegistry::AddChild(this, &frontRightMotor);
+ wpi::SendableRegistry::AddChild(this, &rearRightMotor);
+}
+
+WPI_UNIGNORE_DEPRECATED
+
+MecanumDrive::MecanumDrive(std::function<void(double)> frontLeftMotor,
+ std::function<void(double)> rearLeftMotor,
+ std::function<void(double)> frontRightMotor,
+ std::function<void(double)> rearRightMotor)
+ : m_frontLeftMotor{std::move(frontLeftMotor)},
+ m_rearLeftMotor{std::move(rearLeftMotor)},
+ m_frontRightMotor{std::move(frontRightMotor)},
+ m_rearRightMotor{std::move(rearRightMotor)} {
static int instances = 0;
++instances;
wpi::SendableRegistry::AddLW(this, "MecanumDrive", instances);
@@ -47,10 +61,15 @@
auto [frontLeft, frontRight, rearLeft, rearRight] =
DriveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
- m_frontLeftMotor->Set(frontLeft * m_maxOutput);
- m_frontRightMotor->Set(frontRight * m_maxOutput);
- m_rearLeftMotor->Set(rearLeft * m_maxOutput);
- m_rearRightMotor->Set(rearRight * m_maxOutput);
+ m_frontLeftOutput = frontLeft * m_maxOutput;
+ m_rearLeftOutput = rearLeft * m_maxOutput;
+ m_frontRightOutput = frontRight * m_maxOutput;
+ m_rearRightOutput = rearRight * m_maxOutput;
+
+ m_frontLeftMotor(m_frontLeftOutput);
+ m_frontRightMotor(m_frontRightOutput);
+ m_rearLeftMotor(m_rearLeftOutput);
+ m_rearRightMotor(m_rearRightOutput);
Feed();
}
@@ -68,10 +87,16 @@
}
void MecanumDrive::StopMotor() {
- m_frontLeftMotor->StopMotor();
- m_frontRightMotor->StopMotor();
- m_rearLeftMotor->StopMotor();
- m_rearRightMotor->StopMotor();
+ m_frontLeftOutput = 0.0;
+ m_frontRightOutput = 0.0;
+ m_rearLeftOutput = 0.0;
+ m_rearRightOutput = 0.0;
+
+ m_frontLeftMotor(0.0);
+ m_frontRightMotor(0.0);
+ m_rearLeftMotor(0.0);
+ m_rearRightMotor(0.0);
+
Feed();
}
@@ -108,15 +133,15 @@
builder.SetActuator(true);
builder.SetSafeState([=, this] { StopMotor(); });
builder.AddDoubleProperty(
- "Front Left Motor Speed", [=, this] { return m_frontLeftMotor->Get(); },
- [=, this](double value) { m_frontLeftMotor->Set(value); });
+ "Front Left Motor Speed", [&] { return m_frontLeftOutput; },
+ m_frontLeftMotor);
builder.AddDoubleProperty(
- "Front Right Motor Speed", [=, this] { return m_frontRightMotor->Get(); },
- [=, this](double value) { m_frontRightMotor->Set(value); });
+ "Front Right Motor Speed", [&] { return m_frontRightOutput; },
+ m_frontRightMotor);
builder.AddDoubleProperty(
- "Rear Left Motor Speed", [=, this] { return m_rearLeftMotor->Get(); },
- [=, this](double value) { m_rearLeftMotor->Set(value); });
+ "Rear Left Motor Speed", [&] { return m_rearLeftOutput; },
+ m_rearLeftMotor);
builder.AddDoubleProperty(
- "Rear Right Motor Speed", [=, this] { return m_rearRightMotor->Get(); },
- [=, this](double value) { m_rearRightMotor->Set(value); });
+ "Rear Right Motor Speed", [&] { return m_rearRightOutput; },
+ m_rearRightMotor);
}
diff --git a/wpilibc/src/main/native/cpp/event/EventLoop.cpp b/wpilibc/src/main/native/cpp/event/EventLoop.cpp
index 5af79c9..c85286b 100644
--- a/wpilibc/src/main/native/cpp/event/EventLoop.cpp
+++ b/wpilibc/src/main/native/cpp/event/EventLoop.cpp
@@ -4,20 +4,41 @@
#include "frc/event/EventLoop.h"
+#include "frc/Errors.h"
+
using namespace frc;
+namespace {
+struct RunningSetter {
+ bool& m_running;
+ explicit RunningSetter(bool& running) noexcept : m_running{running} {
+ m_running = true;
+ }
+ ~RunningSetter() noexcept { m_running = false; }
+};
+} // namespace
+
EventLoop::EventLoop() {}
void EventLoop::Bind(wpi::unique_function<void()> action) {
+ if (m_running) {
+ throw FRC_MakeError(err::Error,
+ "Cannot bind EventLoop while it is running");
+ }
m_bindings.emplace_back(std::move(action));
}
void EventLoop::Poll() {
+ RunningSetter runSetter{m_running};
for (wpi::unique_function<void()>& action : m_bindings) {
action();
}
}
void EventLoop::Clear() {
+ if (m_running) {
+ throw FRC_MakeError(err::Error,
+ "Cannot clear EventLoop while it is running");
+ }
m_bindings.clear();
}
diff --git a/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp b/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp
index 4085658..78d580e 100644
--- a/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp
+++ b/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp
@@ -23,13 +23,14 @@
}
}
-void DriverStationModeThread::InAutonomous(bool entering) {
- m_userInAutonomous = entering;
-}
void DriverStationModeThread::InDisabled(bool entering) {
m_userInDisabled = entering;
}
+void DriverStationModeThread::InAutonomous(bool entering) {
+ m_userInAutonomous = entering;
+}
+
void DriverStationModeThread::InTeleop(bool entering) {
m_userInTeleop = entering;
}
diff --git a/wpilibc/src/main/native/cpp/motorcontrol/MotorController.cpp b/wpilibc/src/main/native/cpp/motorcontrol/MotorController.cpp
index 9d20144..2b64da2 100644
--- a/wpilibc/src/main/native/cpp/motorcontrol/MotorController.cpp
+++ b/wpilibc/src/main/native/cpp/motorcontrol/MotorController.cpp
@@ -9,5 +9,6 @@
using namespace frc;
void MotorController::SetVoltage(units::volt_t output) {
+ // NOLINTNEXTLINE(bugprone-integer-division)
Set(output / RobotController::GetBatteryVoltage());
}
diff --git a/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp b/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp
index f855d14..cf57f7c 100644
--- a/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp
+++ b/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp
@@ -12,6 +12,8 @@
// Can't use a delegated constructor here because of an MSVC bug.
// https://developercommunity.visualstudio.com/content/problem/583/compiler-bug-with-delegating-a-constructor.html
+WPI_IGNORE_DEPRECATED
+
MotorControllerGroup::MotorControllerGroup(
std::vector<std::reference_wrapper<MotorController>>&& motorControllers)
: m_motorControllers(std::move(motorControllers)) {
@@ -74,3 +76,5 @@
"Value", [=, this] { return Get(); },
[=, this](double value) { Set(value); });
}
+
+WPI_UNIGNORE_DEPRECATED
diff --git a/wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp b/wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp
index f25aa91..a05b7cc 100644
--- a/wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp
+++ b/wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp
@@ -11,6 +11,8 @@
using namespace frc;
+WPI_IGNORE_DEPRECATED
+
NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
: m_dio(dioChannel), m_pwm(pwmChannel) {
wpi::SendableRegistry::AddChild(this, &m_dio);
@@ -26,6 +28,8 @@
wpi::SendableRegistry::AddLW(this, "Nidec Brushless", pwmChannel);
}
+WPI_UNIGNORE_DEPRECATED
+
void NidecBrushless::Set(double speed) {
if (!m_disabled) {
m_speed = speed;
diff --git a/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp b/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
index 3692f75..924e4fd 100644
--- a/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
+++ b/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
@@ -8,13 +8,31 @@
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
+#include "frc/RobotController.h"
+
using namespace frc;
void PWMMotorController::Set(double speed) {
- m_pwm.SetSpeed(m_isInverted ? -speed : speed);
+ if (m_isInverted) {
+ speed = -speed;
+ }
+ m_pwm.SetSpeed(speed);
+
+ for (auto& follower : m_nonowningFollowers) {
+ follower->Set(speed);
+ }
+ for (auto& follower : m_owningFollowers) {
+ follower->Set(speed);
+ }
+
Feed();
}
+void PWMMotorController::SetVoltage(units::volt_t output) {
+ // NOLINTNEXTLINE(bugprone-integer-division)
+ Set(output / RobotController::GetBatteryVoltage());
+}
+
double PWMMotorController::Get() const {
return m_pwm.GetSpeed() * (m_isInverted ? -1.0 : 1.0);
}
@@ -48,11 +66,19 @@
m_pwm.EnableDeadbandElimination(eliminateDeadband);
}
+void PWMMotorController::AddFollower(PWMMotorController& follower) {
+ m_nonowningFollowers.emplace_back(&follower);
+}
+
+WPI_IGNORE_DEPRECATED
+
PWMMotorController::PWMMotorController(std::string_view name, int channel)
: m_pwm(channel, false) {
wpi::SendableRegistry::AddLW(this, name, channel);
}
+WPI_UNIGNORE_DEPRECATED
+
void PWMMotorController::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("Motor Controller");
builder.SetActuator(true);
diff --git a/wpilibc/src/main/native/cpp/motorcontrol/PWMSparkFlex.cpp b/wpilibc/src/main/native/cpp/motorcontrol/PWMSparkFlex.cpp
new file mode 100644
index 0000000..945a70e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/motorcontrol/PWMSparkFlex.cpp
@@ -0,0 +1,20 @@
+// 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/motorcontrol/PWMSparkFlex.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+PWMSparkFlex::PWMSparkFlex(int channel)
+ : PWMMotorController("PWMSparkFlex", channel) {
+ m_pwm.SetBounds(2.003_ms, 1.55_ms, 1.50_ms, 1.46_ms, 0.999_ms);
+ m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+ m_pwm.SetSpeed(0.0);
+ m_pwm.SetZeroLatch();
+
+ HAL_Report(HALUsageReporting::kResourceType_RevSparkFlexPWM,
+ GetChannel() + 1);
+}
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
index a19cc6a..8c0880a 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
@@ -30,12 +30,17 @@
m_impl->selectedTabPub =
m_impl->rootMetaTable->GetStringTopic("Selected")
.Publish(nt::PubSubOptions{.keepDuplicates = true});
- HAL_Report(HALUsageReporting::kResourceType_Shuffleboard, 0);
}
ShuffleboardInstance::~ShuffleboardInstance() = default;
+static bool gReported = false;
+
frc::ShuffleboardTab& ShuffleboardInstance::GetTab(std::string_view title) {
+ if (!gReported) {
+ HAL_Report(HALUsageReporting::kResourceType_Shuffleboard, 0);
+ gReported = true;
+ }
if (m_impl->tabs.find(title) == m_impl->tabs.end()) {
m_impl->tabs.try_emplace(title,
std::make_unique<ShuffleboardTab>(*this, title));
diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
index 8c27cdf..d5addb2 100644
--- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -58,7 +58,7 @@
}
void DifferentialDrivetrainSim::Update(units::second_t dt) {
- m_x = RK4([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
+ m_x = RKDP([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs);
}
@@ -92,26 +92,22 @@
}
units::ampere_t DifferentialDrivetrainSim::GetLeftCurrentDraw() const {
- auto loadIleft =
- m_motor.Current(
- units::radians_per_second_t{m_x(State::kLeftVelocity) *
- m_currentGearing / m_wheelRadius.value()},
- units::volt_t{m_u(0)}) *
- wpi::sgn(m_u(0));
-
- return loadIleft;
+ return m_motor.Current(units::radians_per_second_t{m_x(State::kLeftVelocity) *
+ m_currentGearing /
+ m_wheelRadius.value()},
+ units::volt_t{m_u(0)}) *
+ wpi::sgn(m_u(0));
}
units::ampere_t DifferentialDrivetrainSim::GetRightCurrentDraw() const {
- auto loadIRight =
- m_motor.Current(
- units::radians_per_second_t{m_x(State::kRightVelocity) *
- m_currentGearing / m_wheelRadius.value()},
- units::volt_t{m_u(1)}) *
- wpi::sgn(m_u(1));
-
- return loadIRight;
+ return m_motor.Current(
+ units::radians_per_second_t{m_x(State::kRightVelocity) *
+ m_currentGearing /
+ m_wheelRadius.value()},
+ units::volt_t{m_u(1)}) *
+ wpi::sgn(m_u(1));
}
+
units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
return GetLeftCurrentDraw() + GetRightCurrentDraw();
}
diff --git a/wpilibc/src/main/native/cpp/simulation/PneumaticsBaseSim.cpp b/wpilibc/src/main/native/cpp/simulation/PneumaticsBaseSim.cpp
index 476d07a..04aef3a 100644
--- a/wpilibc/src/main/native/cpp/simulation/PneumaticsBaseSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/PneumaticsBaseSim.cpp
@@ -12,11 +12,6 @@
using namespace frc;
using namespace frc::sim;
-PneumaticsBaseSim::PneumaticsBaseSim(int module) : m_index{module} {}
-
-PneumaticsBaseSim::PneumaticsBaseSim(const PneumaticsBase& module)
- : m_index{module.GetModuleNumber()} {}
-
std::shared_ptr<PneumaticsBaseSim> PneumaticsBaseSim::GetForType(
int module, PneumaticsModuleType type) {
switch (type) {
@@ -31,3 +26,8 @@
static_cast<int>(module));
}
}
+
+PneumaticsBaseSim::PneumaticsBaseSim(int module) : m_index{module} {}
+
+PneumaticsBaseSim::PneumaticsBaseSim(const PneumaticsBase& module)
+ : m_index{module.GetModuleNumber()} {}
diff --git a/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp b/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
index 90d9651..728d358 100644
--- a/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
@@ -338,6 +338,23 @@
HALSIM_SetRoboRioComments(comments.data(), comments.size());
}
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterRadioLEDStateCallback(
+ NotifyCallback callback, bool initialNotify) {
+ auto store = std::make_unique<CallbackStore>(
+ -1, callback, &HALSIM_CancelRoboRioRadioLEDStateCallback);
+ store->SetUid(HALSIM_RegisterRoboRioRadioLEDStateCallback(
+ &CallbackStoreThunk, store.get(), initialNotify));
+ return store;
+}
+
+RadioLEDState RoboRioSim::GetRadioLEDState() {
+ return static_cast<RadioLEDState>(HALSIM_GetRoboRioRadioLEDState());
+}
+
+void RoboRioSim::SetRadioLEDState(RadioLEDState state) {
+ HALSIM_SetRoboRioRadioLEDState(static_cast<HAL_RadioLEDState>(state));
+}
+
void RoboRioSim::ResetData() {
HALSIM_ResetRoboRioData();
}
diff --git a/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp b/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp
index 30a5650..1016c4b 100644
--- a/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp
@@ -18,8 +18,8 @@
m_simRange = deviceSim.GetDouble("Range (in)");
}
-void UltrasonicSim::SetRangeValid(bool isValid) {
- m_simRangeValid.Set(isValid);
+void UltrasonicSim::SetRangeValid(bool valid) {
+ m_simRangeValid.Set(valid);
}
void UltrasonicSim::SetRange(units::inch_t range) {
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
index e6991d4..d57473f 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -19,8 +19,6 @@
namespace {
struct Instance {
- Instance() { HAL_Report(HALUsageReporting::kResourceType_SmartDashboard, 0); }
-
detail::ListenerExecutor listenerExecutor;
std::shared_ptr<nt::NetworkTable> table =
nt::NetworkTableInstance::GetDefault().GetTable("SmartDashboard");
@@ -46,6 +44,8 @@
} // namespace frc::impl
#endif
+static bool gReported = false;
+
void SmartDashboard::init() {
GetInstance();
}
@@ -59,18 +59,23 @@
}
void SmartDashboard::SetPersistent(std::string_view key) {
- GetInstance().table->GetEntry(key).SetPersistent();
+ GetEntry(key).SetPersistent();
}
void SmartDashboard::ClearPersistent(std::string_view key) {
- GetInstance().table->GetEntry(key).ClearPersistent();
+ GetEntry(key).ClearPersistent();
}
bool SmartDashboard::IsPersistent(std::string_view key) {
- return GetInstance().table->GetEntry(key).IsPersistent();
+ return GetEntry(key).IsPersistent();
}
nt::NetworkTableEntry SmartDashboard::GetEntry(std::string_view key) {
+ if (!gReported) {
+ HAL_Report(HALUsageReporting::kResourceType_SmartDashboard,
+ HALUsageReporting::kSmartDashboard_Instance);
+ gReported = true;
+ }
return GetInstance().table->GetEntry(key);
}
@@ -78,6 +83,11 @@
if (!data) {
throw FRC_MakeError(err::NullParameter, "value");
}
+ if (!gReported) {
+ HAL_Report(HALUsageReporting::kResourceType_SmartDashboard,
+ HALUsageReporting::kSmartDashboard_Instance);
+ gReported = true;
+ }
auto& inst = GetInstance();
std::scoped_lock lock(inst.tablesToDataMutex);
auto& uid = inst.tablesToData[key];
@@ -120,7 +130,7 @@
bool SmartDashboard::SetDefaultBoolean(std::string_view key,
bool defaultValue) {
- return GetInstance().table->GetEntry(key).SetDefaultBoolean(defaultValue);
+ return GetEntry(key).SetDefaultBoolean(defaultValue);
}
bool SmartDashboard::GetBoolean(std::string_view keyName, bool defaultValue) {
@@ -133,7 +143,7 @@
bool SmartDashboard::SetDefaultNumber(std::string_view key,
double defaultValue) {
- return GetInstance().table->GetEntry(key).SetDefaultDouble(defaultValue);
+ return GetEntry(key).SetDefaultDouble(defaultValue);
}
double SmartDashboard::GetNumber(std::string_view keyName,
@@ -148,7 +158,7 @@
bool SmartDashboard::SetDefaultString(std::string_view key,
std::string_view defaultValue) {
- return GetInstance().table->GetEntry(key).SetDefaultString(defaultValue);
+ return GetEntry(key).SetDefaultString(defaultValue);
}
std::string SmartDashboard::GetString(std::string_view keyName,
@@ -158,63 +168,62 @@
bool SmartDashboard::PutBooleanArray(std::string_view key,
std::span<const int> value) {
- return GetInstance().table->GetEntry(key).SetBooleanArray(value);
+ return GetEntry(key).SetBooleanArray(value);
}
bool SmartDashboard::SetDefaultBooleanArray(std::string_view key,
std::span<const int> defaultValue) {
- return GetInstance().table->GetEntry(key).SetDefaultBooleanArray(
- defaultValue);
+ return GetEntry(key).SetDefaultBooleanArray(defaultValue);
}
std::vector<int> SmartDashboard::GetBooleanArray(
std::string_view key, std::span<const int> defaultValue) {
- return GetInstance().table->GetEntry(key).GetBooleanArray(defaultValue);
+ return GetEntry(key).GetBooleanArray(defaultValue);
}
bool SmartDashboard::PutNumberArray(std::string_view key,
std::span<const double> value) {
- return GetInstance().table->GetEntry(key).SetDoubleArray(value);
+ return GetEntry(key).SetDoubleArray(value);
}
bool SmartDashboard::SetDefaultNumberArray(
std::string_view key, std::span<const double> defaultValue) {
- return GetInstance().table->GetEntry(key).SetDefaultDoubleArray(defaultValue);
+ return GetEntry(key).SetDefaultDoubleArray(defaultValue);
}
std::vector<double> SmartDashboard::GetNumberArray(
std::string_view key, std::span<const double> defaultValue) {
- return GetInstance().table->GetEntry(key).GetDoubleArray(defaultValue);
+ return GetEntry(key).GetDoubleArray(defaultValue);
}
bool SmartDashboard::PutStringArray(std::string_view key,
std::span<const std::string> value) {
- return GetInstance().table->GetEntry(key).SetStringArray(value);
+ return GetEntry(key).SetStringArray(value);
}
bool SmartDashboard::SetDefaultStringArray(
std::string_view key, std::span<const std::string> defaultValue) {
- return GetInstance().table->GetEntry(key).SetDefaultStringArray(defaultValue);
+ return GetEntry(key).SetDefaultStringArray(defaultValue);
}
std::vector<std::string> SmartDashboard::GetStringArray(
std::string_view key, std::span<const std::string> defaultValue) {
- return GetInstance().table->GetEntry(key).GetStringArray(defaultValue);
+ return GetEntry(key).GetStringArray(defaultValue);
}
bool SmartDashboard::PutRaw(std::string_view key,
std::span<const uint8_t> value) {
- return GetInstance().table->GetEntry(key).SetRaw(value);
+ return GetEntry(key).SetRaw(value);
}
bool SmartDashboard::SetDefaultRaw(std::string_view key,
std::span<const uint8_t> defaultValue) {
- return GetInstance().table->GetEntry(key).SetDefaultRaw(defaultValue);
+ return GetEntry(key).SetDefaultRaw(defaultValue);
}
std::vector<uint8_t> SmartDashboard::GetRaw(
std::string_view key, std::span<const uint8_t> defaultValue) {
- return GetInstance().table->GetEntry(key).GetRaw(defaultValue);
+ return GetEntry(key).GetRaw(defaultValue);
}
bool SmartDashboard::PutValue(std::string_view keyName,
@@ -224,7 +233,7 @@
bool SmartDashboard::SetDefaultValue(std::string_view key,
const nt::Value& defaultValue) {
- return GetInstance().table->GetEntry(key).SetDefaultValue(defaultValue);
+ return GetEntry(key).SetDefaultValue(defaultValue);
}
nt::Value SmartDashboard::GetValue(std::string_view keyName) {
diff --git a/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp b/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp
new file mode 100644
index 0000000..b99b792
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/sysid/SysIdRoutineLog.cpp
@@ -0,0 +1,66 @@
+// 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/sysid/SysIdRoutineLog.h"
+
+#include <fmt/format.h>
+
+#include "frc/DataLogManager.h"
+
+using namespace frc::sysid;
+
+SysIdRoutineLog::SysIdRoutineLog(std::string_view logName)
+ : m_logName(logName),
+ m_state(wpi::log::StringLogEntry{
+ frc::DataLogManager::GetLog(),
+ fmt::format("sysid-test-state{}", logName)}) {
+ m_state.Append(StateEnumToString(State::kNone));
+}
+
+SysIdRoutineLog::MotorLog::MotorLog(std::string_view motorName,
+ std::string_view logName,
+ LogEntries* logEntries)
+ : m_motorName(motorName), m_logName(logName), m_logEntries(logEntries) {
+ (*logEntries)[motorName] = MotorEntries();
+}
+
+SysIdRoutineLog::MotorLog& SysIdRoutineLog::MotorLog::value(
+ std::string_view name, double value, std::string_view unit) {
+ auto& motorEntries = (*m_logEntries)[m_motorName];
+
+ if (!motorEntries.contains(name)) {
+ wpi::log::DataLog& log = frc::DataLogManager::GetLog();
+
+ motorEntries[name] = wpi::log::DoubleLogEntry(
+ log, fmt::format("{}-{}-{}", name, m_motorName, m_logName), unit);
+ }
+
+ motorEntries[name].Append(value);
+ return *this;
+}
+
+SysIdRoutineLog::MotorLog SysIdRoutineLog::Motor(std::string_view motorName) {
+ return MotorLog{motorName, m_logName, &m_logEntries};
+}
+
+void SysIdRoutineLog::RecordState(State state) {
+ m_state.Append(StateEnumToString(state));
+}
+
+std::string SysIdRoutineLog::StateEnumToString(State state) {
+ switch (state) {
+ case State::kQuasistaticForward:
+ return "quasistatic-forward";
+ case State::kQuasistaticReverse:
+ return "quasistatic-reverse";
+ case State::kDynamicForward:
+ return "dynamic-forward";
+ case State::kDynamicReverse:
+ return "dynamic-reverse";
+ case State::kNone:
+ return "none";
+ default:
+ return "none";
+ }
+}
diff --git a/wpilibc/src/main/native/cpp/util/Color.cpp b/wpilibc/src/main/native/cpp/util/Color.cpp
deleted file mode 100644
index e3adaf2..0000000
--- a/wpilibc/src/main/native/cpp/util/Color.cpp
+++ /dev/null
@@ -1,15 +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/util/Color.h"
-
-#include <fmt/format.h>
-
-using namespace frc;
-
-std::string Color::HexString() const {
- return fmt::format("#{:02X}{:02X}{:02X}", static_cast<int>(255.0 * red),
- static_cast<int>(255.0 * green),
- static_cast<int>(255.0 * blue));
-}
diff --git a/wpilibc/src/main/native/cpp/util/Color8Bit.cpp b/wpilibc/src/main/native/cpp/util/Color8Bit.cpp
deleted file mode 100644
index af200a2..0000000
--- a/wpilibc/src/main/native/cpp/util/Color8Bit.cpp
+++ /dev/null
@@ -1,13 +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/util/Color8Bit.h"
-
-#include <fmt/format.h>
-
-using namespace frc;
-
-std::string Color8Bit::HexString() const {
- return fmt::format("#{:02X}{:02X}{:02X}", red, green, blue);
-}