Squashed 'third_party/allwpilib/' changes from e473a00f97..e4b91005cf
e4b91005cf [examples] Update SwerveModule constructor doc (NFC) (#4042)
a260bfd83b [examples] Remove "this" keyword from SwerveModule (#4043)
18e262a100 [examples] Fix multiple doc typos in SwerveControllerCommand example (NFC) (#4044)
4bd1f526ab [wpilibc] Prevent StopMotor from terminating robot during MotorSafety check (#4038)
27847d7eb2 [sim] Expose GUI control functions via HAL_RegisterExtension (#4034)
b2a8d3f0f3 [wpilibc] Add mechanism to reset MotorSafety list (#4037)
49adac9564 [wpilib] Check for signedness in ArcadeDriveIK() (#4028)
a19d1133b1 [wpiutil] libuv: Fix sign compare warnings in gcc 11.2 (#4031)
dde91717e4 [build] cmake: Add ability to customize target warnings (#4032)
e9050afd67 [sim] Update sim match time to match real robot (#4024)
165d2837cf [wpilib] Preferences: Set Persistent in Init methods (#4025)
ac7549edca [glass] Fix snprintf truncation warning (#4029)
4d96bc72e0 [wpilibj] Fix typos in error messages for non-null assertions (#4014)
3411eee20f [hal] Replace hardcoded sim array sizes with constants (#4015)
74de97eeca [wpilibc] Add mechanism to reset various global structures (#4007)
4e3cc25012 [examples] Fix periodic function rate comment (NFC) (#4013)
90c1db393e [sim] Add exported functions to control the sim GUI (#3995)
2f43274aa4 [wpilibj] MechanismRoot2d: Add flush to setPosition (#4011)
aeca09db09 [glass] Support remapping of Enter key (#3994)
c107f22c67 [sim] Sim GUI: don't force-show Timing and Other Devices (#4001)
68fe51e8da [wpigui] Update PFD to latest, fix kdialog multiselect (#4005)
8d08d67cf1 [wpigui] PFD: Add console warning if file chooser unavailable (#4003)
4f1782f66e [wpilibc] Only call HAL_Report when initializing SmartDashboard (#4006)
3f77725cd3 Remove uses of iostream (#4004)
5635f33a32 [glass] Increase plot depth to 20K points (#3993)
bca4b7111b [glass] Fix PlotSeries::SetSource() (#3991)
6a6366b0d6 [commands] Add until() as alias for withInterrupt() (#3981)
16bf2c70c5 [wpilib] Fix joystick out of range error messages (#3988)
4b3edb742c [wpilib] Fix ADIS16448 IMU default constructor not working in Java (#3989)
fcf23fc9e9 [hal] Fix potential gamedata out of bounds read (#3983)
af5ef510c5 [wpilibc] Fix REV PH pulse duration units (#3982)
05401e2b81 [wpilib] Write REV PH firmware version to roboRIO to display on driver station (#3977)
9fde0110b6 Update to 2022 v4.0 image (#3944)
b03f8ddb2e [examples] fix incorrect variable in Arm Simulation Pref (#3980)
a26df2a022 [examples] Update ArmSimulation example to use Preferences (#3976)
d68d6674e8 [examples] Armbot: rename kCos to kG (#3975)
a8f0f6bb90 [wpilibj] Fix ADIS16448 getRate to return rate instead of angle (#3974)
dd9c92d5bf [build] Remove debug info from examples (#3971)
84df14dd70 [rtns] Fix icons (#3972)
560094ad92 [examples] Correct Mecanum example axes (#3955)
7ea1be9c01 [wpilibc] Fix typo in hardware version for REV PDH (#3969)
700f13bffd [wpilibj] Make methods public for Java REV PDH (#3970)
b6aa7c1aa9 [wpilibj] Make methods public for Java REVPH (#3968)
eb4d183e48 [wpimath] Fix clang-tidy bugprone-integer-division warning (#3966)
77e4e81e1e [wpilib] Add Field widget to BuiltInWidgets in shuffleboard (#3961)
88f5cb6eb0 [build] Publish PDBs with C++ tools (#3960)
efae552f3e [wpimath] Remove DifferentialDriveKinematics include from odometry (#3958)
46b277421a [glass] Update Speed Controller Type name for 2022 WPILib (#3952)
42908126b9 [wpilib] Add DCMotorSim (#3910)
a467392cbd [wpiutil] StackTrace: Add ability to override default implementation (#3951)
78d0bcf49d [templates] Add SimulationInit()/SimulationPeriodic() to robot templates (#3943)
02a0ced9b0 [wpilib] MecanumDrive: update docs for axis to match implementation (NFC) (#3942)
4ccfe1c9f2 [wpilib] Added docs clarification on units for drive class WheelSpeeds (NFC) (#3939)
830c0c5c2f [wpilib] MechanismLigament2d: Add getters for color and line weight (#3947)
5548a37465 [wpilib] PowerDistribution: Add module type getter (#3948)
2f9a600de2 [hal] Fix PCM one shot (#3949)
559db11a20 [myRobot] Skip deploying debug libraries for myRobot deploys (#3950)
76c78e295b [examples] Reorder SwerveModules in SwerveControllerCommand example odometry update (#3934)
debbd5ff4b [wpilib] Improve PowerDistribution docs (NFC) (#3925)
841174f302 [commands] Change command vendordep JSON version number to 1.0.0 (#3938)
8c55844f91 [wpilib] Remove comment about Mecanum right side inverted (NFC) (#3929)
0b990bf0f5 [hal] Fix PCM sticky faults clear function crashing (#3932)
104d7e2abc [hal] Don't throw exceptions in PCM JNI (#3933)
5ba69e1af1 [examples] Updated type in Java SwerveModule (#3928)
f3a0b5c7d7 [wpimath] Fix Java SimpleMotorFeedforward Docs (NFC) (#3926)
7f4265facc [wpimath] Add LinearFilter::FiniteDifference() (#3900)
63d1fb3bed [wpiutil] Modify fmt to not throw on write failure (#3919)
36af6d25a5 [wpimath] Fix input vector in pose estimator docs (NFC) (#3923)
8f387f7255 [wpilibj] Switch ControlWord mutex to actual reentrant mutex (#3922)
792e735e08 [wpimath] Move TrajectoryGenerator::SetErrorHandler definition to .cpp (#3920)
3b76de83eb [commands] Fix ProfiledPIDCommand use-after-free (#3904)
ad9f738cfa [fieldimages] Fix maven publishing (#3897)
49455199e5 [examples] Use left/rightGroup.Get() for simulator inputs to fix inversions (#3908)
64426502ea [wpimath] Fix arm -> flywheel typo (NFC) (#3911)
8cc112d196 [wpiutil] Fix wpi::array for move-only types (#3917)
e78cd49861 [build] Upgrade Java formatter plugins (#3894)
cfb4f756d6 [build] Upgrade to shadow 7.1.2 (#3893)
ba0908216c [wpimath] Fix crash in KF latency compensator (#3888)
a3a0334fad [build] cmake: Move fieldImages to WITH_GUI (#3885)
cf7460c3a8 [fieldImages] Add 2022 field (#3883)
db0fbb6448 [wpimath] Fix LQR matrix constructor overload for Q, R, and N (#3884)
8ac45f20bb [commands] Update Command documentation (NFC) (#3881)
b3707cca0b [wpiutil] Upgrade to fmt 8.1.1 (#3879)
a69ee3ece9 [wpimath] Const-qualify Twist2d scalar multiply (#3882)
750d9a30c9 [examples] Fix Eigen out of range error when running example (#3877)
41c5b2b5ac [rtns] Add cmake build (#3866)
6cf3f9b28e [build] Upgrade to Gradle 7.3.3 (#3878)
269cf03472 [examples] Add communication examples (e.g. arduino) (#2500)
5ccfc4adbd [oldcommands] Deprecate PIDWrappers, since they use deprecated interfaces (#3868)
b6f44f98be [hal] Add warning about onboard I2C (#3871)
0dca57e9ec [templates] romieducational: Invert drivetrain and disable motor safety (#3869)
22c4da152e [wpilib] Add GetRate() to ADIS classes (#3864)
05d66f862d [templates] Change the template ordering to put command based first (#3863)
b09f5b2cf2 [wpilibc] Add virtual dtor for LinearSystemSim (#3861)
a2510aaa0e [wpilib] Make ADIS IMU classes unit-safe (#3860)
947f589916 [wpilibc] Rename ADIS_16470_IMU.cpp to match class name (#3859)
bbd8980a20 [myRobot] Fix cameraserver library order (#3858)
831052f118 [wpilib] Add simulation support to ADIS classes (#3857)
c137569f91 [wpilib] Throw exception if the REV Pneumatic Hub firmware version is older than 22.0.0 (#3853)
dae61226fa Fix Maven Artifacts readme (#3856)
3ad4594a88 Update Maven artifacts readme for 2022 (#3855)
112acb9a62 [wpilibc] Move ADIS IMU constants to inside class (#3852)
ecee224e81 [wpilib] Allow SendableCameraWrappers to take arbitrary URLs (#3850)
a3645dea34 LICENSE: Bump year range to include 2022 (#3854)
7c09f44898 [wpilib] Use PSI for compressor config and sensor reading (#3847)
f401ea9aae [wpigui] Remove wpiutil linkage (#3851)
bf8517f1e6 [wpimath] TimeInterpolatableBufferTest: Fix lint warnings (#3849)
528087e308 [hal] Use enums with fixed underlying type in clang C (#3297)
1f59ff72f9 [wpilib] Add ADIS IMUs (#3777)
315be873c4 [wpimath] Add TimeInterpolatableBuffer (#2695)
b8d019cdb4 [wpilib] Rename NormalizeWheelSpeeds to DesaturateWheelSpeeds (#3791)
102f23bbdb [wpilibj] DriverStation: Set thread interrupted state (#3846)
b85c24a79c [wpilib] Add warning about onboard I2C (#3842)
eee29daaf9 [newCommands] Trigger: Allow override of debounce type (#3845)
aa9dfabde2 [wpimath] Move debouncer to filters (#3838)
5999a26fba [wpiutil] Add GetSystemTime() (#3840)
1e82595ffb [examples] Fix arcade inversions (#3841)
e373fa476b [wpiutil] Add disableMockTime to JNI (#3839)
dceb5364f4 [examples] Ensure right side motors are inverted (#3836)
baacbc8e24 [wpilib] Tachometer: Add function to return RPS (#3833)
84b15f0883 [templates] Add Java Romi Educational template (#3837)
c0da9d2d35 [examples] Invert Right Motor in Romi Java examples (#3828)
0fe0be2733 [build] Change project year to intellisense (#3835)
eafa947338 [wpimath] Make copies of trajectory constraint arguments (#3832)
9d13ae8d01 [wpilib] Add notes for Servo get that it only returns cmd (NFC) (#3820)
2a64e4bae5 [wpimath] Give drivetrain a more realistic width in TrajectoryJsonTest.java (#3822)
c3fd20db59 [wpilib] Fix trajectory sampling in DifferentialDriveSim test (#3821)
6f91f37cd0 [examples] Fix SwerveControllerCommand order of Module States (#3815)
5158730b81 [wpigui] Upgrade to imgui 1.86, GLFW 3.3.6 (#3817)
2ad2d2ca96 [wpiutil] MulticastServiceResolver: Fix C array returning functions (#3816)
b5fd29774f [wpilibj] Trigger: implement BooleanSupplier interface (#3811)
9f8f330e96 [wpilib] Fix Mecanum and SwerveControllerCommand when desired rotation passed (#3808)
1ad3b1b333 [hal] Don't copy byte to where null terminator goes (#3807)
dfc24425c3 [build] Fix gazebo gradle IDE warnings (#3806)
c02577bb51 [glass] Configure delay loading for windows camera server support (#3803)
c9e6a96a61 [wpilib] Document range of Servo angle (NFC) (#3796)
9778626f34 [wpilib, hal] Add support for getting faults and versions from power distribution (#3794)
34b2d0dae1 [wpilib, hal] High Level REV PH changes (#3792)
59a7528fd6 [cscore] Fix crash when usbcamera is deleted before message pump thread fully starts (#3804)
11d9859ef1 [build] Update plugins to remove log4j vulnerabilities (#3805)
e44ed752ad [glass] Fix CollapsingHeader in Encoder, PCM, and DeviceTree (#3797)
52b2dd5b89 [build] Bump native utils to remove log4j (#3802)
c46636f218 [wpilib] Improve new counter classes documentation (NFC) (#3801)
dc531462e1 [build] Update to gradle 7.3.2 (#3800)
92ba98621c [wpimath] Add helper variable templates for units type traits (#3790)
d41d051f1b [wpilibc] Fix Mecanum & Swerve ControllerCommand lambda capture (#3795)
c5ae0effac OtherVersions.md: Add one missing case of useLocal (#3788)
b3974c6ed3 [wpimath] Upgrade to Drake v0.37.0 (#3786)
589a00e379 [wpilibc] Start DriverStation thread from RobotBase (#3785)
8d9836ca02 [wpilib] Improve curvature drive documentation (NFC) (#3783)
8b5bf8632e [myRobot] Add wpimath and wpiutil JNI (#3784)
1846114491 [examples] Update references from characterization to SysId (NFC) (#3782)
2c461c794e [build] Update to gradle 7.3 (#3778)
109363daa4 [hal] Add remaining driver functions for REVPH (#3776)
41d26bee8d [hal] Refactor REV PDH (#3775)
7269a170fb Upgrade maven deps to latest versions and fix new linter errors (#3772)
441f2ed9b0 [build] actions: use fixed image versions instead latest (#3761)
15275433d4 [examples] Fix duplicate port allocations in C++ SwerveBot/SwerveDrivePoseEstimator/RomiReference (#3773)
1ac02d2f58 [examples] Fix drive Joystick axes in several examples (#3769)
8ee6257e92 [wpilib] DifferentialDrivetrainSim.KitbotMotor: Add NEO and Falcon 500 (#3762)
d81ef2bc5c [wpilib] Fix deadlocks in Mechanism2d et al. (#3770)
acb64dff97 [wpimath] Make RamseteController::Calculate() more concise (#3763)
3f6cf76a8c [hal] Refactor REV PH CAN frames (#3756)
3ef2dab465 [wpilib] DutyCycleEncoder: add setting of duty cycle range (#3759)
a5a56dd067 Readme: Add Visual Studio 2022 (#3760)
04957a6d30 [wpimath] Fix units of RamseteController's b and zeta (#3757)
5da54888f8 [glass] Upgrade imgui to 0.85, implot to HEAD, glfw to 3.3.5 (#3754)
6c93365b0f [wpiutil] MulticastService cleanup (#3750)
1c4a8bfb66 [cscore] Cleanup Windows USB camera impl (#3751)
d51a1d3b3d [rtns] Fix icon (#3749)
aced2e7da6 Add roboRIO Team Number Setter tool (#3744)
fa1ceca83a [wpilibj] Use DS cache for iterative robot control word cache (#3748)
0ea05d34e6 [build] Update to gradle 7.2 (#3746)
09db4f672b [build] Update to native utils 2022.6.1 (#3745)
4ba80a3a8c [wpigui] Don't recursively render frames in size callback (#3743)
ae208d2b17 [wpiutil] StringExtras: Add substr() (#3742)
6f51cb3b98 [wpiutil] MulticastResolver: make event manual reset, change to multiple read (#3736)
f6159ee1a2 [glass] Fix Drive widget handling of negative rotation (#3739)
7f401ae895 [build] Update NI libraries to 2022.2.3 (#3738)
0587b7043a [glass] Use JSON files for storage instead of imgui ini
0bbf51d566 [wpigui] Change maximized to bool
92c6eae6b0 [wpigui] PFD: Add explicit to constructors
141354cd79 [wpigui] Add hooks for custom load/save settings
f6e9fc7d71 [wpiutil] Handle multicast service collision on linux (#3734)
d8418be7d1 [glass, outlineviewer] Return 0 from WinMain (#3735)
82066946e5 [wpiutil] Add mDNS resolver and announcer (#3733)
4b1defc8d8 [wpilib] Remove automatic PD type from module type enum (#3732)
da90c1cd2c [wpilib] Add bang-bang controller (#3676)
3aa54fa027 [wpilib] Add new counter implementations (#2447)
b156db400d [hal, wpilib] Incorporate pneumatic control type into wpilibc/j (#3728)
9aba2b7583 [oldCommands] Add wrappers for WPILib objects to work with old PID Controller (#3710)
a9931223f0 [hal] Add REV PH faults (#3729)
aacf9442e4 [wpimath] Fix units typo in LinearSystemId source comment (#3730)
7db10ecf00 [wpilibc] Make SPI destructor virtual since SPI contains virtual functions (#3727)
a0a5b2aea5 [wpimath] Upgrade to EJML 0.41 (#3726)
eb835598a4 [hal] Add HAL functions for compressor config modes on REV PH (#3724)
f0ab6df5b6 [wpimath] Upgrade to Drake v0.36.0 (#3722)
075144faa3 [docs] Parse files without extensions with Doxygen (#3721)
32468a40cb [hal] Remove use of getDmaDescriptor from autospi (#3717)
38611e9dd7 [hal] Fix REVPH Analog Pressure channel selection (#3716)
4d78def31e [wpilib] Add DeadbandElimination forwarding to PWMMotorController (#3714)
3be0c1217a [wpilibcExamples] Make GearsBot use idiomatic C++ (#3711)
42d3a50aa2 [hal] Check error codes during serial port initialization (#3712)
52f1464029 Add project with field images and their json config files (#3668)
68ce62e2e9 [hal] Add autodetect for power modules (#3706)
3dd41c0d37 [wpilib] Don't print PD errors for LiveWindow reads (#3708)
7699a1f827 [hal] Fix sim not working with automatic PD type and default module (#3707)
Change-Id: I6b0b8fa8b2d2a24071240f624db9ec6d127f6648
git-subtree-dir: third_party/allwpilib
git-subtree-split: e4b91005cf69161f1cb3d934f6526232e6b9169e
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
new file mode 100644
index 0000000..25d674f
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
@@ -0,0 +1,874 @@
+// 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.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. 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. */
+/* */
+/* Modified by Juan Chong - frcsupport@analog.com */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ADIS16448_IMU.h"
+
+#include <frc/DigitalInput.h>
+#include <frc/DigitalOutput.h>
+#include <frc/DigitalSource.h>
+#include <frc/DriverStation.h>
+#include <frc/SPI.h>
+#include <frc/Timer.h>
+
+#include <algorithm>
+#include <cmath>
+#include <string>
+
+#include <hal/HAL.h>
+#include <networktables/NTSendableBuilder.h>
+#include <wpi/numbers>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+
+/* Helpful conversion functions */
+static inline uint16_t BuffToUShort(const uint32_t* buf) {
+ return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
+}
+
+static inline uint8_t BuffToUByte(const uint32_t* buf) {
+ return static_cast<uint8_t>(buf[0]);
+}
+
+static inline int16_t BuffToShort(const uint32_t* buf) {
+ return (static_cast<int16_t>(buf[0]) << 8) | buf[1];
+}
+
+static inline uint16_t ToUShort(const uint8_t* buf) {
+ return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
+}
+
+using namespace frc;
+
+namespace {
+template <typename S, typename... Args>
+inline void ADISReportError(int32_t status, const char* file, int line,
+ const char* function, const S& format,
+ Args&&... args) {
+ frc::ReportErrorV(status, file, line, function, format,
+ fmt::make_args_checked<Args...>(format, args...));
+}
+} // namespace
+
+#define REPORT_WARNING(msg) \
+ ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
+#define REPORT_ERROR(msg) \
+ ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
+
+ADIS16448_IMU::ADIS16448_IMU()
+ : ADIS16448_IMU(kZ, SPI::Port::kMXP, CalibrationTime::_512ms) {}
+
+ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
+ CalibrationTime cal_time)
+ : m_yaw_axis(yaw_axis),
+ m_spi_port(port),
+ m_simDevice("Gyro:ADIS16448", port) {
+ if (m_simDevice) {
+ m_simGyroAngleX =
+ m_simDevice.CreateDouble("gyro_angle_x", hal::SimDevice::kInput, 0.0);
+ m_simGyroAngleY =
+ m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0);
+ m_simGyroAngleZ =
+ m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0);
+ m_simGyroRateX =
+ m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
+ m_simGyroRateY =
+ m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
+ m_simGyroRateZ =
+ m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
+ m_simAccelX =
+ m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
+ m_simAccelY =
+ m_simDevice.CreateDouble("accel_y", hal::SimDevice::kInput, 0.0);
+ m_simAccelZ =
+ m_simDevice.CreateDouble("accel_z", hal::SimDevice::kInput, 0.0);
+ }
+
+ if (!m_simDevice) {
+ // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+ // Relies on the RIO hardware by default configuring an output as low
+ // and configuring an input as high Z. The 10k pull-up resistor internal to
+ // the IMU then forces the reset line high for normal operation.
+ DigitalOutput* m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
+ Wait(10_ms); // Wait 10ms
+ delete m_reset_out;
+ new DigitalInput(18); // Set MXP DIO8 high
+ Wait(500_ms); // Wait 500ms for reset to complete
+
+ ConfigCalTime(cal_time);
+
+ // Configure standard SPI
+ if (!SwitchToStandardSPI()) {
+ return;
+ }
+
+ // Set IMU internal decimation to 819.2 SPS
+ WriteRegister(SMPL_PRD, 0x0001);
+ // Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
+ WriteRegister(MSC_CTRL, 0x0016);
+ // Disable IMU internal Bartlett filter
+ WriteRegister(SENS_AVG, 0x0400);
+ // Clear offset registers
+ WriteRegister(XGYRO_OFF, 0x0000);
+ WriteRegister(YGYRO_OFF, 0x0000);
+ WriteRegister(ZGYRO_OFF, 0x0000);
+ // Configure and enable auto SPI
+ if (!SwitchToAutoSPI()) {
+ return;
+ }
+ // Notify DS that IMU calibration delay is active
+ REPORT_WARNING(
+ "ADIS16448 IMU Detected. Starting initial calibration delay.");
+ // Wait for whatever time the user set as the start-up delay
+ Wait(static_cast<double>(m_calibration_time) * 1.2_s);
+ // Execute calibration routine
+ Calibrate();
+ // Reset accumulated offsets
+ Reset();
+ // Tell the acquire loop that we're done starting up
+ m_start_up_mode = false;
+
+ // Let the user know the IMU was initiallized successfully
+ REPORT_WARNING("ADIS16448 IMU Successfully Initialized!");
+
+ // TODO: Find what the proper pin is to turn this LED
+ // Drive MXP PWM5 (IMU ready LED) low (active low)
+ new DigitalOutput(19);
+ }
+
+ // Report usage and post data to DS
+ HAL_Report(HALUsageReporting::kResourceType_ADIS16448, 0);
+
+ wpi::SendableRegistry::AddLW(this, "ADIS16448", port);
+}
+
+/**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in standard SPI mode.
+ *
+ * This function switches the active SPI port to standard SPI and is used
+ *primarily when exiting auto SPI. Exiting auto SPI is required to read or write
+ *using SPI since the auto SPI configuration, once active, locks the SPI message
+ *being transacted. This function also verifies that the SPI port is operating
+ *in standard SPI mode by reading back the IMU product ID.
+ **/
+bool ADIS16448_IMU::SwitchToStandardSPI() {
+ // Check to see whether the acquire thread is active. If so, wait for it to
+ // stop producing data.
+ if (m_thread_active) {
+ m_thread_active = false;
+ while (!m_thread_idle) {
+ Wait(10_ms);
+ }
+ // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
+ if (m_spi != nullptr && m_auto_configured) {
+ m_spi->StopAuto();
+ // We need to get rid of all the garbage left in the auto SPI buffer after
+ // stopping it. Sometimes data magically reappears, so we have to check
+ // the buffer size a couple of times
+ // to be sure we got it all. Yuck.
+ uint32_t trashBuffer[200];
+ Wait(100_ms);
+ int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ while (data_count > 0) {
+ /* Dequeue 200 at a time, or the remainder of the buffer if less than
+ * 200 */
+ m_spi->ReadAutoReceivedData(trashBuffer, std::min(200, data_count),
+ 0_s);
+ /* Update remaining buffer count */
+ data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ }
+ }
+ }
+ // There doesn't seem to be a SPI port active. Let's try to set one up
+ if (m_spi == nullptr) {
+ m_spi = new SPI(m_spi_port);
+ m_spi->SetClockRate(1000000);
+ m_spi->SetMSBFirst();
+ m_spi->SetSampleDataOnTrailingEdge();
+ m_spi->SetClockActiveLow();
+ m_spi->SetChipSelectActiveLow();
+ ReadRegister(PROD_ID); // Dummy read
+
+ // Validate the product ID
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16448) {
+ REPORT_ERROR("Could not find ADIS16448!");
+ Close();
+ return false;
+ }
+ return true;
+ } else {
+ // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
+ // product ID.
+ ReadRegister(PROD_ID); // Dummy read
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16448) {
+ REPORT_ERROR("Could not find ADIS16448!");
+ Close();
+ return false;
+ } else {
+ return true;
+ }
+ }
+}
+
+void ADIS16448_IMU::InitOffsetBuffer(int size) {
+ // avoid exceptions in the case of bad arguments
+ if (size < 1) {
+ size = 1;
+ }
+
+ // set average size to size (correct bad values)
+ m_avg_size = size;
+
+ // resize vector
+ if (m_offset_buffer != nullptr) {
+ delete[] m_offset_buffer;
+ }
+ m_offset_buffer = new OffsetData[size];
+
+ // set accumulate count to 0
+ m_accum_count = 0;
+}
+
+/**
+ * This function switches the active SPI port to auto SPI and is used primarily
+ *when exiting standard SPI. Auto SPI is required to asynchronously read data
+ *over SPI as it utilizes special FPGA hardware to react to an external data
+ *ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
+ *can be read by the CPU asynchronously. Standard SPI transactions are
+ * impossible on the selected SPI port once auto SPI is enabled. The stall
+ *settings, GPIO interrupt pin, and data packet settings used in this function
+ *are hard-coded to work only with the ADIS16448 IMU.
+ **/
+bool ADIS16448_IMU::SwitchToAutoSPI() {
+ // No SPI port has been set up. Go set one up first.
+ if (m_spi == nullptr) {
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to start/restart auto SPI");
+ return false;
+ }
+ }
+ // Only set up the interrupt if needed.
+ if (m_auto_interrupt == nullptr) {
+ m_auto_interrupt = new DigitalInput(10);
+ }
+ // The auto SPI controller gets angry if you try to set up two instances on
+ // one bus.
+ if (!m_auto_configured) {
+ m_spi->InitAuto(8200);
+ m_auto_configured = true;
+ }
+ // Set auto SPI packet data and size
+ m_spi->SetAutoTransmitData({{GLOB_CMD}}, 27);
+ // Configure auto stall time
+ m_spi->ConfigureAutoStall(HAL_SPI_kMXP, 100, 1000, 255);
+ // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
+ // activated)
+ m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
+ // Check to see if the acquire thread is running. If not, kick one off.
+ if (!m_thread_idle) {
+ m_first_run = true;
+ m_thread_active = true;
+ // Set up circular buffer
+ InitOffsetBuffer(m_avg_size);
+ // Kick off acquire thread
+ m_acquire_task = std::thread(&ADIS16448_IMU::Acquire, this);
+ } else {
+ m_first_run = true;
+ m_thread_active = true;
+ }
+ // Looks like the thread didn't start for some reason. Abort.
+ /*
+ if(!m_thread_idle) {
+ REPORT_ERROR("Failed to start/restart the acquire() thread.");
+ Close();
+ return false;
+ }
+ */
+ return true;
+}
+
+/**
+ *
+ **/
+int ADIS16448_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
+ if (m_calibration_time == new_cal_time) {
+ return 1;
+ } else {
+ m_calibration_time = new_cal_time;
+ m_avg_size = static_cast<uint16_t>(m_calibration_time) * 819;
+ InitOffsetBuffer(m_avg_size);
+ return 0;
+ }
+}
+
+/**
+ *
+ **/
+void ADIS16448_IMU::Calibrate() {
+ std::scoped_lock sync(m_mutex);
+ // Calculate the running average
+ int gyroAverageSize = std::min(m_accum_count, m_avg_size);
+ double accum_gyro_rate_x = 0.0;
+ double accum_gyro_rate_y = 0.0;
+ double accum_gyro_rate_z = 0.0;
+ for (int i = 0; i < gyroAverageSize; i++) {
+ accum_gyro_rate_x += m_offset_buffer[i].gyro_rate_x;
+ accum_gyro_rate_y += m_offset_buffer[i].gyro_rate_y;
+ accum_gyro_rate_z += m_offset_buffer[i].gyro_rate_z;
+ }
+ m_gyro_rate_offset_x = accum_gyro_rate_x / gyroAverageSize;
+ m_gyro_rate_offset_y = accum_gyro_rate_y / gyroAverageSize;
+ m_gyro_rate_offset_z = accum_gyro_rate_z / gyroAverageSize;
+ m_integ_gyro_angle_x = 0.0;
+ m_integ_gyro_angle_y = 0.0;
+ m_integ_gyro_angle_z = 0.0;
+}
+
+/**
+ * This function reads the contents of an 8-bit register location by
+ *transmitting the register location byte along with a null (0x00) byte using
+ *the standard WPILib API. The response (two bytes) is read back using the
+ *WPILib API and joined using a helper function. This function assumes the
+ *controller is set to standard SPI mode.
+ **/
+uint16_t ADIS16448_IMU::ReadRegister(uint8_t reg) {
+ uint8_t buf[2];
+ buf[0] = reg & 0x7f;
+ buf[1] = 0;
+
+ m_spi->Write(buf, 2);
+ m_spi->Read(false, buf, 2);
+
+ return ToUShort(buf);
+}
+
+/**
+ * This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
+ *via SPI. The upper and lower bytes that make up the 16-bit value are split
+ *into two unsined, 8-bit values and written to the upper and lower addresses of
+ *the specified register value. Only the lower (base) address must be specified.
+ *This function assumes the controller is set to standard SPI mode.
+ **/
+void ADIS16448_IMU::WriteRegister(uint8_t reg, uint16_t val) {
+ uint8_t buf[2];
+ buf[0] = 0x80 | reg;
+ buf[1] = val & 0xff;
+ m_spi->Write(buf, 2);
+ buf[0] = 0x81 | reg;
+ buf[1] = val >> 8;
+ m_spi->Write(buf, 2);
+}
+
+/**
+ * This function resets (zeros) the accumulated (integrated) angle estimates for
+ *the xgyro, ygyro, and zgyro outputs.
+ **/
+void ADIS16448_IMU::Reset() {
+ std::scoped_lock sync(m_mutex);
+ m_integ_gyro_angle_x = 0.0;
+ m_integ_gyro_angle_y = 0.0;
+ m_integ_gyro_angle_z = 0.0;
+}
+
+void ADIS16448_IMU::Close() {
+ if (m_thread_active) {
+ m_thread_active = false;
+ if (m_acquire_task.joinable()) {
+ m_acquire_task.join();
+ }
+ }
+ if (m_spi != nullptr) {
+ if (m_auto_configured) {
+ m_spi->StopAuto();
+ }
+ delete m_spi;
+ m_auto_configured = false;
+ if (m_auto_interrupt != nullptr) {
+ delete m_auto_interrupt;
+ m_auto_interrupt = nullptr;
+ }
+ m_spi = nullptr;
+ }
+ delete[] m_offset_buffer;
+}
+
+ADIS16448_IMU::~ADIS16448_IMU() {
+ Close();
+}
+
+void ADIS16448_IMU::Acquire() {
+ // Set data packet length
+ const int dataset_len = 29; // 18 data points + timestamp
+
+ const int BUFFER_SIZE = 4000;
+
+ // This buffer can contain many datasets
+ uint32_t buffer[BUFFER_SIZE];
+ int data_count = 0;
+ int data_remainder = 0;
+ int data_to_read = 0;
+ int bufferAvgIndex = 0;
+ uint32_t previous_timestamp = 0;
+ double gyro_rate_x = 0.0;
+ double gyro_rate_y = 0.0;
+ double gyro_rate_z = 0.0;
+ double accel_x = 0.0;
+ double accel_y = 0.0;
+ double accel_z = 0.0;
+ double mag_x = 0.0;
+ double mag_y = 0.0;
+ double mag_z = 0.0;
+ double baro = 0.0;
+ double temp = 0.0;
+ double gyro_rate_x_si = 0.0;
+ double gyro_rate_y_si = 0.0;
+ // double gyro_rate_z_si = 0.0;
+ double accel_x_si = 0.0;
+ double accel_y_si = 0.0;
+ double accel_z_si = 0.0;
+ double compAngleX = 0.0;
+ double compAngleY = 0.0;
+ double accelAngleX = 0.0;
+ double accelAngleY = 0.0;
+
+ while (true) {
+ // Sleep loop for 10ms (wait for data)
+ Wait(10_ms);
+
+ if (m_thread_active) {
+ data_count = m_spi->ReadAutoReceivedData(
+ buffer, 0,
+ 0_s); // Read number of bytes currently stored in the buffer
+ data_remainder =
+ data_count % dataset_len; // Check if frame is incomplete
+ data_to_read = data_count -
+ data_remainder; // Remove incomplete data from read count
+ /* Want to cap the data to read in a single read at the buffer size */
+ if (data_to_read > BUFFER_SIZE) {
+ REPORT_WARNING(
+ "ADIS16448 data processing thread overrun has occurred!");
+ data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+ }
+ m_spi->ReadAutoReceivedData(buffer, data_to_read,
+ 0_s); // Read data from DMA buffer
+
+ // Could be multiple data sets in the buffer. Handle each one.
+ for (int i = 0; i < data_to_read; i += dataset_len) {
+ // Calculate CRC-16 on each data packet
+ uint16_t calc_crc = 0xFFFF; // Starting word
+ uint8_t byte = 0;
+ uint16_t imu_crc = 0;
+ for (int k = 5; k < 27;
+ k += 2) // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP
+ // (Ignore Status & CRC)
+ {
+ byte = BuffToUByte(&buffer[i + k + 1]); // Process LSB
+ calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
+ byte = BuffToUByte(&buffer[i + k]); // Process MSB
+ calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
+ }
+ calc_crc = ~calc_crc; // Complement
+ calc_crc = static_cast<uint16_t>((calc_crc << 8) |
+ (calc_crc >> 8)); // Flip LSB & MSB
+ imu_crc =
+ BuffToUShort(&buffer[i + 27]); // Extract DUT CRC from data buffer
+
+ // Compare calculated vs read CRC. Don't update outputs or dt if CRC-16
+ // is bad
+ if (calc_crc == imu_crc) {
+ // Timestamp is at buffer[i]
+ m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
+ // Split array and scale data
+ gyro_rate_x = BuffToShort(&buffer[i + 5]) * 0.04;
+ gyro_rate_y = BuffToShort(&buffer[i + 7]) * 0.04;
+ gyro_rate_z = BuffToShort(&buffer[i + 9]) * 0.04;
+ accel_x = BuffToShort(&buffer[i + 11]) * 0.833;
+ accel_y = BuffToShort(&buffer[i + 13]) * 0.833;
+ accel_z = BuffToShort(&buffer[i + 15]) * 0.833;
+ mag_x = BuffToShort(&buffer[i + 17]) * 0.1429;
+ mag_y = BuffToShort(&buffer[i + 19]) * 0.1429;
+ mag_z = BuffToShort(&buffer[i + 21]) * 0.1429;
+ baro = BuffToShort(&buffer[i + 23]) * 0.02;
+ temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0;
+
+ // Convert scaled sensor data to SI units
+ gyro_rate_x_si = gyro_rate_x * deg_to_rad;
+ gyro_rate_y_si = gyro_rate_y * deg_to_rad;
+ // gyro_rate_z_si = gyro_rate_z * deg_to_rad;
+ accel_x_si = accel_x * grav;
+ accel_y_si = accel_y * grav;
+ accel_z_si = accel_z * grav;
+ // Store timestamp for next iteration
+ previous_timestamp = buffer[i];
+ // Calculate alpha for use with the complementary filter
+ m_alpha = m_tau / (m_tau + m_dt);
+ // Calculate complementary filter
+ if (m_first_run) {
+ accelAngleX = atan2f(
+ -accel_x_si,
+ sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+ accelAngleY =
+ atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
+ (-accel_z_si * -accel_z_si)));
+ compAngleX = accelAngleX;
+ compAngleY = accelAngleY;
+ } else {
+ accelAngleX = atan2f(
+ -accel_x_si,
+ sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+ accelAngleY =
+ atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
+ (-accel_z_si * -accel_z_si)));
+ accelAngleX = FormatAccelRange(accelAngleX, -accel_z_si);
+ accelAngleY = FormatAccelRange(accelAngleY, -accel_z_si);
+ compAngleX =
+ CompFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
+ compAngleY =
+ CompFilterProcess(compAngleY, accelAngleY, -gyro_rate_x_si);
+ }
+
+ // Update global variables and state
+ {
+ std::scoped_lock sync(m_mutex);
+ // Ignore first, integrated sample
+ if (m_first_run) {
+ m_integ_gyro_angle_x = 0.0;
+ m_integ_gyro_angle_y = 0.0;
+ m_integ_gyro_angle_z = 0.0;
+ } else {
+ // Accumulate gyro for offset calibration
+ // Add most recent sample data to buffer
+ bufferAvgIndex = m_accum_count % m_avg_size;
+ m_offset_buffer[bufferAvgIndex] =
+ OffsetData{gyro_rate_x, gyro_rate_y, gyro_rate_z};
+ // Increment counter
+ m_accum_count++;
+ }
+ // Don't post accumulated data to the global variables until an
+ // initial gyro offset has been calculated
+ if (!m_start_up_mode) {
+ m_gyro_rate_x = gyro_rate_x;
+ m_gyro_rate_y = gyro_rate_y;
+ m_gyro_rate_z = gyro_rate_z;
+ m_accel_x = accel_x;
+ m_accel_y = accel_y;
+ m_accel_z = accel_z;
+ m_mag_x = mag_x;
+ m_mag_y = mag_y;
+ m_mag_z = mag_z;
+ m_baro = baro;
+ m_temp = temp;
+ m_compAngleX = compAngleX * rad_to_deg;
+ m_compAngleY = compAngleY * rad_to_deg;
+ m_accelAngleX = accelAngleX * rad_to_deg;
+ m_accelAngleY = accelAngleY * rad_to_deg;
+ // Accumulate gyro for angle integration and publish to global
+ // variables
+ m_integ_gyro_angle_x +=
+ (gyro_rate_x - m_gyro_rate_offset_x) * m_dt;
+ m_integ_gyro_angle_y +=
+ (gyro_rate_y - m_gyro_rate_offset_y) * m_dt;
+ m_integ_gyro_angle_z +=
+ (gyro_rate_z - m_gyro_rate_offset_z) * m_dt;
+ }
+ }
+ m_first_run = false;
+ }
+ }
+ } else {
+ m_thread_idle = true;
+ data_count = 0;
+ data_remainder = 0;
+ data_to_read = 0;
+ previous_timestamp = 0.0;
+ gyro_rate_x = 0.0;
+ gyro_rate_y = 0.0;
+ gyro_rate_z = 0.0;
+ accel_x = 0.0;
+ accel_y = 0.0;
+ accel_z = 0.0;
+ mag_x = 0.0;
+ mag_y = 0.0;
+ mag_z = 0.0;
+ baro = 0.0;
+ temp = 0.0;
+ gyro_rate_x_si = 0.0;
+ gyro_rate_y_si = 0.0;
+ // gyro_rate_z_si = 0.0;
+ accel_x_si = 0.0;
+ accel_y_si = 0.0;
+ accel_z_si = 0.0;
+ compAngleX = 0.0;
+ compAngleY = 0.0;
+ accelAngleX = 0.0;
+ accelAngleY = 0.0;
+ }
+ }
+}
+
+/* Complementary filter functions */
+double ADIS16448_IMU::FormatFastConverge(double compAngle, double accAngle) {
+ if (compAngle > accAngle + wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ } else if (accAngle > compAngle + wpi::numbers::pi) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16448_IMU::FormatRange0to2PI(double compAngle) {
+ while (compAngle >= 2 * wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ while (compAngle < 0.0) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16448_IMU::FormatAccelRange(double accelAngle, double accelZ) {
+ if (accelZ < 0.0) {
+ accelAngle = wpi::numbers::pi - accelAngle;
+ } else if (accelZ > 0.0 && accelAngle < 0.0) {
+ accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
+ }
+ return accelAngle;
+}
+
+double ADIS16448_IMU::CompFilterProcess(double compAngle, double accelAngle,
+ double omega) {
+ compAngle = FormatFastConverge(compAngle, accelAngle);
+ compAngle =
+ m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+ compAngle = FormatRange0to2PI(compAngle);
+ if (compAngle > wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+int ADIS16448_IMU::ConfigDecRate(uint16_t DecimationSetting) {
+ uint16_t writeValue = DecimationSetting;
+ uint16_t readbackValue;
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ return 2;
+ }
+
+ /* Check max */
+ if (DecimationSetting > 9) {
+ REPORT_ERROR("Attemted to write an invalid decimation value. Capping at 9");
+ DecimationSetting = 9;
+ }
+
+ /* Shift decimation setting to correct position and select internal sync */
+ writeValue = (DecimationSetting << 8) | 0x1;
+
+ /* Apply to IMU */
+ WriteRegister(SMPL_PRD, writeValue);
+
+ /* Perform read back to verify write */
+ readbackValue = ReadRegister(SMPL_PRD);
+
+ /* Throw error for invalid write */
+ if (readbackValue != writeValue) {
+ REPORT_ERROR("ADIS16448 SMPL_PRD write failed.");
+ }
+
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ return 2;
+ }
+ return 0;
+}
+
+units::degree_t ADIS16448_IMU::GetAngle() const {
+ switch (m_yaw_axis) {
+ case kX:
+ return GetGyroAngleX();
+ case kY:
+ return GetGyroAngleY();
+ case kZ:
+ return GetGyroAngleZ();
+ default:
+ return 0_deg;
+ }
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetRate() const {
+ switch (m_yaw_axis) {
+ case kX:
+ return GetGyroRateX();
+ case kY:
+ return GetGyroRateY();
+ case kZ:
+ return GetGyroRateZ();
+ default:
+ return 0_deg_per_s;
+ }
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleX() const {
+ if (m_simGyroAngleX) {
+ return units::degree_t{m_simGyroAngleX.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_integ_gyro_angle_x};
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleY() const {
+ if (m_simGyroAngleY) {
+ return units::degree_t{m_simGyroAngleY.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_integ_gyro_angle_y};
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleZ() const {
+ if (m_simGyroAngleZ) {
+ return units::degree_t{m_simGyroAngleZ.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_integ_gyro_angle_z};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateX() const {
+ 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};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateY() const {
+ 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};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateZ() const {
+ 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};
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelX() const {
+ if (m_simAccelX) {
+ return units::meters_per_second_squared_t{m_simAccelX.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return m_accel_x * 9.81_mps_sq;
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelY() const {
+ if (m_simAccelY) {
+ return units::meters_per_second_squared_t{m_simAccelY.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return m_accel_y * 9.81_mps_sq;
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelZ() const {
+ if (m_simAccelZ) {
+ return units::meters_per_second_squared_t{m_simAccelZ.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return m_accel_z * 9.81_mps_sq;
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldX() const {
+ std::scoped_lock sync(m_mutex);
+ return units::gauss_t{m_mag_x * 1e-3};
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldY() const {
+ std::scoped_lock sync(m_mutex);
+ return units::gauss_t{m_mag_y * 1e-3};
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldZ() const {
+ std::scoped_lock sync(m_mutex);
+ return units::gauss_t{m_mag_z * 1e-3};
+}
+
+units::degree_t ADIS16448_IMU::GetXComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_compAngleX};
+}
+
+units::degree_t ADIS16448_IMU::GetYComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_compAngleY};
+}
+
+units::degree_t ADIS16448_IMU::GetXFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_accelAngleX};
+}
+
+units::degree_t ADIS16448_IMU::GetYFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_accelAngleY};
+}
+
+units::pounds_per_square_inch_t ADIS16448_IMU::GetBarometricPressure() const {
+ std::scoped_lock sync(m_mutex);
+ return units::mbar_t{m_baro};
+}
+
+units::celsius_t ADIS16448_IMU::GetTemperature() const {
+ std::scoped_lock sync(m_mutex);
+ return units::celsius_t{m_temp};
+}
+
+ADIS16448_IMU::IMUAxis ADIS16448_IMU::GetYawAxis() const {
+ return m_yaw_axis;
+}
+
+int ADIS16448_IMU::SetYawAxis(IMUAxis yaw_axis) {
+ if (m_yaw_axis == yaw_axis) {
+ return 1;
+ } else {
+ m_yaw_axis = yaw_axis;
+ Reset();
+ return 0;
+ }
+}
+
+int ADIS16448_IMU::GetPort() const {
+ return m_spi_port;
+}
+
+/**
+ * @brief Builds a Sendable object to push IMU data to the driver station.
+ *
+ * This function pushes the most recent angle estimates for all axes to the
+ *driver station.
+ **/
+void ADIS16448_IMU::InitSendable(nt::NTSendableBuilder& builder) {
+ builder.SetSmartDashboardType("ADIS16448 IMU");
+ auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
+ builder.SetUpdateTable([=]() {
+ nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
new file mode 100644
index 0000000..f48d827
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
@@ -0,0 +1,809 @@
+// 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.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. 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. */
+/* */
+/* Juan Chong - frcsupport@analog.com */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ADIS16470_IMU.h"
+
+#include <frc/DigitalInput.h>
+#include <frc/DigitalSource.h>
+#include <frc/DriverStation.h>
+#include <frc/Timer.h>
+
+#include <cmath>
+#include <string>
+
+#include <hal/HAL.h>
+#include <networktables/NTSendableBuilder.h>
+#include <wpi/numbers>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+
+/* Helpful conversion functions */
+static inline int32_t ToInt(const uint32_t* buf) {
+ return static_cast<int32_t>((buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) |
+ buf[3]);
+}
+
+static inline int16_t BuffToShort(const uint32_t* buf) {
+ return (static_cast<int16_t>(buf[0]) << 8) | buf[1];
+}
+
+static inline uint16_t ToUShort(const uint8_t* buf) {
+ return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
+}
+
+using namespace frc;
+
+namespace {
+template <typename S, typename... Args>
+inline void ADISReportError(int32_t status, const char* file, int line,
+ const char* function, const S& format,
+ Args&&... args) {
+ frc::ReportErrorV(status, file, line, function, format,
+ fmt::make_args_checked<Args...>(format, args...));
+}
+} // namespace
+
+#define REPORT_WARNING(msg) \
+ ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
+#define REPORT_ERROR(msg) \
+ ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
+
+/**
+ * Constructor.
+ */
+ADIS16470_IMU::ADIS16470_IMU()
+ : ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {}
+
+ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
+ CalibrationTime cal_time)
+ : m_yaw_axis(yaw_axis),
+ m_spi_port(port),
+ m_calibration_time(static_cast<uint16_t>(cal_time)),
+ m_simDevice("Gyro:ADIS16470", port) {
+ if (m_simDevice) {
+ m_simGyroAngleX =
+ m_simDevice.CreateDouble("gyro_angle_x", hal::SimDevice::kInput, 0.0);
+ m_simGyroAngleY =
+ m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0);
+ m_simGyroAngleZ =
+ m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0);
+ m_simGyroRateX =
+ m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
+ m_simGyroRateY =
+ m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
+ m_simGyroRateZ =
+ m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
+ m_simAccelX =
+ m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
+ m_simAccelY =
+ m_simDevice.CreateDouble("accel_y", hal::SimDevice::kInput, 0.0);
+ m_simAccelZ =
+ m_simDevice.CreateDouble("accel_z", hal::SimDevice::kInput, 0.0);
+ }
+
+ if (!m_simDevice) {
+ // Force the IMU reset pin to toggle on startup (doesn't require DS enable)
+ // Relies on the RIO hardware by default configuring an output as low
+ // and configuring an input as high Z. The 10k pull-up resistor internal to
+ // the IMU then forces the reset line high for normal operation.
+ DigitalOutput* m_reset_out =
+ new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
+ Wait(10_ms); // Wait 10ms
+ delete m_reset_out;
+ new DigitalInput(27); // Set SPI CS2 (IMU RST) high
+ Wait(500_ms); // Wait 500ms for reset to complete
+
+ // Configure standard SPI
+ if (!SwitchToStandardSPI()) {
+ return;
+ }
+
+ // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1)
+ // = 400Hz)
+ WriteRegister(DEC_RATE, 0x0004);
+ // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation
+ // and PoP
+ WriteRegister(MSC_CTRL, 0x0001);
+ // Configure IMU internal Bartlett filter
+ WriteRegister(FILT_CTRL, 0x0000);
+ // Configure continuous bias calibration time based on user setting
+ WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
+
+ // Notify DS that IMU calibration delay is active
+ REPORT_WARNING(
+ "ADIS16470 IMU Detected. Starting initial calibration delay.");
+
+ // Wait for samples to accumulate internal to the IMU (110% of user-defined
+ // time)
+ Wait(units::second_t{pow(2, m_calibration_time) / 2000 * 64 * 1.1});
+
+ // Write offset calibration command to IMU
+ WriteRegister(GLOB_CMD, 0x0001);
+
+ // Configure and enable auto SPI
+ if (!SwitchToAutoSPI()) {
+ return;
+ }
+
+ // Let the user know the IMU was initiallized successfully
+ REPORT_WARNING("ADIS16470 IMU Successfully Initialized!");
+
+ // Drive SPI CS3 (IMU ready LED) low (active low)
+ new DigitalOutput(28);
+ }
+
+ // Report usage and post data to DS
+ HAL_Report(HALUsageReporting::kResourceType_ADIS16470, 0);
+
+ wpi::SendableRegistry::AddLW(this, "ADIS16470", port);
+}
+
+/**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in standard SPI mode.
+ *
+ * This function switches the active SPI port to standard SPI and is used
+ *primarily when exiting auto SPI. Exiting auto SPI is required to read or write
+ *using SPI since the auto SPI configuration, once active, locks the SPI message
+ *being transacted. This function also verifies that the SPI port is operating
+ *in standard SPI mode by reading back the IMU product ID.
+ **/
+bool ADIS16470_IMU::SwitchToStandardSPI() {
+ // Check to see whether the acquire thread is active. If so, wait for it to
+ // stop producing data.
+ if (m_thread_active) {
+ m_thread_active = false;
+ while (!m_thread_idle) {
+ Wait(10_ms);
+ }
+ // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
+ if (m_spi != nullptr && m_auto_configured) {
+ m_spi->StopAuto();
+ // We need to get rid of all the garbage left in the auto SPI buffer after
+ // stopping it. Sometimes data magically reappears, so we have to check
+ // the buffer size a couple of times
+ // to be sure we got it all. Yuck.
+ uint32_t trashBuffer[200];
+ Wait(100_ms);
+ int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ while (data_count > 0) {
+ /* Receive data, max of 200 words at a time (prevent potential segfault)
+ */
+ m_spi->ReadAutoReceivedData(trashBuffer, std::min(data_count, 200),
+ 0_s);
+ /*Get the reamining data count */
+ data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ }
+ }
+ }
+ // There doesn't seem to be a SPI port active. Let's try to set one up
+ if (m_spi == nullptr) {
+ m_spi = new SPI(m_spi_port);
+ m_spi->SetClockRate(2000000);
+ m_spi->SetMSBFirst();
+ m_spi->SetSampleDataOnTrailingEdge();
+ m_spi->SetClockActiveLow();
+ m_spi->SetChipSelectActiveLow();
+ ReadRegister(PROD_ID); // Dummy read
+
+ // Validate the product ID
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16982 && prod_id != 16470) {
+ REPORT_ERROR("Could not find ADIS16470!");
+ Close();
+ return false;
+ }
+ return true;
+ } else {
+ // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
+ // product ID.
+ ReadRegister(PROD_ID); // Dummy read
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16982 && prod_id != 16470) {
+ REPORT_ERROR("Could not find ADIS16470!");
+ Close();
+ return false;
+ } else {
+ return true;
+ }
+ }
+}
+
+/**
+ * @brief Switches to auto SPI operation. Primarily used when exiting standard
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in auto SPI mode.
+ *
+ * This function switches the active SPI port to auto SPI and is used primarily
+ *when exiting standard SPI. Auto SPI is required to asynchronously read data
+ *over SPI as it utilizes special FPGA hardware to react to an external data
+ *ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
+ *can be read by the CPU asynchronously. Standard SPI transactions are
+ * impossible on the selected SPI port once auto SPI is enabled. The stall
+ *settings, GPIO interrupt pin, and data packet settings used in this function
+ *are hard-coded to work only with the ADIS16470 IMU.
+ **/
+bool ADIS16470_IMU::SwitchToAutoSPI() {
+ // No SPI port has been set up. Go set one up first.
+ if (m_spi == nullptr) {
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to start/restart auto SPI");
+ return false;
+ }
+ }
+ // Only set up the interrupt if needed.
+ if (m_auto_interrupt == nullptr) {
+ m_auto_interrupt = new DigitalInput(26);
+ }
+ // The auto SPI controller gets angry if you try to set up two instances on
+ // one bus.
+ if (!m_auto_configured) {
+ m_spi->InitAuto(8200);
+ 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;
+ }
+ // Configure auto stall time
+ m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
+ // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
+ // activated) DR High = Data good (data capture should be triggered on the
+ // rising edge)
+ m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
+ // Check to see if the acquire thread is running. If not, kick one off.
+ if (!m_thread_idle) {
+ m_first_run = true;
+ m_thread_active = true;
+ m_acquire_task = std::thread(&ADIS16470_IMU::Acquire, this);
+ } else {
+ m_first_run = true;
+ m_thread_active = true;
+ }
+ // Looks like the thread didn't start for some reason. Abort.
+ /*
+ if(!m_thread_idle) {
+ REPORT_ERROR("Failed to start/restart the acquire() thread.");
+ Close();
+ return false;
+ }
+ */
+ return true;
+}
+
+/**
+ * @brief Switches the active SPI port to standard SPI mode, writes a new value
+ *to the NULL_CNFG register in the IMU, and re-enables auto SPI.
+ *
+ * @param new_cal_time Calibration time to be set.
+ *
+ * @return An int indicating the success or failure of writing the new NULL_CNFG
+ *setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
+ *Failure
+ *
+ * This function enters standard SPI mode, writes a new NULL_CNFG setting to the
+ *IMU, and re-enters auto SPI mode. This function does not include a blocking
+ *sleep, so the user must keep track of the elapsed offset calibration time
+ * themselves. After waiting the configured calibration time, the Calibrate()
+ *function should be called to activate the new offset calibration.
+ **/
+int ADIS16470_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
+ if (m_calibration_time == static_cast<uint16_t>(new_cal_time)) {
+ return 1;
+ }
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ return 2;
+ }
+ m_calibration_time = static_cast<uint16_t>(new_cal_time);
+ WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ return 2;
+ }
+ 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;
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ return 2;
+ }
+ if (m_reg > 1999) {
+ REPORT_ERROR("Attempted to write an invalid decimation value.");
+ m_reg = 1999;
+ }
+ m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
+ WriteRegister(DEC_RATE, m_reg);
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ return 2;
+ }
+ return 0;
+}
+
+/**
+ * @brief Switches the active SPI port to standard SPI mode, writes the command
+ *to activate the new null configuration, and re-enables auto SPI.
+ *
+ * This function enters standard SPI mode, writes 0x0001 to the GLOB_CMD
+ *register (thus making the new offset active in the IMU), and re-enters auto
+ *SPI mode. This function does not include a blocking sleep, so the user must
+ *keep track of the elapsed offset calibration time themselves.
+ **/
+void ADIS16470_IMU::Calibrate() {
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ }
+ WriteRegister(GLOB_CMD, 0x0001);
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ }
+}
+
+/**
+ * @brief Reads the contents of a specified register location over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @return An unsigned, 16-bit number representing the contents of the requested
+ *register location.
+ *
+ * This function reads the contents of an 8-bit register location by
+ *transmitting the register location byte along with a null (0x00) byte using
+ *the standard WPILib API. The response (two bytes) is read back using the
+ *WPILib API and joined using a helper function. This function assumes the
+ *controller is set to standard SPI mode.
+ **/
+uint16_t ADIS16470_IMU::ReadRegister(uint8_t reg) {
+ uint8_t buf[2];
+ buf[0] = reg & 0x7f;
+ buf[1] = 0;
+
+ m_spi->Write(buf, 2);
+ m_spi->Read(false, buf, 2);
+
+ return ToUShort(buf);
+}
+
+/**
+ * @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
+ *locations over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @param val An unsigned, 16-bit value to be written to the specified register
+ *location.
+ *
+ * This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
+ *via SPI. The upper and lower bytes that make up the 16-bit value are split
+ *into two unsined, 8-bit values and written to the upper and lower addresses of
+ *the specified register value. Only the lower (base) address must be specified.
+ *This function assumes the controller is set to standard SPI mode.
+ **/
+void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) {
+ uint8_t buf[2];
+ buf[0] = 0x80 | reg;
+ buf[1] = val & 0xff;
+ m_spi->Write(buf, 2);
+ buf[0] = 0x81 | reg;
+ buf[1] = val >> 8;
+ m_spi->Write(buf, 2);
+}
+
+/**
+ * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
+ *
+ * This function resets (zeros) the accumulated (integrated) angle estimates for
+ *the xgyro, ygyro, and zgyro outputs.
+ **/
+void ADIS16470_IMU::Reset() {
+ std::scoped_lock sync(m_mutex);
+ m_integ_angle = 0.0;
+}
+
+void ADIS16470_IMU::Close() {
+ if (m_thread_active) {
+ m_thread_active = false;
+ if (m_acquire_task.joinable()) {
+ m_acquire_task.join();
+ }
+ }
+ if (m_spi != nullptr) {
+ if (m_auto_configured) {
+ m_spi->StopAuto();
+ }
+ delete m_spi;
+ m_auto_configured = false;
+ if (m_auto_interrupt != nullptr) {
+ delete m_auto_interrupt;
+ m_auto_interrupt = nullptr;
+ }
+ m_spi = nullptr;
+ }
+}
+
+ADIS16470_IMU::~ADIS16470_IMU() {
+ Close();
+}
+
+/**
+ * @brief Main acquisition loop. Typically called asynchronously and free-wheels
+ *while the robot code is active.
+ *
+ * This is the main acquisiton loop for the IMU. During each iteration, data
+ *read using auto SPI is extracted from the FPGA FIFO, split, scaled, and
+ *integrated. Each X, Y, and Z value is 32-bits split across 4 indices (bytes)
+ *in the buffer. Auto SPI puts one byte in each index location. Each index is
+ *32-bits wide because the timestamp is an unsigned 32-bit int. The timestamp is
+ *always located at the beginning of the frame. Two indices (request_1 and
+ *request_2 below) are always invalid (garbage) and can be disregarded.
+ *
+ * Data order: [timestamp, request_1, request_2, x_1, x_2, x_3, x_4, y_1, y_2,
+ *y_3, y_4, z_1, z_2, z_3, z_4] x = delta x (32-bit x_1 = highest bit) y = delta
+ *y (32-bit y_1 = highest bit) z = delta z (32-bit z_1 = highest bit)
+ *
+ * Complementary filter code was borrowed from
+ *https://github.com/tcleg/Six_Axis_Complementary_Filter
+ **/
+void ADIS16470_IMU::Acquire() {
+ // Set data packet length
+ const int dataset_len = 19; // 18 data points + timestamp
+
+ /* Fixed buffer size */
+ const int BUFFER_SIZE = 4000;
+
+ // This buffer can contain many datasets
+ uint32_t buffer[BUFFER_SIZE];
+ int data_count = 0;
+ int data_remainder = 0;
+ int data_to_read = 0;
+ uint32_t previous_timestamp = 0;
+ double delta_angle = 0.0;
+ double gyro_rate_x = 0.0;
+ double gyro_rate_y = 0.0;
+ double gyro_rate_z = 0.0;
+ double accel_x = 0.0;
+ double accel_y = 0.0;
+ double accel_z = 0.0;
+ double gyro_rate_x_si = 0.0;
+ double gyro_rate_y_si = 0.0;
+ // double gyro_rate_z_si = 0.0;
+ double accel_x_si = 0.0;
+ double accel_y_si = 0.0;
+ double accel_z_si = 0.0;
+ double compAngleX = 0.0;
+ double compAngleY = 0.0;
+ double accelAngleX = 0.0;
+ double accelAngleY = 0.0;
+
+ while (true) {
+ // Sleep loop for 10ms (wait for data)
+ Wait(10_ms);
+
+ if (m_thread_active) {
+ m_thread_idle = false;
+
+ data_count = m_spi->ReadAutoReceivedData(
+ buffer, 0,
+ 0_s); // Read number of bytes currently stored in the buffer
+ data_remainder =
+ data_count % dataset_len; // Check if frame is incomplete. Add 1
+ // because of timestamp
+ data_to_read = data_count -
+ data_remainder; // Remove incomplete data from read count
+ /* Want to cap the data to read in a single read at the buffer size */
+ if (data_to_read > BUFFER_SIZE) {
+ REPORT_WARNING(
+ "ADIS16470 data processing thread overrun has occurred!");
+ data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+ }
+ m_spi->ReadAutoReceivedData(
+ buffer, data_to_read,
+ 0_s); // Read data from DMA buffer (only complete sets)
+
+ // Could be multiple data sets in the buffer. Handle each one.
+ for (int i = 0; i < data_to_read; i += dataset_len) {
+ // Timestamp is at buffer[i]
+ 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);
+
+ // Convert scaled sensor data to SI units
+ gyro_rate_x_si = gyro_rate_x * deg_to_rad;
+ gyro_rate_y_si = gyro_rate_y * deg_to_rad;
+ // gyro_rate_z_si = gyro_rate_z * deg_to_rad;
+ accel_x_si = accel_x * grav;
+ accel_y_si = accel_y * grav;
+ accel_z_si = accel_z * grav;
+
+ // Store timestamp for next iteration
+ previous_timestamp = buffer[i];
+
+ m_alpha = m_tau / (m_tau + m_dt);
+
+ if (m_first_run) {
+ accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
+ (accel_z_si * accel_z_si)));
+ accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
+ (accel_z_si * accel_z_si)));
+ compAngleX = accelAngleX;
+ compAngleY = accelAngleY;
+ } else {
+ // Process X angle
+ accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
+ (accel_z_si * accel_z_si)));
+ accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
+ (accel_z_si * accel_z_si)));
+ accelAngleX = FormatAccelRange(accelAngleX, accel_z_si);
+ accelAngleY = FormatAccelRange(accelAngleY, accel_z_si);
+ compAngleX =
+ CompFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
+ compAngleY =
+ CompFilterProcess(compAngleY, accelAngleY, gyro_rate_x_si);
+ }
+
+ {
+ std::scoped_lock sync(m_mutex);
+ /* Push data to global variables */
+ 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;
+ } else {
+ m_integ_angle += delta_angle;
+ }
+ m_gyro_rate_x = gyro_rate_x;
+ m_gyro_rate_y = gyro_rate_y;
+ m_gyro_rate_z = gyro_rate_z;
+ m_accel_x = accel_x;
+ m_accel_y = accel_y;
+ m_accel_z = accel_z;
+ m_compAngleX = compAngleX * rad_to_deg;
+ m_compAngleY = compAngleY * rad_to_deg;
+ m_accelAngleX = accelAngleX * rad_to_deg;
+ m_accelAngleY = accelAngleY * rad_to_deg;
+ }
+ m_first_run = false;
+ }
+ } else {
+ m_thread_idle = true;
+ data_count = 0;
+ data_remainder = 0;
+ data_to_read = 0;
+ previous_timestamp = 0.0;
+ delta_angle = 0.0;
+ gyro_rate_x = 0.0;
+ gyro_rate_y = 0.0;
+ gyro_rate_z = 0.0;
+ accel_x = 0.0;
+ accel_y = 0.0;
+ accel_z = 0.0;
+ gyro_rate_x_si = 0.0;
+ gyro_rate_y_si = 0.0;
+ // gyro_rate_z_si = 0.0;
+ accel_x_si = 0.0;
+ accel_y_si = 0.0;
+ accel_z_si = 0.0;
+ compAngleX = 0.0;
+ compAngleY = 0.0;
+ accelAngleX = 0.0;
+ accelAngleY = 0.0;
+ }
+ }
+}
+
+/* Complementary filter functions */
+double ADIS16470_IMU::FormatFastConverge(double compAngle, double accAngle) {
+ if (compAngle > accAngle + wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ } else if (accAngle > compAngle + wpi::numbers::pi) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16470_IMU::FormatRange0to2PI(double compAngle) {
+ while (compAngle >= 2 * wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ while (compAngle < 0.0) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16470_IMU::FormatAccelRange(double accelAngle, double accelZ) {
+ if (accelZ < 0.0) {
+ accelAngle = wpi::numbers::pi - accelAngle;
+ } else if (accelZ > 0.0 && accelAngle < 0.0) {
+ accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
+ }
+ return accelAngle;
+}
+
+double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
+ double omega) {
+ compAngle = FormatFastConverge(compAngle, accelAngle);
+ compAngle =
+ m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+ compAngle = FormatRange0to2PI(compAngle);
+ if (compAngle > wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+units::degree_t ADIS16470_IMU::GetAngle() const {
+ switch (m_yaw_axis) {
+ case kX:
+ if (m_simGyroAngleX) {
+ return units::degree_t{m_simGyroAngleX.Get()};
+ }
+ break;
+ case kY:
+ if (m_simGyroAngleY) {
+ return units::degree_t{m_simGyroAngleY.Get()};
+ }
+ break;
+ case kZ:
+ if (m_simGyroAngleZ) {
+ return units::degree_t{m_simGyroAngleZ.Get()};
+ }
+ break;
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_integ_angle};
+}
+
+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::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const {
+ if (m_simAccelX) {
+ return units::meters_per_second_squared_t{m_simAccelX.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::meters_per_second_squared_t{m_accel_x};
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelY() const {
+ if (m_simAccelY) {
+ return units::meters_per_second_squared_t{m_simAccelY.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::meters_per_second_squared_t{m_accel_y};
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelZ() const {
+ if (m_simAccelZ) {
+ return units::meters_per_second_squared_t{m_simAccelZ.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::meters_per_second_squared_t{m_accel_z};
+}
+
+units::degree_t ADIS16470_IMU::GetXComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_compAngleX};
+}
+
+units::degree_t ADIS16470_IMU::GetYComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_compAngleY};
+}
+
+units::degree_t ADIS16470_IMU::GetXFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_accelAngleX};
+}
+
+units::degree_t ADIS16470_IMU::GetYFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_accelAngleY};
+}
+
+ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const {
+ 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;
+}
+
+int ADIS16470_IMU::GetPort() const {
+ return m_spi_port;
+}
+
+/**
+ * @brief Builds a Sendable object to push IMU data to the driver station.
+ *
+ * This function pushes the most recent angle estimates for all axes to the
+ *driver station.
+ **/
+void ADIS16470_IMU::InitSendable(nt::NTSendableBuilder& builder) {
+ builder.SetSmartDashboardType("ADIS16470 IMU");
+ auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
+ builder.SetUpdateTable([=]() {
+ nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp
index 0302acb..d994b14 100644
--- a/wpilibc/src/main/native/cpp/Compressor.cpp
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -19,7 +19,7 @@
throw FRC_MakeError(err::ResourceAlreadyAllocated, "{}", module);
}
- SetClosedLoopControl(true);
+ m_module->EnableCompressorDigital();
HAL_Report(HALUsageReporting::kResourceType_Compressor, module + 1);
wpi::SendableRegistry::AddLW(this, "Compressor", module);
@@ -33,11 +33,11 @@
}
void Compressor::Start() {
- SetClosedLoopControl(true);
+ EnableDigital();
}
void Compressor::Stop() {
- SetClosedLoopControl(false);
+ Disable();
}
bool Compressor::Enabled() const {
@@ -48,24 +48,43 @@
return m_module->GetPressureSwitch();
}
-double Compressor::GetCompressorCurrent() const {
+units::ampere_t Compressor::GetCurrent() const {
return m_module->GetCompressorCurrent();
}
-void Compressor::SetClosedLoopControl(bool on) {
- m_module->SetClosedLoopControl(on);
+units::volt_t Compressor::GetAnalogVoltage() const {
+ return m_module->GetAnalogVoltage(0);
}
-bool Compressor::GetClosedLoopControl() const {
- return m_module->GetClosedLoopControl();
+units::pounds_per_square_inch_t Compressor::GetPressure() const {
+ return m_module->GetPressure(0);
+}
+
+void Compressor::Disable() {
+ m_module->DisableCompressor();
+}
+
+void Compressor::EnableDigital() {
+ m_module->EnableCompressorDigital();
+}
+
+void Compressor::EnableAnalog(units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ m_module->EnableCompressorAnalog(minPressure, maxPressure);
+}
+
+void Compressor::EnableHybrid(units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ m_module->EnableCompressorHybrid(minPressure, maxPressure);
+}
+
+CompressorConfigType Compressor::GetConfigType() const {
+ return m_module->GetCompressorConfigType();
}
void Compressor::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("Compressor");
builder.AddBooleanProperty(
- "Closed Loop Control", [=]() { return GetClosedLoopControl(); },
- [=](bool value) { SetClosedLoopControl(value); });
- builder.AddBooleanProperty(
"Enabled", [=] { return Enabled(); }, nullptr);
builder.AddBooleanProperty(
"Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr);
diff --git a/wpilibc/src/main/native/cpp/Debouncer.cpp b/wpilibc/src/main/native/cpp/Debouncer.cpp
deleted file mode 100644
index eb402cd..0000000
--- a/wpilibc/src/main/native/cpp/Debouncer.cpp
+++ /dev/null
@@ -1,37 +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/Debouncer.h"
-
-using namespace frc;
-
-Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
- : m_debounceTime(debounceTime), m_debounceType(type) {
- switch (type) {
- case DebounceType::kBoth: // fall-through
- case DebounceType::kRising:
- m_baseline = false;
- break;
- case DebounceType::kFalling:
- m_baseline = true;
- break;
- }
- m_timer.Start();
-}
-
-bool Debouncer::Calculate(bool input) {
- if (input == m_baseline) {
- m_timer.Reset();
- }
-
- if (m_timer.HasElapsed(m_debounceTime)) {
- if (m_debounceType == DebounceType::kBoth) {
- m_baseline = input;
- m_timer.Reset();
- }
- return input;
- } else {
- return m_baseline;
- }
-}
diff --git a/wpilibc/src/main/native/cpp/DigitalInput.cpp b/wpilibc/src/main/native/cpp/DigitalInput.cpp
index 7bce921..86f1c3a 100644
--- a/wpilibc/src/main/native/cpp/DigitalInput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -4,7 +4,6 @@
#include "frc/DigitalInput.h"
-#include <iostream>
#include <limits>
#include <hal/DIO.h>
diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
index 8e994e9..452f5cd 100644
--- a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
+++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -89,6 +89,14 @@
auto counter2 = m_counter->Get();
auto pos2 = m_dutyCycle->GetOutput();
if (counter == counter2 && pos == pos2) {
+ // map sensor range
+ if (pos < m_sensorMin) {
+ pos = m_sensorMin;
+ }
+ if (pos > m_sensorMax) {
+ pos = m_sensorMax;
+ }
+ pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
units::turn_t turns{counter + pos - m_positionOffset};
m_lastPosition = turns;
return turns;
@@ -102,6 +110,11 @@
return m_lastPosition;
}
+void DutyCycleEncoder::SetDutyCycleRange(double min, double max) {
+ m_sensorMin = std::clamp(min, 0.0, 1.0);
+ m_sensorMax = std::clamp(max, 0.0, 1.0);
+}
+
void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) {
m_distancePerRotation = distancePerRotation;
m_simDistancePerRotation.Set(distancePerRotation);
diff --git a/wpilibc/src/main/native/cpp/I2C.cpp b/wpilibc/src/main/native/cpp/I2C.cpp
index adf1c54..dcfb2b8 100644
--- a/wpilibc/src/main/native/cpp/I2C.cpp
+++ b/wpilibc/src/main/native/cpp/I2C.cpp
@@ -16,6 +16,14 @@
I2C::I2C(Port port, int deviceAddress)
: m_port(static_cast<HAL_I2CPort>(port)), m_deviceAddress(deviceAddress) {
int32_t status = 0;
+
+ if (port == I2C::Port::kOnboard) {
+ FRC_ReportError(warn::Warning, "{}",
+ "Onboard I2C port is subject to system lockups. See Known "
+ "Issues page for "
+ "details");
+ }
+
HAL_InitializeI2C(m_port, &status);
FRC_CheckErrorStatus(status, "Port {}", port);
diff --git a/wpilibc/src/main/native/cpp/MotorSafety.cpp b/wpilibc/src/main/native/cpp/MotorSafety.cpp
index 9cb28cd..388a887 100644
--- a/wpilibc/src/main/native/cpp/MotorSafety.cpp
+++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp
@@ -17,6 +17,15 @@
static wpi::SmallPtrSet<MotorSafety*, 32> instanceList;
static wpi::mutex listMutex;
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetMotorSafety() {
+ std::scoped_lock lock(listMutex);
+ instanceList.clear();
+}
+} // namespace frc::impl
+#endif
+
MotorSafety::MotorSafety() {
std::scoped_lock lock(listMutex);
instanceList.insert(this);
@@ -89,7 +98,15 @@
if (stopTime < Timer::GetFPGATimestamp()) {
FRC_ReportError(err::Timeout, "{}... Output not updated often enough",
GetDescription());
- StopMotor();
+
+ try {
+ StopMotor();
+ } catch (frc::RuntimeError& e) {
+ e.Report();
+ } catch (std::exception& e) {
+ FRC_ReportError(err::Error, "{} StopMotor threw unexpected exception: {}",
+ GetDescription(), e.what());
+ }
}
}
diff --git a/wpilibc/src/main/native/cpp/PneumaticHub.cpp b/wpilibc/src/main/native/cpp/PneumaticHub.cpp
index 8217e86..0f32e1e 100644
--- a/wpilibc/src/main/native/cpp/PneumaticHub.cpp
+++ b/wpilibc/src/main/native/cpp/PneumaticHub.cpp
@@ -4,6 +4,7 @@
#include "frc/PneumaticHub.h"
+#include <fmt/format.h>
#include <hal/REVPH.h>
#include <wpi/NullDeleter.h>
#include <wpi/StackTrace.h>
@@ -11,11 +12,27 @@
#include "frc/Compressor.h"
#include "frc/DoubleSolenoid.h"
#include "frc/Errors.h"
+#include "frc/RobotBase.h"
#include "frc/SensorUtil.h"
#include "frc/Solenoid.h"
+#include "frc/fmt/Units.h"
using namespace frc;
+/** 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};
+}
+
+/** 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};
+}
+
wpi::mutex PneumaticHub::m_handleLock;
std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<PneumaticHub::DataStore>>>
PneumaticHub::m_handleMap = nullptr;
@@ -39,6 +56,38 @@
m_moduleObject = PneumaticHub{handle, module};
m_moduleObject.m_dataStore =
std::shared_ptr<DataStore>{this, wpi::NullDeleter<DataStore>()};
+
+ auto version = m_moduleObject.GetVersion();
+
+ if (version.FirmwareMajor > 0 && RobotBase::IsReal()) {
+ // Write PH firmware version to roboRIO
+ std::FILE* file = nullptr;
+ file = std::fopen(
+ fmt::format("/tmp/frc_versions/REV_PH_{:0>2}_WPILib_Version.ini",
+ module)
+ .c_str(),
+ "w");
+ if (file != nullptr) {
+ std::fputs("[Version]\n", file);
+ std::fputs(fmt::format("model=REV PH\n").c_str(), file);
+ std::fputs(fmt::format("deviceID={:x}\n", (0x9052600 | module)).c_str(),
+ file);
+ std::fputs(fmt::format("currentVersion={}.{}.{}", version.FirmwareMajor,
+ version.FirmwareMinor, version.FirmwareFix)
+ .c_str(),
+ file);
+ std::fclose(file);
+ }
+ }
+
+ // Check PH firmware version
+ if (version.FirmwareMajor > 0 && version.FirmwareMajor < 22) {
+ throw FRC_MakeError(
+ err::AssertionFailure,
+ "The Pneumatic Hub has firmware version {}.{}.{}, and must be "
+ "updated to version 2022.0.0 or later using the REV Hardware Client",
+ version.FirmwareMajor, version.FirmwareMinor, version.FirmwareFix);
+ }
}
~DataStore() noexcept { HAL_FreeREVPH(m_moduleObject.m_handle); }
@@ -52,6 +101,7 @@
bool m_compressorReserved{false};
wpi::mutex m_reservedLock;
PneumaticHub m_moduleObject{HAL_kInvalidHandle, 0};
+ std::array<units::millisecond_t, 16> m_oneShotDurMs{0_ms};
};
PneumaticHub::PneumaticHub()
@@ -76,47 +126,103 @@
bool PneumaticHub::GetCompressor() const {
int32_t status = 0;
auto result = HAL_GetREVPHCompressor(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
-void PneumaticHub::SetClosedLoopControl(bool enabled) {
+void PneumaticHub::DisableCompressor() {
int32_t status = 0;
- HAL_SetREVPHClosedLoopControl(m_handle, enabled, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ HAL_SetREVPHClosedLoopControlDisabled(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
}
-bool PneumaticHub::GetClosedLoopControl() const {
+void PneumaticHub::EnableCompressorDigital() {
int32_t status = 0;
- auto result = HAL_GetREVPHClosedLoopControl(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
- return result;
+ HAL_SetREVPHClosedLoopControlDigital(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticHub::EnableCompressorAnalog(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ if (minPressure >= maxPressure) {
+ throw FRC_MakeError(err::InvalidParameter, "{}",
+ "maxPressure must be greater than minPresure");
+ }
+ if (minPressure < 0_psi || minPressure > 120_psi) {
+ throw FRC_MakeError(err::ParameterOutOfRange,
+ "minPressure must be between 0 and 120 PSI, got {}",
+ minPressure);
+ }
+ if (maxPressure < 0_psi || maxPressure > 120_psi) {
+ throw FRC_MakeError(err::ParameterOutOfRange,
+ "maxPressure must be between 0 and 120 PSI, got {}",
+ maxPressure);
+ }
+ int32_t status = 0;
+ units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
+ units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
+ HAL_SetREVPHClosedLoopControlAnalog(m_handle, minAnalogVoltage.value(),
+ maxAnalogVoltage.value(), &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticHub::EnableCompressorHybrid(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ if (minPressure >= maxPressure) {
+ throw FRC_MakeError(err::InvalidParameter, "{}",
+ "maxPressure must be greater than minPresure");
+ }
+ if (minPressure < 0_psi || minPressure > 120_psi) {
+ throw FRC_MakeError(err::ParameterOutOfRange,
+ "minPressure must be between 0 and 120 PSI, got {}",
+ minPressure);
+ }
+ if (maxPressure < 0_psi || maxPressure > 120_psi) {
+ throw FRC_MakeError(err::ParameterOutOfRange,
+ "maxPressure must be between 0 and 120 PSI, got {}",
+ maxPressure);
+ }
+ int32_t status = 0;
+ units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
+ units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
+ HAL_SetREVPHClosedLoopControlHybrid(m_handle, minAnalogVoltage.value(),
+ maxAnalogVoltage.value(), &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+CompressorConfigType PneumaticHub::GetCompressorConfigType() const {
+ int32_t status = 0;
+ auto result = HAL_GetREVPHCompressorConfig(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return static_cast<CompressorConfigType>(result);
}
bool PneumaticHub::GetPressureSwitch() const {
int32_t status = 0;
auto result = HAL_GetREVPHPressureSwitch(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
-double PneumaticHub::GetCompressorCurrent() const {
+units::ampere_t PneumaticHub::GetCompressorCurrent() const {
int32_t status = 0;
auto result = HAL_GetREVPHCompressorCurrent(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
- return result;
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::ampere_t{result};
}
void PneumaticHub::SetSolenoids(int mask, int values) {
int32_t status = 0;
HAL_SetREVPHSolenoids(m_handle, mask, values, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
}
int PneumaticHub::GetSolenoids() const {
int32_t status = 0;
auto result = HAL_GetREVPHSolenoids(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
@@ -125,27 +231,26 @@
}
int PneumaticHub::GetSolenoidDisabledList() const {
- return 0;
- // TODO Fix me
- // int32_t status = 0;
- // auto result = HAL_GetREVPHSolenoidDisabledList(m_handle, &status);
- // FRC_CheckErrorStatus(status, "Module {}", m_module);
- // return result;
+ int32_t status = 0;
+ HAL_REVPHStickyFaults faults;
+ std::memset(&faults, 0, sizeof(faults));
+ HAL_GetREVPHStickyFaults(m_handle, &faults, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ uint32_t intFaults = 0;
+ static_assert(sizeof(faults) == sizeof(intFaults));
+ std::memcpy(&intFaults, &faults, sizeof(faults));
+ return intFaults & 0xFFFF;
}
void PneumaticHub::FireOneShot(int index) {
- // TODO Fix me
- // int32_t status = 0;
- // HAL_FireREVPHOneShot(m_handle, index, &status);
- // FRC_CheckErrorStatus(status, "Module {}", m_module);
+ int32_t status = 0;
+ HAL_FireREVPHOneShot(m_handle, index,
+ m_dataStore->m_oneShotDurMs[index].value(), &status);
+ FRC_ReportError(status, "Module {}", m_module);
}
void PneumaticHub::SetOneShotDuration(int index, units::second_t duration) {
- // TODO Fix me
- // int32_t status = 0;
- // units::millisecond_t millis = duration;
- // HAL_SetREVPHOneShotDuration(m_handle, index, millis.to<int32_t>(),
- // &status); FRC_CheckErrorStatus(status, "Module {}", m_module);
+ m_dataStore->m_oneShotDurMs[index] = duration;
}
bool PneumaticHub::CheckSolenoidChannel(int channel) const {
@@ -181,6 +286,98 @@
m_dataStore->m_compressorReserved = false;
}
+PneumaticHub::Version PneumaticHub::GetVersion() const {
+ int32_t status = 0;
+ HAL_REVPHVersion halVersions;
+ std::memset(&halVersions, 0, sizeof(halVersions));
+ HAL_GetREVPHVersion(m_handle, &halVersions, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PneumaticHub::Version versions;
+ static_assert(sizeof(halVersions) == sizeof(versions));
+ static_assert(std::is_standard_layout_v<decltype(versions)>);
+ static_assert(std::is_trivial_v<decltype(versions)>);
+ std::memcpy(&versions, &halVersions, sizeof(versions));
+ return versions;
+}
+
+PneumaticHub::Faults PneumaticHub::GetFaults() const {
+ int32_t status = 0;
+ HAL_REVPHFaults halFaults;
+ std::memset(&halFaults, 0, sizeof(halFaults));
+ HAL_GetREVPHFaults(m_handle, &halFaults, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PneumaticHub::Faults faults;
+ static_assert(sizeof(halFaults) == sizeof(faults));
+ static_assert(std::is_standard_layout_v<decltype(faults)>);
+ static_assert(std::is_trivial_v<decltype(faults)>);
+ std::memcpy(&faults, &halFaults, sizeof(faults));
+ return faults;
+}
+
+PneumaticHub::StickyFaults PneumaticHub::GetStickyFaults() const {
+ int32_t status = 0;
+ HAL_REVPHStickyFaults halStickyFaults;
+ std::memset(&halStickyFaults, 0, sizeof(halStickyFaults));
+ HAL_GetREVPHStickyFaults(m_handle, &halStickyFaults, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PneumaticHub::StickyFaults stickyFaults;
+ static_assert(sizeof(halStickyFaults) == sizeof(stickyFaults));
+ static_assert(std::is_standard_layout_v<decltype(stickyFaults)>);
+ static_assert(std::is_trivial_v<decltype(stickyFaults)>);
+ std::memcpy(&stickyFaults, &halStickyFaults, sizeof(stickyFaults));
+ return stickyFaults;
+}
+
+void PneumaticHub::ClearStickyFaults() {
+ int32_t status = 0;
+ HAL_ClearREVPHStickyFaults(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+units::volt_t PneumaticHub::GetInputVoltage() const {
+ int32_t status = 0;
+ auto voltage = HAL_GetREVPHVoltage(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::volt_t{voltage};
+}
+
+units::volt_t PneumaticHub::Get5VRegulatedVoltage() const {
+ int32_t status = 0;
+ auto voltage = HAL_GetREVPH5VVoltage(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::volt_t{voltage};
+}
+
+units::ampere_t PneumaticHub::GetSolenoidsTotalCurrent() const {
+ int32_t status = 0;
+ auto current = HAL_GetREVPHSolenoidCurrent(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::ampere_t{current};
+}
+
+units::volt_t PneumaticHub::GetSolenoidsVoltage() const {
+ int32_t status = 0;
+ auto voltage = HAL_GetREVPHSolenoidVoltage(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::volt_t{voltage};
+}
+
+units::volt_t PneumaticHub::GetAnalogVoltage(int channel) const {
+ int32_t status = 0;
+ auto voltage = HAL_GetREVPHAnalogVoltage(m_handle, channel, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::volt_t{voltage};
+}
+
+units::pounds_per_square_inch_t PneumaticHub::GetPressure(int channel) const {
+ int32_t status = 0;
+ auto sensorVoltage = HAL_GetREVPHAnalogVoltage(m_handle, channel, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ auto supplyVoltage = HAL_GetREVPH5VVoltage(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return VoltsToPSI(units::volt_t{sensorVoltage}, units::volt_t{supplyVoltage});
+}
+
Solenoid PneumaticHub::MakeSolenoid(int channel) {
return Solenoid{m_module, PneumaticsModuleType::REVPH, channel};
}
diff --git a/wpilibc/src/main/native/cpp/PneumaticsBase.cpp b/wpilibc/src/main/native/cpp/PneumaticsBase.cpp
index 9d96651..bd9e033 100644
--- a/wpilibc/src/main/native/cpp/PneumaticsBase.cpp
+++ b/wpilibc/src/main/native/cpp/PneumaticsBase.cpp
@@ -4,6 +4,8 @@
#include "frc/PneumaticsBase.h"
+#include <hal/REVPH.h>
+
#include "frc/Errors.h"
#include "frc/PneumaticHub.h"
#include "frc/PneumaticsControlModule.h"
@@ -11,6 +13,19 @@
using namespace frc;
+static_assert(
+ static_cast<int>(CompressorConfigType::Disabled) ==
+ HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kDisabled);
+static_assert(
+ static_cast<int>(CompressorConfigType::Digital) ==
+ HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kDigital);
+static_assert(
+ static_cast<int>(CompressorConfigType::Analog) ==
+ HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kAnalog);
+static_assert(
+ static_cast<int>(CompressorConfigType::Hybrid) ==
+ HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kHybrid);
+
std::shared_ptr<PneumaticsBase> PneumaticsBase::GetForType(
int module, PneumaticsModuleType moduleType) {
if (moduleType == PneumaticsModuleType::CTREPCM) {
@@ -18,7 +33,8 @@
} else if (moduleType == PneumaticsModuleType::REVPH) {
return PneumaticHub::GetForModule(module);
}
- throw FRC_MakeError(err::InvalidParameter, "{}", moduleType);
+ throw FRC_MakeError(err::InvalidParameter, "{}",
+ static_cast<int>(moduleType));
}
int PneumaticsBase::GetDefaultForType(PneumaticsModuleType moduleType) {
@@ -27,5 +43,6 @@
} else if (moduleType == PneumaticsModuleType::REVPH) {
return SensorUtil::GetDefaultREVPHModule();
}
- throw FRC_MakeError(err::InvalidParameter, "{}", moduleType);
+ throw FRC_MakeError(err::InvalidParameter, "{}",
+ static_cast<int>(moduleType));
}
diff --git a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
index 32de5ca..60cd84d 100644
--- a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
+++ b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
@@ -80,105 +80,128 @@
bool PneumaticsControlModule::GetCompressor() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressor(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
-void PneumaticsControlModule::SetClosedLoopControl(bool enabled) {
+void PneumaticsControlModule::DisableCompressor() {
int32_t status = 0;
- HAL_SetCTREPCMClosedLoopControl(m_handle, enabled, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ HAL_SetCTREPCMClosedLoopControl(m_handle, false, &status);
+ FRC_ReportError(status, "Module {}", m_module);
}
-bool PneumaticsControlModule::GetClosedLoopControl() const {
+void PneumaticsControlModule::EnableCompressorDigital() {
+ int32_t status = 0;
+ HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::EnableCompressorAnalog(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ int32_t status = 0;
+ HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::EnableCompressorHybrid(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ int32_t status = 0;
+ HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+CompressorConfigType PneumaticsControlModule::GetCompressorConfigType() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMClosedLoopControl(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
- return result;
+ FRC_ReportError(status, "Module {}", m_module);
+ return result ? CompressorConfigType::Digital
+ : CompressorConfigType::Disabled;
}
bool PneumaticsControlModule::GetPressureSwitch() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMPressureSwitch(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
-double PneumaticsControlModule::GetCompressorCurrent() const {
+units::ampere_t PneumaticsControlModule::GetCompressorCurrent() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressorCurrent(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
- return result;
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::ampere_t{result};
}
bool PneumaticsControlModule::GetCompressorCurrentTooHighFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressorCurrentTooHighFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetCompressorCurrentTooHighStickyFault() const {
int32_t status = 0;
auto result =
HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetCompressorShortedFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressorShortedFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetCompressorShortedStickyFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressorShortedStickyFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetCompressorNotConnectedFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressorNotConnectedFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetCompressorNotConnectedStickyFault() const {
int32_t status = 0;
auto result =
HAL_GetCTREPCMCompressorNotConnectedStickyFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetSolenoidVoltageFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMSolenoidVoltageFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetSolenoidVoltageStickyFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMSolenoidVoltageStickyFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
void PneumaticsControlModule::ClearAllStickyFaults() {
int32_t status = 0;
HAL_ClearAllCTREPCMStickyFaults(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
}
void PneumaticsControlModule::SetSolenoids(int mask, int values) {
int32_t status = 0;
HAL_SetCTREPCMSolenoids(m_handle, mask, values, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
}
int PneumaticsControlModule::GetSolenoids() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMSolenoids(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
@@ -189,14 +212,14 @@
int PneumaticsControlModule::GetSolenoidDisabledList() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMSolenoidDisabledList(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
void PneumaticsControlModule::FireOneShot(int index) {
int32_t status = 0;
HAL_FireCTREPCMOneShot(m_handle, index, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
}
void PneumaticsControlModule::SetOneShotDuration(int index,
@@ -204,7 +227,7 @@
int32_t status = 0;
units::millisecond_t millis = duration;
HAL_SetCTREPCMOneShotDuration(m_handle, index, millis.to<int32_t>(), &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
}
bool PneumaticsControlModule::CheckSolenoidChannel(int channel) const {
@@ -240,6 +263,15 @@
m_dataStore->m_compressorReserved = false;
}
+units::volt_t PneumaticsControlModule::GetAnalogVoltage(int channel) const {
+ return units::volt_t{0};
+}
+
+units::pounds_per_square_inch_t PneumaticsControlModule::GetPressure(
+ int channel) const {
+ return 0_psi;
+}
+
Solenoid PneumaticsControlModule::MakeSolenoid(int channel) {
return Solenoid{m_module, PneumaticsModuleType::CTREPCM, channel};
}
diff --git a/wpilibc/src/main/native/cpp/PowerDistribution.cpp b/wpilibc/src/main/native/cpp/PowerDistribution.cpp
index 4c2acfc..21101f2 100644
--- a/wpilibc/src/main/native/cpp/PowerDistribution.cpp
+++ b/wpilibc/src/main/native/cpp/PowerDistribution.cpp
@@ -16,9 +16,6 @@
#include "frc/SensorUtil.h"
static_assert(static_cast<HAL_PowerDistributionType>(
- frc::PowerDistribution::ModuleType::kAutomatic) ==
- HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic);
-static_assert(static_cast<HAL_PowerDistributionType>(
frc::PowerDistribution::ModuleType::kCTRE) ==
HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE);
static_assert(static_cast<HAL_PowerDistributionType>(
@@ -29,8 +26,21 @@
using namespace frc;
-PowerDistribution::PowerDistribution()
- : PowerDistribution(-1, ModuleType::kAutomatic) {}
+PowerDistribution::PowerDistribution() {
+ auto stack = wpi::GetStackTrace(1);
+
+ int32_t status = 0;
+ m_handle = HAL_InitializePowerDistribution(
+ kDefaultModule,
+ HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic,
+ stack.c_str(), &status);
+ FRC_CheckErrorStatus(status, "Module {}", kDefaultModule);
+ m_module = HAL_GetPowerDistributionModuleNumber(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+
+ HAL_Report(HALUsageReporting::kResourceType_PDP, m_module + 1);
+ wpi::SendableRegistry::AddLW(this, "PowerDistribution", m_module);
+}
PowerDistribution::PowerDistribution(int module, ModuleType moduleType) {
auto stack = wpi::GetStackTrace(1);
@@ -114,6 +124,13 @@
return m_module;
}
+PowerDistribution::ModuleType PowerDistribution::GetType() const {
+ int32_t status = 0;
+ auto type = HAL_GetPowerDistributionType(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return static_cast<ModuleType>(type);
+}
+
bool PowerDistribution::GetSwitchableChannel() const {
int32_t status = 0;
bool state = HAL_GetPowerDistributionSwitchableChannel(m_handle, &status);
@@ -127,20 +144,85 @@
FRC_ReportError(status, "Module {}", m_module);
}
+PowerDistribution::Version PowerDistribution::GetVersion() const {
+ int32_t status = 0;
+ HAL_PowerDistributionVersion halVersion;
+ std::memset(&halVersion, 0, sizeof(halVersion));
+ HAL_GetPowerDistributionVersion(m_handle, &halVersion, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PowerDistribution::Version version;
+ static_assert(sizeof(halVersion) == sizeof(version));
+ static_assert(std::is_standard_layout_v<decltype(version)>);
+ static_assert(std::is_trivial_v<decltype(version)>);
+ std::memcpy(&version, &halVersion, sizeof(version));
+ return version;
+}
+
+PowerDistribution::Faults PowerDistribution::GetFaults() const {
+ int32_t status = 0;
+ HAL_PowerDistributionFaults halFaults;
+ std::memset(&halFaults, 0, sizeof(halFaults));
+ HAL_GetPowerDistributionFaults(m_handle, &halFaults, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PowerDistribution::Faults faults;
+ static_assert(sizeof(halFaults) == sizeof(faults));
+ static_assert(std::is_standard_layout_v<decltype(faults)>);
+ static_assert(std::is_trivial_v<decltype(faults)>);
+ std::memcpy(&faults, &halFaults, sizeof(faults));
+ return faults;
+}
+
+PowerDistribution::StickyFaults PowerDistribution::GetStickyFaults() const {
+ int32_t status = 0;
+ HAL_PowerDistributionStickyFaults halStickyFaults;
+ std::memset(&halStickyFaults, 0, sizeof(halStickyFaults));
+ HAL_GetPowerDistributionStickyFaults(m_handle, &halStickyFaults, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PowerDistribution::StickyFaults stickyFaults;
+ static_assert(sizeof(halStickyFaults) == sizeof(stickyFaults));
+ static_assert(std::is_standard_layout_v<decltype(stickyFaults)>);
+ static_assert(std::is_trivial_v<decltype(stickyFaults)>);
+ std::memcpy(&stickyFaults, &halStickyFaults, sizeof(stickyFaults));
+ return stickyFaults;
+}
+
void PowerDistribution::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("PowerDistribution");
int32_t status = 0;
int numChannels = HAL_GetPowerDistributionNumChannels(m_handle, &status);
FRC_ReportError(status, "Module {}", m_module);
+ // Use manual reads to avoid printing errors
for (int i = 0; i < numChannels; ++i) {
builder.AddDoubleProperty(
- fmt::format("Chan{}", i), [=] { return GetCurrent(i); }, nullptr);
+ fmt::format("Chan{}", i),
+ [=] {
+ int32_t lStatus = 0;
+ return HAL_GetPowerDistributionChannelCurrent(m_handle, i, &lStatus);
+ },
+ nullptr);
}
builder.AddDoubleProperty(
- "Voltage", [=] { return GetVoltage(); }, nullptr);
+ "Voltage",
+ [=] {
+ int32_t lStatus = 0;
+ return HAL_GetPowerDistributionVoltage(m_handle, &lStatus);
+ },
+ nullptr);
builder.AddDoubleProperty(
- "TotalCurrent", [=] { return GetTotalCurrent(); }, nullptr);
+ "TotalCurrent",
+ [=] {
+ int32_t lStatus = 0;
+ return HAL_GetPowerDistributionTotalCurrent(m_handle, &lStatus);
+ },
+ nullptr);
builder.AddBooleanProperty(
- "SwitchableChannel", [=] { return GetSwitchableChannel(); },
- [=](bool value) { SetSwitchableChannel(value); });
+ "SwitchableChannel",
+ [=] {
+ int32_t lStatus = 0;
+ return HAL_GetPowerDistributionSwitchableChannel(m_handle, &lStatus);
+ },
+ [=](bool value) {
+ int32_t lStatus = 0;
+ HAL_SetPowerDistributionSwitchableChannel(m_handle, value, &lStatus);
+ });
}
diff --git a/wpilibc/src/main/native/cpp/Preferences.cpp b/wpilibc/src/main/native/cpp/Preferences.cpp
index 0c82f54..4663875 100644
--- a/wpilibc/src/main/native/cpp/Preferences.cpp
+++ b/wpilibc/src/main/native/cpp/Preferences.cpp
@@ -30,6 +30,14 @@
return instance;
}
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetPreferencesInstance() {
+ GetInstance() = Instance();
+}
+} // namespace frc::impl
+#endif
+
Preferences* Preferences::GetInstance() {
::GetInstance();
static Preferences instance;
@@ -79,6 +87,7 @@
void Preferences::InitString(std::string_view key, std::string_view value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultString(value);
+ entry.SetPersistent();
}
void Preferences::SetInt(std::string_view key, int value) {
@@ -94,6 +103,7 @@
void Preferences::InitInt(std::string_view key, int value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultDouble(value);
+ entry.SetPersistent();
}
void Preferences::SetDouble(std::string_view key, double value) {
@@ -109,6 +119,7 @@
void Preferences::InitDouble(std::string_view key, double value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultDouble(value);
+ entry.SetPersistent();
}
void Preferences::SetFloat(std::string_view key, float value) {
@@ -124,6 +135,7 @@
void Preferences::InitFloat(std::string_view key, float value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultDouble(value);
+ entry.SetPersistent();
}
void Preferences::SetBoolean(std::string_view key, bool value) {
@@ -139,6 +151,7 @@
void Preferences::InitBoolean(std::string_view key, bool value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultBoolean(value);
+ entry.SetPersistent();
}
void Preferences::SetLong(std::string_view key, int64_t value) {
@@ -154,6 +167,7 @@
void Preferences::InitLong(std::string_view key, int64_t value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultDouble(value);
+ entry.SetPersistent();
}
bool Preferences::ContainsKey(std::string_view key) {
diff --git a/wpilibc/src/main/native/cpp/counter/ExternalDirectionCounter.cpp b/wpilibc/src/main/native/cpp/counter/ExternalDirectionCounter.cpp
new file mode 100644
index 0000000..8c7bb58
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/counter/ExternalDirectionCounter.cpp
@@ -0,0 +1,103 @@
+// 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/counter/ExternalDirectionCounter.h"
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+ExternalDirectionCounter::ExternalDirectionCounter(
+ DigitalSource& countSource, DigitalSource& directionSource)
+ : ExternalDirectionCounter(
+ {&countSource, wpi::NullDeleter<DigitalSource>()},
+ {&directionSource, wpi::NullDeleter<DigitalSource>()}) {}
+
+ExternalDirectionCounter::ExternalDirectionCounter(
+ std::shared_ptr<DigitalSource> countSource,
+ std::shared_ptr<DigitalSource> directionSource) {
+ if (countSource == nullptr) {
+ throw FRC_MakeError(err::NullParameter, "{}", "countSource");
+ }
+ if (directionSource == nullptr) {
+ throw FRC_MakeError(err::NullParameter, "{}", "directionSource");
+ }
+
+ m_countSource = countSource;
+ m_directionSource = directionSource;
+
+ int32_t status = 0;
+ m_handle = HAL_InitializeCounter(
+ HAL_Counter_Mode::HAL_Counter_kExternalDirection, &m_index, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+
+ HAL_SetCounterUpSource(m_handle, m_countSource->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_countSource->GetAnalogTriggerTypeForRouting()),
+ &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+
+ HAL_SetCounterDownSource(
+ m_handle, m_directionSource->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_directionSource->GetAnalogTriggerTypeForRouting()),
+ &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ HAL_SetCounterDownSourceEdge(m_handle, false, true, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+
+ Reset();
+
+ HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+ wpi::SendableRegistry::AddLW(this, "External Direction Counter", m_index);
+}
+
+ExternalDirectionCounter::~ExternalDirectionCounter() {
+ int32_t status = 0;
+ HAL_FreeCounter(m_handle, &status);
+}
+
+int ExternalDirectionCounter::GetCount() const {
+ int32_t status = 0;
+ int val = HAL_GetCounter(m_handle, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ return val;
+}
+
+void ExternalDirectionCounter::SetReverseDirection(bool reverseDirection) {
+ int32_t status = 0;
+ HAL_SetCounterReverseDirection(m_handle, reverseDirection, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::Reset() {
+ int32_t status = 0;
+ HAL_ResetCounter(m_handle, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::SetEdgeConfiguration(
+ EdgeConfiguration configuration) {
+ int32_t status = 0;
+ bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ HAL_SetCounterUpSourceEdge(m_handle, rising, falling, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::InitSendable(wpi::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("External Direction Counter");
+ builder.AddDoubleProperty(
+ "Count", [&] { return GetCount(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/counter/Tachometer.cpp b/wpilibc/src/main/native/cpp/counter/Tachometer.cpp
new file mode 100644
index 0000000..a52bea5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/counter/Tachometer.cpp
@@ -0,0 +1,122 @@
+// 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/counter/Tachometer.h"
+
+#include <frc/DigitalSource.h>
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+Tachometer::Tachometer(DigitalSource& source)
+ : Tachometer({&source, wpi::NullDeleter<DigitalSource>()}) {}
+Tachometer::Tachometer(std::shared_ptr<DigitalSource> source) {
+ if (source == nullptr) {
+ throw FRC_MakeError(err::NullParameter, "{}", "source");
+ }
+
+ m_source = source;
+
+ int32_t status = 0;
+ HAL_SetCounterUpSource(m_handle, m_source->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_source->GetAnalogTriggerTypeForRouting()),
+ &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+
+ HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+ wpi::SendableRegistry::AddLW(this, "Tachometer", m_index);
+}
+
+Tachometer::~Tachometer() {
+ int32_t status = 0;
+ HAL_FreeCounter(m_handle, &status);
+}
+
+units::hertz_t Tachometer::GetFrequency() const {
+ auto period = GetPeriod();
+ if (period.to<double>() == 0) {
+ return units::hertz_t{0.0};
+ }
+ return 1 / period;
+}
+
+units::second_t Tachometer::GetPeriod() const {
+ int32_t status = 0;
+ double period = HAL_GetCounterPeriod(m_handle, &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+ return units::second_t{period};
+}
+
+int Tachometer::GetEdgesPerRevolution() const {
+ return m_edgesPerRevolution;
+}
+void Tachometer::SetEdgesPerRevolution(int edges) {
+ m_edgesPerRevolution = edges;
+}
+
+units::turns_per_second_t Tachometer::GetRevolutionsPerSecond() const {
+ auto period = GetPeriod();
+ if (period.to<double>() == 0) {
+ return units::turns_per_second_t{0.0};
+ }
+ int edgesPerRevolution = GetEdgesPerRevolution();
+ if (edgesPerRevolution == 0) {
+ return units::turns_per_second_t{0.0};
+ }
+ auto rotationHz = ((1.0 / edgesPerRevolution) / period);
+ return units::turns_per_second_t{rotationHz.to<double>()};
+}
+
+units::revolutions_per_minute_t Tachometer::GetRevolutionsPerMinute() const {
+ return units::revolutions_per_minute_t{GetRevolutionsPerSecond()};
+}
+
+bool Tachometer::GetStopped() const {
+ int32_t status = 0;
+ bool stopped = HAL_GetCounterStopped(m_handle, &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+ return stopped;
+}
+
+int Tachometer::GetSamplesToAverage() const {
+ int32_t status = 0;
+ int32_t samplesToAverage = HAL_GetCounterSamplesToAverage(m_handle, &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+ return samplesToAverage;
+}
+
+void Tachometer::SetSamplesToAverage(int samples) {
+ int32_t status = 0;
+ HAL_SetCounterSamplesToAverage(m_handle, samples, &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::SetMaxPeriod(units::second_t maxPeriod) {
+ int32_t status = 0;
+ HAL_SetCounterMaxPeriod(m_handle, maxPeriod.to<double>(), &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::SetUpdateWhenEmpty(bool updateWhenEmpty) {
+ int32_t status = 0;
+ HAL_SetCounterUpdateWhenEmpty(m_handle, updateWhenEmpty, &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::InitSendable(wpi::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Tachometer");
+ builder.AddDoubleProperty(
+ "RPS", [&] { return GetRevolutionsPerSecond().to<double>(); }, nullptr);
+ builder.AddDoubleProperty(
+ "RPM", [&] { return GetRevolutionsPerMinute().to<double>(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/counter/UpDownCounter.cpp b/wpilibc/src/main/native/cpp/counter/UpDownCounter.cpp
new file mode 100644
index 0000000..62537ee
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/counter/UpDownCounter.cpp
@@ -0,0 +1,106 @@
+// 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/counter/UpDownCounter.h"
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+UpDownCounter::UpDownCounter(DigitalSource& upSource, DigitalSource& downSource)
+ : UpDownCounter({&upSource, wpi::NullDeleter<DigitalSource>()},
+ {&downSource, wpi::NullDeleter<DigitalSource>()}) {}
+
+UpDownCounter::UpDownCounter(std::shared_ptr<DigitalSource> upSource,
+ std::shared_ptr<DigitalSource> downSource) {
+ m_upSource = upSource;
+ m_downSource = downSource;
+
+ int32_t status = 0;
+ m_handle = HAL_InitializeCounter(HAL_Counter_Mode::HAL_Counter_kTwoPulse,
+ &m_index, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+
+ if (m_upSource) {
+ HAL_SetCounterUpSource(m_handle, m_upSource->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_upSource->GetAnalogTriggerTypeForRouting()),
+ &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ }
+
+ if (m_downSource) {
+ HAL_SetCounterDownSource(
+ m_handle, m_downSource->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_downSource->GetAnalogTriggerTypeForRouting()),
+ &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ HAL_SetCounterDownSourceEdge(m_handle, true, false, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ }
+
+ Reset();
+
+ HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+ wpi::SendableRegistry::AddLW(this, "UpDown Counter", m_index);
+}
+
+UpDownCounter::~UpDownCounter() {
+ int32_t status = 0;
+ HAL_FreeCounter(m_handle, &status);
+}
+
+int UpDownCounter::GetCount() const {
+ int32_t status = 0;
+ int val = HAL_GetCounter(m_handle, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ return val;
+}
+
+void UpDownCounter::SetReverseDirection(bool reverseDirection) {
+ int32_t status = 0;
+ HAL_SetCounterReverseDirection(m_handle, reverseDirection, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::Reset() {
+ int32_t status = 0;
+ HAL_ResetCounter(m_handle, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::SetUpEdgeConfiguration(EdgeConfiguration configuration) {
+ int32_t status = 0;
+ bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ HAL_SetCounterUpSourceEdge(m_handle, rising, falling, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::SetDownEdgeConfiguration(EdgeConfiguration configuration) {
+ int32_t status = 0;
+ bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ HAL_SetCounterDownSourceEdge(m_handle, rising, falling, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::InitSendable(wpi::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("UpDown Counter");
+ builder.AddDoubleProperty(
+ "Count", [&] { return GetCount(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index b54b607..eed9495 100644
--- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -112,9 +112,10 @@
double maxInput =
std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
- if (xSpeed >= 0.0) {
+ // Sign is used because `xSpeed >= 0.0` succeeds for -0.0
+ if (!std::signbit(xSpeed)) {
// First quadrant, else second quadrant
- if (zRotation >= 0.0) {
+ if (!std::signbit(zRotation)) {
leftSpeed = maxInput;
rightSpeed = xSpeed - zRotation;
} else {
@@ -123,7 +124,7 @@
}
} else {
// Third quadrant, else fourth quadrant
- if (zRotation >= 0.0) {
+ if (!std::signbit(zRotation)) {
leftSpeed = xSpeed + zRotation;
rightSpeed = maxInput;
} else {
diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
index c847678..268ae64 100644
--- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -102,7 +102,7 @@
wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
- Normalize(wheelSpeeds);
+ Desaturate(wheelSpeeds);
return {wheelSpeeds[kLeft], wheelSpeeds[kRight], wheelSpeeds[kBack]};
}
diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index b74c611..70e90d4 100644
--- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -103,7 +103,7 @@
wheelSpeeds[kRearLeft] = input.x - input.y + zRotation;
wheelSpeeds[kRearRight] = input.x + input.y - zRotation;
- Normalize(wheelSpeeds);
+ Desaturate(wheelSpeeds);
return {wheelSpeeds[kFrontLeft], wheelSpeeds[kFrontRight],
wheelSpeeds[kRearLeft], wheelSpeeds[kRearRight]};
diff --git a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
index 2159958..97ea0ee 100644
--- a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
+++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
@@ -35,7 +35,7 @@
return frc::ApplyDeadband(value, deadband);
}
-void RobotDriveBase::Normalize(wpi::span<double> wheelSpeeds) {
+void RobotDriveBase::Desaturate(wpi::span<double> wheelSpeeds) {
double maxMagnitude = std::abs(wheelSpeeds[0]);
for (size_t i = 1; i < wheelSpeeds.size(); i++) {
double temp = std::abs(wheelSpeeds[i]);
diff --git a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
index f8be7de..ac1c8ad 100644
--- a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -48,11 +48,23 @@
};
} // namespace
-static Instance& GetInstance() {
- static Instance instance;
+static std::unique_ptr<Instance>& GetInstanceHolder() {
+ static std::unique_ptr<Instance> instance = std::make_unique<Instance>();
return instance;
}
+static Instance& GetInstance() {
+ return *GetInstanceHolder();
+}
+
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetLiveWindow() {
+ std::make_unique<Instance>().swap(GetInstanceHolder());
+}
+} // namespace frc::impl
+#endif
+
std::shared_ptr<Component> Instance::GetOrAdd(wpi::Sendable* sendable) {
auto data = std::static_pointer_cast<Component>(
wpi::SendableRegistry::GetData(sendable, dataHandle));
diff --git a/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp b/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
index 9829a75..f9ff4b8 100644
--- a/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
+++ b/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
@@ -42,6 +42,10 @@
return m_pwm.GetChannel();
}
+void PWMMotorController::EnableDeadbandElimination(bool eliminateDeadband) {
+ m_pwm.EnableDeadbandElimination(eliminateDeadband);
+}
+
PWMMotorController::PWMMotorController(std::string_view name, int channel)
: m_pwm(channel, false) {
wpi::SendableRegistry::AddLW(this, name, channel);
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
index 1675c64..d5e076a 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
@@ -8,19 +8,19 @@
#include <memory>
#include <string>
-#include <wpi/DenseMap.h>
+#include <wpi/StringMap.h>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
namespace frc {
namespace detail {
std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
- CS_Source source) {
- static wpi::DenseMap<int, std::shared_ptr<SendableCameraWrapper>> wrappers;
- return wrappers[static_cast<int>(source)];
+ std::string_view cameraName) {
+ static wpi::StringMap<std::shared_ptr<SendableCameraWrapper>> wrappers;
+ return wrappers[cameraName];
}
-void AddToSendableRegistry(wpi::Sendable* sendable, std::string name) {
+void AddToSendableRegistry(wpi::Sendable* sendable, std::string_view name) {
wpi::SendableRegistry::Add(sendable, name);
}
} // namespace detail
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
index 81f6671..31e4b11 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
@@ -31,6 +31,7 @@
"Differential Drivebase",
"Mecanum Drivebase",
"Camera Stream",
+ "Field",
};
const char* detail::GetStringForWidgetType(BuiltInWidgets type) {
diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
new file mode 100644
index 0000000..46c412a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
@@ -0,0 +1,59 @@
+// 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/simulation/ADIS16448_IMUSim.h"
+
+#include <frc/ADIS16448_IMU.h>
+#include <frc/simulation/SimDeviceSim.h>
+
+using namespace frc::sim;
+
+ADIS16448_IMUSim::ADIS16448_IMUSim(const frc::ADIS16448_IMU& imu) {
+ frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16448", imu.GetPort()};
+ m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
+ m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
+ m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
+ m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
+ m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
+ m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
+ m_simAccelX = deviceSim.GetDouble("accel_x");
+ m_simAccelY = deviceSim.GetDouble("accel_y");
+ m_simAccelZ = deviceSim.GetDouble("accel_z");
+}
+
+void ADIS16448_IMUSim::SetGyroAngleX(units::degree_t angle) {
+ m_simGyroAngleX.Set(angle.value());
+}
+
+void ADIS16448_IMUSim::SetGyroAngleY(units::degree_t angle) {
+ m_simGyroAngleY.Set(angle.value());
+}
+
+void ADIS16448_IMUSim::SetGyroAngleZ(units::degree_t angle) {
+ m_simGyroAngleZ.Set(angle.value());
+}
+
+void ADIS16448_IMUSim::SetGyroRateX(units::degrees_per_second_t angularRate) {
+ m_simGyroRateX.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) {
+ m_simGyroRateY.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) {
+ m_simGyroRateZ.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
+ m_simAccelX.Set(accel.value());
+}
+
+void ADIS16448_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
+ m_simAccelY.Set(accel.value());
+}
+
+void ADIS16448_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
+ m_simAccelZ.Set(accel.value());
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
new file mode 100644
index 0000000..40c09c3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
@@ -0,0 +1,59 @@
+// 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/simulation/ADIS16470_IMUSim.h"
+
+#include <frc/ADIS16470_IMU.h>
+#include <frc/simulation/SimDeviceSim.h>
+
+using namespace frc::sim;
+
+ADIS16470_IMUSim::ADIS16470_IMUSim(const frc::ADIS16470_IMU& imu) {
+ frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16470", imu.GetPort()};
+ m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
+ m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
+ m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
+ m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
+ m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
+ m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
+ m_simAccelX = deviceSim.GetDouble("accel_x");
+ m_simAccelY = deviceSim.GetDouble("accel_y");
+ m_simAccelZ = deviceSim.GetDouble("accel_z");
+}
+
+void ADIS16470_IMUSim::SetGyroAngleX(units::degree_t angle) {
+ m_simGyroAngleX.Set(angle.value());
+}
+
+void ADIS16470_IMUSim::SetGyroAngleY(units::degree_t angle) {
+ m_simGyroAngleY.Set(angle.value());
+}
+
+void ADIS16470_IMUSim::SetGyroAngleZ(units::degree_t angle) {
+ m_simGyroAngleZ.Set(angle.value());
+}
+
+void ADIS16470_IMUSim::SetGyroRateX(units::degrees_per_second_t angularRate) {
+ m_simGyroRateX.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) {
+ m_simGyroRateY.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) {
+ m_simGyroRateZ.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
+ m_simAccelX.Set(accel.value());
+}
+
+void ADIS16470_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
+ m_simAccelY.Set(accel.value());
+}
+
+void ADIS16470_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
+ m_simAccelZ.Set(accel.value());
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
new file mode 100644
index 0000000..7860e32
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
@@ -0,0 +1,46 @@
+// 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/simulation/DCMotorSim.h"
+
+#include <wpi/MathExtras.h>
+
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DCMotorSim::DCMotorSim(const LinearSystem<2, 1, 2>& plant,
+ const DCMotor& gearbox, double gearing,
+ const std::array<double, 2>& measurementStdDevs)
+ : LinearSystemSim<2, 1, 2>(plant, measurementStdDevs),
+ m_gearbox(gearbox),
+ m_gearing(gearing) {}
+
+DCMotorSim::DCMotorSim(const DCMotor& gearbox, double gearing,
+ units::kilogram_square_meter_t moi,
+ const std::array<double, 2>& measurementStdDevs)
+ : DCMotorSim(LinearSystemId::DCMotorSystem(gearbox, moi, gearing), gearbox,
+ gearing, measurementStdDevs) {}
+
+units::radian_t DCMotorSim::GetAngularPosition() const {
+ return units::radian_t{GetOutput(0)};
+}
+
+units::radians_per_second_t DCMotorSim::GetAngularVelocity() const {
+ return units::radians_per_second_t{GetOutput(1)};
+}
+
+units::ampere_t DCMotorSim::GetCurrentDraw() const {
+ // I = V / R - omega / (Kv * R)
+ // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+ // is spinning 10x faster than the output.
+ return m_gearbox.Current(GetAngularVelocity() * m_gearing,
+ units::volt_t{m_u(0)}) *
+ wpi::sgn(m_u(0));
+}
+
+void DCMotorSim::SetInputVoltage(units::volt_t voltage) {
+ SetInput(Eigen::Vector<double, 1>{voltage.value()});
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
index 0bbf9c3..78b9dc7 100644
--- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -43,8 +43,8 @@
Eigen::Vector<double, 2> DifferentialDrivetrainSim::ClampInput(
const Eigen::Vector<double, 2>& u) {
- return frc::NormalizeInputVector<2>(u,
- frc::RobotController::GetInputVoltage());
+ return frc::DesaturateInputVector<2>(u,
+ frc::RobotController::GetInputVoltage());
}
void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage,
diff --git a/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp b/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
index dd6dba8..ca7384a 100644
--- a/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
@@ -73,21 +73,23 @@
HALSIM_SetREVPHCompressorOn(m_index, compressorOn);
}
-std::unique_ptr<CallbackStore> REVPHSim::RegisterClosedLoopEnabledCallback(
+std::unique_ptr<CallbackStore> REVPHSim::RegisterCompressorConfigTypeCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
- m_index, -1, callback, &HALSIM_CancelREVPHClosedLoopEnabledCallback);
- store->SetUid(HALSIM_RegisterREVPHClosedLoopEnabledCallback(
+ m_index, -1, callback, &HALSIM_CancelREVPHCompressorConfigTypeCallback);
+ store->SetUid(HALSIM_RegisterREVPHCompressorConfigTypeCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
-bool REVPHSim::GetClosedLoopEnabled() const {
- return HALSIM_GetREVPHClosedLoopEnabled(m_index);
+int REVPHSim::GetCompressorConfigType() const {
+ return HALSIM_GetREVPHCompressorConfigType(m_index);
}
-void REVPHSim::SetClosedLoopEnabled(bool closedLoopEnabled) {
- HALSIM_SetREVPHClosedLoopEnabled(m_index, closedLoopEnabled);
+void REVPHSim::SetCompressorConfigType(int compressorConfigType) {
+ HALSIM_SetREVPHCompressorConfigType(
+ m_index,
+ static_cast<HAL_REVPHCompressorConfigType>(compressorConfigType));
}
std::unique_ptr<CallbackStore> REVPHSim::RegisterPressureSwitchCallback(
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
index 175db0d..b49bc99 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
@@ -21,7 +21,6 @@
void MechanismLigament2d::UpdateEntries(
std::shared_ptr<nt::NetworkTable> table) {
- std::scoped_lock lock(m_mutex);
table->GetEntry(".type").SetString("line");
m_colorEntry = table->GetEntry("color");
@@ -50,7 +49,20 @@
Flush();
}
+Color8Bit MechanismLigament2d::GetColor() {
+ std::scoped_lock lock(m_mutex);
+ if (m_colorEntry) {
+ auto color = m_colorEntry.GetString("");
+ std::strncpy(m_color, color.c_str(), sizeof(m_color));
+ m_color[sizeof(m_color) - 1] = '\0';
+ }
+ unsigned int r = 0, g = 0, b = 0;
+ std::sscanf(m_color, "#%02X%02X%02X", &r, &g, &b);
+ return {static_cast<int>(r), static_cast<int>(g), static_cast<int>(b)};
+}
+
double MechanismLigament2d::GetAngle() {
+ std::scoped_lock lock(m_mutex);
if (m_angleEntry) {
m_angle = m_angleEntry.GetDouble(0.0);
}
@@ -58,12 +70,21 @@
}
double MechanismLigament2d::GetLength() {
+ std::scoped_lock lock(m_mutex);
if (m_lengthEntry) {
m_length = m_lengthEntry.GetDouble(0.0);
}
return m_length;
}
+double MechanismLigament2d::GetLineWeight() {
+ std::scoped_lock lock(m_mutex);
+ if (m_weightEntry) {
+ m_weight = m_weightEntry.GetDouble(0.0);
+ }
+ return m_weight;
+}
+
void MechanismLigament2d::SetLength(double length) {
std::scoped_lock lock(m_mutex);
m_length = length;
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp
index 07d15f0..637b24b 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp
@@ -20,14 +20,12 @@
}
void MechanismRoot2d::UpdateEntries(std::shared_ptr<nt::NetworkTable> table) {
- std::scoped_lock lock(m_mutex);
m_xEntry = table->GetEntry("x");
m_yEntry = table->GetEntry("y");
Flush();
}
inline void MechanismRoot2d::Flush() {
- std::scoped_lock lock(m_mutex);
if (m_xEntry) {
m_xEntry.SetDouble(m_x);
}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
index 25bb61f..5088394 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -19,6 +19,8 @@
namespace {
struct Instance {
+ Instance() { HAL_Report(HALUsageReporting::kResourceType_SmartDashboard, 0); }
+
detail::ListenerExecutor listenerExecutor;
std::shared_ptr<nt::NetworkTable> table =
nt::NetworkTableInstance::GetDefault().GetTable("SmartDashboard");
@@ -27,12 +29,23 @@
};
} // namespace
-static Instance& GetInstance() {
- HAL_Report(HALUsageReporting::kResourceType_SmartDashboard, 0);
- static Instance instance;
+static std::unique_ptr<Instance>& GetInstanceHolder() {
+ static std::unique_ptr<Instance> instance = std::make_unique<Instance>();
return instance;
}
+static Instance& GetInstance() {
+ return *GetInstanceHolder();
+}
+
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetSmartDashboardInstance() {
+ std::make_unique<Instance>().swap(GetInstanceHolder());
+}
+} // namespace frc::impl
+#endif
+
void SmartDashboard::init() {
GetInstance();
}
diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp
index 78c286c..3433a02 100644
--- a/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -251,6 +251,9 @@
}
}
+ // Call DriverStation::InDisabled() to kick off DS thread
+ DriverStation::InDisabled(true);
+
// First and one-time initialization
inst.GetTable("LiveWindow")
->GetSubTable(".status")
diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
new file mode 100644
index 0000000..0dbfa68
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
@@ -0,0 +1,428 @@
+// 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.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. 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. */
+/* */
+/* Modified by Juan Chong - frcsupport@analog.com */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+#include <atomic>
+#include <memory>
+#include <thread>
+
+#include <hal/SimDevice.h>
+#include <networktables/NTSendable.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/magnetic_field_strength.h>
+#include <units/pressure.h>
+#include <units/temperature.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+#include "frc/DigitalSource.h"
+#include "frc/SPI.h"
+
+namespace frc {
+/**
+ * Use DMA SPI to read rate, acceleration, and magnetometer data from the
+ * ADIS16448 IMU and return the robots heading relative to a starting position,
+ * AHRS, and instant measurements
+ *
+ * The ADIS16448 gyro angle outputs track the robot's heading based on the
+ * starting position. As the robot rotates the new heading is computed by
+ * integrating the rate of rotation returned by the IMU. When the class is
+ * instantiated, a short calibration routine is performed where the IMU samples
+ * the gyros while at rest to determine the initial offset. This is subtracted
+ * from each sample to determine the heading.
+ *
+ * This class is for the ADIS16448 IMU connected via the SPI port available on
+ * the RoboRIO MXP port.
+ */
+
+class ADIS16448_IMU : public nt::NTSendable,
+ public wpi::SendableHelper<ADIS16448_IMU> {
+ public:
+ /* ADIS16448 Calibration Time Enum Class */
+ enum class CalibrationTime {
+ _32ms = 0,
+ _64ms = 1,
+ _128ms = 2,
+ _256ms = 3,
+ _512ms = 4,
+ _1s = 5,
+ _2s = 6,
+ _4s = 7,
+ _8s = 8,
+ _16s = 9,
+ _32s = 10,
+ _64s = 11
+ };
+
+ enum IMUAxis { kX, kY, kZ };
+
+ /**
+ * IMU constructor on onboard MXP CS0, Z-up orientation, and complementary
+ * AHRS computation.
+ */
+ ADIS16448_IMU();
+
+ /**
+ * IMU constructor on the specified MXP port and orientation.
+ *
+ * @param yaw_axis The axis where gravity is present. Valid options are kX,
+ * kY, and kZ
+ * @param port The SPI port where the IMU is connected.
+ * @param cal_time The calibration time that should be used on start-up.
+ */
+ explicit ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
+ CalibrationTime cal_time);
+
+ ~ADIS16448_IMU() override;
+
+ ADIS16448_IMU(ADIS16448_IMU&&) = default;
+ ADIS16448_IMU& operator=(ADIS16448_IMU&&) = default;
+
+ /**
+ * Initialize the IMU.
+ *
+ * Perform gyro offset calibration by collecting data for a number of seconds
+ * and computing the center value. The center value is subtracted from
+ * subsequent measurements.
+ *
+ * It's important to make sure that the robot is not moving while the
+ * centering calculations are in progress, this is typically done when the
+ * robot is first turned on while it's sitting at rest before the match
+ * starts.
+ *
+ * The calibration routine can be triggered by the user during runtime.
+ */
+ void Calibrate();
+
+ /**
+ * Configures the calibration time used for the next calibrate.
+ *
+ * @param new_cal_time The calibration time that should be used
+ */
+ int ConfigCalTime(CalibrationTime new_cal_time);
+
+ /**
+ * Reset the gyro.
+ *
+ * Resets the gyro accumulations to a heading of zero. This can be used if
+ * there is significant drift in the gyro and it needs to be recalibrated
+ * after running.
+ */
+ void Reset();
+
+ /**
+ * Returns the yaw axis angle in degrees (CCW positive).
+ */
+ units::degree_t GetAngle() const;
+
+ /**
+ * Returns the yaw axis angular rate in degrees per second (CCW positive).
+ */
+ units::degrees_per_second_t GetRate() const;
+
+ /**
+ * Returns the accumulated gyro angle in the X axis.
+ */
+ units::degree_t GetGyroAngleX() const;
+
+ /**
+ * Returns the accumulated gyro angle in the Y axis.
+ */
+ units::degree_t GetGyroAngleY() const;
+
+ /**
+ * Returns the accumulated gyro angle in the Z axis.
+ */
+ units::degree_t GetGyroAngleZ() const;
+
+ /**
+ * Returns the angular rate in the X axis.
+ */
+ units::degrees_per_second_t GetGyroRateX() const;
+
+ /**
+ * Returns the angular rate in the Y axis.
+ */
+ units::degrees_per_second_t GetGyroRateY() const;
+
+ /**
+ * Returns the angular rate in the Z axis.
+ */
+ units::degrees_per_second_t GetGyroRateZ() const;
+
+ /**
+ * Returns the acceleration in the X axis.
+ */
+ units::meters_per_second_squared_t GetAccelX() const;
+
+ /**
+ * Returns the acceleration in the Y axis.
+ */
+ units::meters_per_second_squared_t GetAccelY() const;
+
+ /**
+ * Returns the acceleration in the Z axis.
+ */
+ units::meters_per_second_squared_t GetAccelZ() const;
+
+ units::degree_t GetXComplementaryAngle() const;
+
+ units::degree_t GetYComplementaryAngle() const;
+
+ units::degree_t GetXFilteredAccelAngle() const;
+
+ units::degree_t GetYFilteredAccelAngle() const;
+
+ units::tesla_t GetMagneticFieldX() const;
+
+ units::tesla_t GetMagneticFieldY() const;
+
+ units::tesla_t GetMagneticFieldZ() const;
+
+ units::pounds_per_square_inch_t GetBarometricPressure() const;
+
+ units::celsius_t GetTemperature() const;
+
+ IMUAxis GetYawAxis() const;
+
+ int SetYawAxis(IMUAxis yaw_axis);
+
+ int ConfigDecRate(uint16_t DecimationRate);
+
+ /**
+ * Get the SPI port number.
+ *
+ * @return The SPI port number.
+ */
+ int GetPort() const;
+
+ void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+ /** @brief ADIS16448 Register Map Declaration */
+ static constexpr uint8_t FLASH_CNT = 0x00; // Flash memory write count
+ static constexpr uint8_t XGYRO_OUT = 0x04; // X-axis gyroscope output
+ static constexpr uint8_t YGYRO_OUT = 0x06; // Y-axis gyroscope output
+ static constexpr uint8_t ZGYRO_OUT = 0x08; // Z-axis gyroscope output
+ static constexpr uint8_t XACCL_OUT = 0x0A; // X-axis accelerometer output
+ static constexpr uint8_t YACCL_OUT = 0x0C; // Y-axis accelerometer output
+ static constexpr uint8_t ZACCL_OUT = 0x0E; // Z-axis accelerometer output
+ static constexpr uint8_t XMAGN_OUT = 0x10; // X-axis magnetometer output
+ static constexpr uint8_t YMAGN_OUT = 0x12; // Y-axis magnetometer output
+ static constexpr uint8_t ZMAGN_OUT = 0x14; // Z-axis magnetometer output
+ static constexpr uint8_t BARO_OUT =
+ 0x16; // Barometer pressure measurement, high word
+ static constexpr uint8_t TEMP_OUT = 0x18; // Temperature output
+ static constexpr uint8_t XGYRO_OFF =
+ 0x1A; // X-axis gyroscope bias offset factor
+ static constexpr uint8_t YGYRO_OFF =
+ 0x1C; // Y-axis gyroscope bias offset factor
+ static constexpr uint8_t ZGYRO_OFF =
+ 0x1E; // Z-axis gyroscope bias offset factor
+ static constexpr uint8_t XACCL_OFF =
+ 0x20; // X-axis acceleration bias offset factor
+ static constexpr uint8_t YACCL_OFF =
+ 0x22; // Y-axis acceleration bias offset factor
+ static constexpr uint8_t ZACCL_OFF =
+ 0x24; // Z-axis acceleration bias offset factor
+ static constexpr uint8_t XMAGN_HIC =
+ 0x26; // X-axis magnetometer, hard iron factor
+ static constexpr uint8_t YMAGN_HIC =
+ 0x28; // Y-axis magnetometer, hard iron factor
+ static constexpr uint8_t ZMAGN_HIC =
+ 0x2A; // Z-axis magnetometer, hard iron factor
+ static constexpr uint8_t XMAGN_SIC =
+ 0x2C; // X-axis magnetometer, soft iron factor
+ static constexpr uint8_t YMAGN_SIC =
+ 0x2E; // Y-axis magnetometer, soft iron factor
+ static constexpr uint8_t ZMAGN_SIC =
+ 0x30; // Z-axis magnetometer, soft iron factor
+ static constexpr uint8_t GPIO_CTRL = 0x32; // GPIO control
+ static constexpr uint8_t MSC_CTRL = 0x34; // MISC control
+ static constexpr uint8_t SMPL_PRD =
+ 0x36; // Sample clock/Decimation filter control
+ static constexpr uint8_t SENS_AVG = 0x38; // Digital filter control
+ static constexpr uint8_t SEQ_CNT = 0x3A; // MAGN_OUT and BARO_OUT counter
+ static constexpr uint8_t DIAG_STAT = 0x3C; // System status
+ static constexpr uint8_t GLOB_CMD = 0x3E; // System command
+ static constexpr uint8_t ALM_MAG1 = 0x40; // Alarm 1 amplitude threshold
+ static constexpr uint8_t ALM_MAG2 = 0x42; // Alarm 2 amplitude threshold
+ static constexpr uint8_t ALM_SMPL1 = 0x44; // Alarm 1 sample size
+ static constexpr uint8_t ALM_SMPL2 = 0x46; // Alarm 2 sample size
+ static constexpr uint8_t ALM_CTRL = 0x48; // Alarm control
+ static constexpr uint8_t LOT_ID1 = 0x52; // Lot identification number
+ static constexpr uint8_t LOT_ID2 = 0x54; // Lot identification number
+ static constexpr uint8_t PROD_ID = 0x56; // Product identifier
+ static constexpr uint8_t SERIAL_NUM = 0x58; // Lot-specific serial number
+
+ /** @brief ADIS16448 Static Constants */
+ static constexpr double rad_to_deg = 57.2957795;
+ static constexpr double deg_to_rad = 0.0174532;
+ static constexpr double grav = 9.81;
+
+ /** @brief struct to store offset data */
+ struct OffsetData {
+ double gyro_rate_x = 0.0;
+ double gyro_rate_y = 0.0;
+ double gyro_rate_z = 0.0;
+ };
+
+ bool SwitchToStandardSPI();
+
+ bool SwitchToAutoSPI();
+
+ uint16_t ReadRegister(uint8_t reg);
+
+ void WriteRegister(uint8_t reg, uint16_t val);
+
+ void Acquire();
+
+ void Close();
+
+ // User-specified yaw axis
+ IMUAxis m_yaw_axis;
+
+ // Last read values (post-scaling)
+ double m_gyro_rate_x = 0.0;
+ double m_gyro_rate_y = 0.0;
+ double m_gyro_rate_z = 0.0;
+ double m_accel_x = 0.0;
+ double m_accel_y = 0.0;
+ double m_accel_z = 0.0;
+ double m_mag_x = 0.0;
+ double m_mag_y = 0.0;
+ double m_mag_z = 0.0;
+ double m_baro = 0.0;
+ double m_temp = 0.0;
+
+ // Complementary filter variables
+ double m_tau = 0.5;
+ double m_dt, m_alpha = 0.0;
+ double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
+
+ // vector for storing most recent imu values
+ OffsetData* m_offset_buffer = nullptr;
+
+ double m_gyro_rate_offset_x = 0.0;
+ double m_gyro_rate_offset_y = 0.0;
+ double m_gyro_rate_offset_z = 0.0;
+
+ // function to re-init offset buffer
+ void InitOffsetBuffer(int size);
+
+ // Accumulated gyro values (for offset calculation)
+ int m_avg_size = 0;
+ int m_accum_count = 0;
+
+ // Integrated gyro values
+ double m_integ_gyro_angle_x = 0.0;
+ double m_integ_gyro_angle_y = 0.0;
+ double m_integ_gyro_angle_z = 0.0;
+
+ // Complementary filter functions
+ double FormatFastConverge(double compAngle, double accAngle);
+
+ double FormatRange0to2PI(double compAngle);
+
+ double FormatAccelRange(double accelAngle, double accelZ);
+
+ double CompFilterProcess(double compAngle, double accelAngle, double omega);
+
+ // State and resource variables
+ volatile bool m_thread_active = false;
+ volatile bool m_first_run = true;
+ volatile bool m_thread_idle = false;
+ volatile bool m_start_up_mode = true;
+
+ bool m_auto_configured = false;
+ SPI::Port m_spi_port;
+ CalibrationTime m_calibration_time{0};
+ SPI* m_spi = nullptr;
+ DigitalInput* m_auto_interrupt = nullptr;
+
+ std::thread m_acquire_task;
+
+ hal::SimDevice m_simDevice;
+ hal::SimDouble m_simGyroAngleX;
+ hal::SimDouble m_simGyroAngleY;
+ hal::SimDouble m_simGyroAngleZ;
+ hal::SimDouble m_simGyroRateX;
+ hal::SimDouble m_simGyroRateY;
+ hal::SimDouble m_simGyroRateZ;
+ hal::SimDouble m_simAccelX;
+ hal::SimDouble m_simAccelY;
+ hal::SimDouble m_simAccelZ;
+
+ struct NonMovableMutexWrapper {
+ wpi::mutex mutex;
+ NonMovableMutexWrapper() = default;
+
+ NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
+ NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
+
+ NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
+ NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
+ return *this;
+ }
+
+ void lock() { mutex.lock(); }
+
+ void unlock() { mutex.unlock(); }
+
+ bool try_lock() noexcept { return mutex.try_lock(); }
+ };
+
+ mutable NonMovableMutexWrapper m_mutex;
+
+ // CRC-16 Look-Up Table
+ static constexpr uint16_t adiscrc[256] = {
+ 0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF, 0x1F3F,
+ 0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890, 0x1E3D, 0x09F3,
+ 0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992, 0x0102, 0x16CC, 0x0EDD,
+ 0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD, 0x1C39, 0x0BF7, 0x13E6, 0x0428,
+ 0x0387, 0x1449, 0x0C58, 0x1B96, 0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8,
+ 0x0B76, 0x1367, 0x04A9, 0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74,
+ 0x1265, 0x05AB, 0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A,
+ 0x1A94, 0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
+ 0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1, 0x060C,
+ 0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3, 0x1933, 0x0EFD,
+ 0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C, 0x0408, 0x13C6, 0x0BD7,
+ 0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7, 0x1B37, 0x0CF9, 0x14E8, 0x0326,
+ 0x0489, 0x1347, 0x0B56, 0x1C98, 0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B,
+ 0x1245, 0x0A54, 0x1D9A, 0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A,
+ 0x156B, 0x02A5, 0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040,
+ 0x178E, 0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
+ 0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3, 0x1123,
+ 0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C, 0x0C18, 0x1BD6,
+ 0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7, 0x1327, 0x04E9, 0x1CF8,
+ 0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488, 0x1225, 0x05EB, 0x1DFA, 0x0A34,
+ 0x0D9B, 0x1A55, 0x0244, 0x158A, 0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4,
+ 0x056A, 0x1D7B, 0x0AB5, 0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060,
+ 0x1871, 0x0FBF, 0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E,
+ 0x1080, 0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
+ 0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD, 0x1429,
+ 0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386, 0x0B16, 0x1CD8,
+ 0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9, 0x0A14, 0x1DDA, 0x05CB,
+ 0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB, 0x152B, 0x02E5, 0x1AF4, 0x0D3A,
+ 0x0A95, 0x1D5B, 0x054A, 0x1284};
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
new file mode 100644
index 0000000..e6f3ce7
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
@@ -0,0 +1,417 @@
+// 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.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. 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. */
+/* */
+/* Juan Chong - frcsupport@analog.com */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+#include <atomic>
+#include <memory>
+#include <thread>
+
+#include <hal/SimDevice.h>
+#include <networktables/NTSendable.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+#include "frc/DigitalSource.h"
+#include "frc/SPI.h"
+
+namespace frc {
+/**
+ * Use DMA SPI to read rate and acceleration data from the ADIS16470 IMU and
+ * return the robot's heading relative to a starting position and instant
+ * measurements
+ *
+ * The ADIS16470 gyro angle outputs track the robot's heading based on the
+ * starting position. As the robot rotates the new heading is computed by
+ * integrating the rate of rotation returned by the IMU. When the class is
+ * instantiated, a short calibration routine is performed where the IMU samples
+ * the gyros while at rest to determine the initial offset. This is subtracted
+ * from each sample to determine the heading.
+ *
+ * This class is for the ADIS16470 IMU connected via the primary SPI port
+ * available on the RoboRIO.
+ */
+
+class ADIS16470_IMU : public nt::NTSendable,
+ public wpi::SendableHelper<ADIS16470_IMU> {
+ public:
+ /* ADIS16470 Calibration Time Enum Class */
+ enum class CalibrationTime {
+ _32ms = 0,
+ _64ms = 1,
+ _128ms = 2,
+ _256ms = 3,
+ _512ms = 4,
+ _1s = 5,
+ _2s = 6,
+ _4s = 7,
+ _8s = 8,
+ _16s = 9,
+ _32s = 10,
+ _64s = 11
+ };
+
+ enum IMUAxis { kX, kY, kZ };
+
+ /**
+ * @brief Default constructor. Uses CS0 on the 10-pin SPI port, the yaw axis
+ * is set to the IMU Z axis, and calibration time is defaulted to 4 seconds.
+ */
+ ADIS16470_IMU();
+
+ /**
+ * @brief Customizable constructor. Allows the SPI port and CS to be
+ * customized, the yaw axis used for GetAngle() is adjustable, and initial
+ * calibration time can be modified.
+ *
+ * @param yaw_axis Selects the "default" axis to use for GetAngle() and
+ * GetRate()
+ *
+ * @param port The SPI port and CS where the IMU is connected.
+ *
+ * @param cal_time The calibration time that should be used on start-up.
+ */
+ explicit ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
+ CalibrationTime cal_time);
+
+ /**
+ * @brief Destructor. Kills the acquisiton loop and closes the SPI peripheral.
+ */
+ ~ADIS16470_IMU() override;
+
+ ADIS16470_IMU(ADIS16470_IMU&&) = default;
+ ADIS16470_IMU& operator=(ADIS16470_IMU&&) = default;
+
+ int ConfigDecRate(uint16_t reg);
+
+ /**
+ * @brief Switches the active SPI port to standard SPI mode, writes the
+ * command to activate the new null configuration, and re-enables auto SPI.
+ */
+ void Calibrate();
+
+ /**
+ * @brief Switches the active SPI port to standard SPI mode, writes a new
+ * value to the NULL_CNFG register in the IMU, and re-enables auto SPI.
+ */
+ int ConfigCalTime(CalibrationTime new_cal_time);
+
+ /**
+ * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
+ *
+ * Resets the gyro accumulations to a heading of zero. This can be used if
+ * the "zero" orientation of the sensor needs to be changed in runtime.
+ */
+ void Reset();
+
+ /**
+ * Returns the yaw axis angle in degrees (CCW positive).
+ */
+ units::degree_t GetAngle() const;
+
+ /**
+ * Returns the yaw axis angular rate in degrees per second (CCW positive).
+ */
+ units::degrees_per_second_t GetRate() const;
+
+ /**
+ * Returns the acceleration in the X axis.
+ */
+ units::meters_per_second_squared_t GetAccelX() const;
+
+ /**
+ * Returns the acceleration in the Y axis.
+ */
+ units::meters_per_second_squared_t GetAccelY() const;
+
+ /**
+ * Returns the acceleration in the Z axis.
+ */
+ units::meters_per_second_squared_t GetAccelZ() const;
+
+ units::degree_t GetXComplementaryAngle() const;
+
+ units::degree_t GetYComplementaryAngle() const;
+
+ units::degree_t GetXFilteredAccelAngle() const;
+
+ units::degree_t GetYFilteredAccelAngle() const;
+
+ IMUAxis GetYawAxis() const;
+
+ int SetYawAxis(IMUAxis yaw_axis);
+
+ // IMU yaw axis
+ IMUAxis m_yaw_axis;
+
+ /**
+ * Get the SPI port number.
+ *
+ * @return The SPI port number.
+ */
+ int GetPort() const;
+
+ void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+ /* ADIS16470 Register Map Declaration */
+ static constexpr uint8_t FLASH_CNT = 0x00; // Flash memory write count
+ static constexpr uint8_t DIAG_STAT =
+ 0x02; // Diagnostic and operational status
+ static constexpr uint8_t X_GYRO_LOW =
+ 0x04; // X-axis gyroscope output, lower word
+ static constexpr uint8_t X_GYRO_OUT =
+ 0x06; // X-axis gyroscope output, upper word
+ static constexpr uint8_t Y_GYRO_LOW =
+ 0x08; // Y-axis gyroscope output, lower word
+ static constexpr uint8_t Y_GYRO_OUT =
+ 0x0A; // Y-axis gyroscope output, upper word
+ static constexpr uint8_t Z_GYRO_LOW =
+ 0x0C; // Z-axis gyroscope output, lower word
+ static constexpr uint8_t Z_GYRO_OUT =
+ 0x0E; // Z-axis gyroscope output, upper word
+ static constexpr uint8_t X_ACCL_LOW =
+ 0x10; // X-axis accelerometer output, lower word
+ static constexpr uint8_t X_ACCL_OUT =
+ 0x12; // X-axis accelerometer output, upper word
+ static constexpr uint8_t Y_ACCL_LOW =
+ 0x14; // Y-axis accelerometer output, lower word
+ static constexpr uint8_t Y_ACCL_OUT =
+ 0x16; // Y-axis accelerometer output, upper word
+ static constexpr uint8_t Z_ACCL_LOW =
+ 0x18; // Z-axis accelerometer output, lower word
+ static constexpr uint8_t Z_ACCL_OUT =
+ 0x1A; // Z-axis accelerometer output, upper word
+ static constexpr uint8_t TEMP_OUT =
+ 0x1C; // Temperature output (internal, not calibrated)
+ static constexpr uint8_t TIME_STAMP = 0x1E; // PPS mode time stamp
+ static constexpr uint8_t X_DELTANG_LOW =
+ 0x24; // X-axis delta angle output, lower word
+ static constexpr uint8_t X_DELTANG_OUT =
+ 0x26; // X-axis delta angle output, upper word
+ static constexpr uint8_t Y_DELTANG_LOW =
+ 0x28; // Y-axis delta angle output, lower word
+ static constexpr uint8_t Y_DELTANG_OUT =
+ 0x2A; // Y-axis delta angle output, upper word
+ static constexpr uint8_t Z_DELTANG_LOW =
+ 0x2C; // Z-axis delta angle output, lower word
+ static constexpr uint8_t Z_DELTANG_OUT =
+ 0x2E; // Z-axis delta angle output, upper word
+ static constexpr uint8_t X_DELTVEL_LOW =
+ 0x30; // X-axis delta velocity output, lower word
+ static constexpr uint8_t X_DELTVEL_OUT =
+ 0x32; // X-axis delta velocity output, upper word
+ static constexpr uint8_t Y_DELTVEL_LOW =
+ 0x34; // Y-axis delta velocity output, lower word
+ static constexpr uint8_t Y_DELTVEL_OUT =
+ 0x36; // Y-axis delta velocity output, upper word
+ static constexpr uint8_t Z_DELTVEL_LOW =
+ 0x38; // Z-axis delta velocity output, lower word
+ static constexpr uint8_t Z_DELTVEL_OUT =
+ 0x3A; // Z-axis delta velocity output, upper word
+ static constexpr uint8_t XG_BIAS_LOW =
+ 0x40; // X-axis gyroscope bias offset correction, lower word
+ static constexpr uint8_t XG_BIAS_HIGH =
+ 0x42; // X-axis gyroscope bias offset correction, upper word
+ static constexpr uint8_t YG_BIAS_LOW =
+ 0x44; // Y-axis gyroscope bias offset correction, lower word
+ static constexpr uint8_t YG_BIAS_HIGH =
+ 0x46; // Y-axis gyroscope bias offset correction, upper word
+ static constexpr uint8_t ZG_BIAS_LOW =
+ 0x48; // Z-axis gyroscope bias offset correction, lower word
+ static constexpr uint8_t ZG_BIAS_HIGH =
+ 0x4A; // Z-axis gyroscope bias offset correction, upper word
+ static constexpr uint8_t XA_BIAS_LOW =
+ 0x4C; // X-axis accelerometer bias offset correction, lower word
+ static constexpr uint8_t XA_BIAS_HIGH =
+ 0x4E; // X-axis accelerometer bias offset correction, upper word
+ static constexpr uint8_t YA_BIAS_LOW =
+ 0x50; // Y-axis accelerometer bias offset correction, lower word
+ static constexpr uint8_t YA_BIAS_HIGH =
+ 0x52; // Y-axis accelerometer bias offset correction, upper word
+ static constexpr uint8_t ZA_BIAS_LOW =
+ 0x54; // Z-axis accelerometer bias offset correction, lower word
+ static constexpr uint8_t ZA_BIAS_HIGH =
+ 0x56; // Z-axis accelerometer bias offset correction, upper word
+ static constexpr uint8_t FILT_CTRL = 0x5C; // Filter control
+ static constexpr uint8_t MSC_CTRL = 0x60; // Miscellaneous control
+ static constexpr uint8_t UP_SCALE = 0x62; // Clock scale factor, PPS mode
+ static constexpr uint8_t DEC_RATE =
+ 0x64; // Decimation rate control (output data rate)
+ static constexpr uint8_t NULL_CNFG = 0x66; // Auto-null configuration control
+ static constexpr uint8_t GLOB_CMD = 0x68; // Global commands
+ static constexpr uint8_t FIRM_REV = 0x6C; // Firmware revision
+ static constexpr uint8_t FIRM_DM =
+ 0x6E; // Firmware revision date, month and day
+ static constexpr uint8_t FIRM_Y = 0x70; // Firmware revision date, year
+ static constexpr uint8_t PROD_ID = 0x72; // Product identification
+ static constexpr uint8_t SERIAL_NUM =
+ 0x74; // Serial number (relative to assembly lot)
+ static constexpr uint8_t USER_SCR1 = 0x76; // User scratch register 1
+ static constexpr uint8_t USER_SCR2 = 0x78; // User scratch register 2
+ static constexpr uint8_t USER_SCR3 = 0x7A; // User scratch register 3
+ static constexpr uint8_t FLSHCNT_LOW =
+ 0x7C; // Flash update count, lower word
+ static constexpr uint8_t FLSHCNT_HIGH =
+ 0x7E; // Flash update count, upper word
+
+ /* ADIS16470 Auto SPI Data Packets */
+ static constexpr uint8_t m_autospi_x_packet[16] = {
+ X_DELTANG_OUT, FLASH_CNT, X_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+ Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+ Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
+
+ static constexpr uint8_t m_autospi_y_packet[16] = {
+ Y_DELTANG_OUT, FLASH_CNT, Y_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+ Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+ Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
+
+ static constexpr uint8_t m_autospi_z_packet[16] = {
+ Z_DELTANG_OUT, FLASH_CNT, Z_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
+ Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
+ Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
+
+ /* ADIS16470 Constants */
+ static constexpr double delta_angle_sf =
+ 2160.0 / 2147483648.0; /* 2160 / (2^31) */
+ static constexpr double rad_to_deg = 57.2957795;
+ static constexpr double deg_to_rad = 0.0174532;
+ static constexpr double grav = 9.81;
+
+ /**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ * SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ * peripheral in standard SPI mode.
+ */
+ bool SwitchToStandardSPI();
+
+ /**
+ * @brief Switches to auto SPI operation. Primarily used when exiting standard
+ * SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ * peripheral in auto SPI mode.
+ */
+ bool SwitchToAutoSPI();
+
+ /**
+ * @brief Reads the contents of a specified register location over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @return An unsigned, 16-bit number representing the contents of the
+ * requested register location.
+ */
+ uint16_t ReadRegister(uint8_t reg);
+
+ /**
+ * @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
+ * locations over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @param val An unsigned, 16-bit value to be written to the specified
+ * register location.
+ */
+ void WriteRegister(uint8_t reg, uint16_t val);
+
+ /**
+ * @brief Main acquisition loop. Typically called asynchronously and
+ * free-wheels while the robot code is active.
+ */
+ void Acquire();
+
+ void Close();
+
+ // Integrated gyro value
+ double m_integ_angle = 0.0;
+
+ // Instant raw outputs
+ double m_gyro_rate_x = 0.0;
+ double m_gyro_rate_y = 0.0;
+ double m_gyro_rate_z = 0.0;
+ double m_accel_x = 0.0;
+ double m_accel_y = 0.0;
+ double m_accel_z = 0.0;
+
+ // Complementary filter variables
+ double m_tau = 1.0;
+ double m_dt, m_alpha = 0.0;
+ double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
+
+ // Complementary filter functions
+ double FormatFastConverge(double compAngle, double accAngle);
+
+ double FormatRange0to2PI(double compAngle);
+
+ double FormatAccelRange(double accelAngle, double accelZ);
+
+ double CompFilterProcess(double compAngle, double accelAngle, double omega);
+
+ // State and resource variables
+ volatile bool m_thread_active = false;
+ volatile bool m_first_run = true;
+ volatile bool m_thread_idle = false;
+ bool m_auto_configured = false;
+ SPI::Port m_spi_port;
+ uint16_t m_calibration_time = 0;
+ SPI* m_spi = nullptr;
+ DigitalInput* m_auto_interrupt = nullptr;
+ double m_scaled_sample_rate = 2500.0; // Default sample rate setting
+
+ std::thread m_acquire_task;
+
+ hal::SimDevice m_simDevice;
+ hal::SimDouble m_simGyroAngleX;
+ hal::SimDouble m_simGyroAngleY;
+ hal::SimDouble m_simGyroAngleZ;
+ hal::SimDouble m_simGyroRateX;
+ hal::SimDouble m_simGyroRateY;
+ hal::SimDouble m_simGyroRateZ;
+ hal::SimDouble m_simAccelX;
+ hal::SimDouble m_simAccelY;
+ hal::SimDouble m_simAccelZ;
+
+ struct NonMovableMutexWrapper {
+ wpi::mutex mutex;
+ NonMovableMutexWrapper() = default;
+
+ NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
+ NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
+
+ NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
+ NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
+ return *this;
+ }
+
+ void lock() { mutex.lock(); }
+
+ void unlock() { mutex.unlock(); }
+
+ bool try_lock() noexcept { return mutex.try_lock(); }
+ };
+
+ mutable NonMovableMutexWrapper m_mutex;
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Compressor.h b/wpilibc/src/main/native/include/frc/Compressor.h
index cc065ff..d8125ed 100644
--- a/wpilibc/src/main/native/include/frc/Compressor.h
+++ b/wpilibc/src/main/native/include/frc/Compressor.h
@@ -7,9 +7,11 @@
#include <memory>
#include <hal/Types.h>
+#include <wpi/deprecated.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
+#include "frc/CompressorConfigType.h"
#include "frc/PneumaticsBase.h"
#include "frc/PneumaticsModuleType.h"
#include "frc/SensorUtil.h"
@@ -59,13 +61,19 @@
/**
* Starts closed-loop control. Note that closed loop control is enabled by
* default.
+ *
+ * @deprecated Use EnableDigital() instead.
*/
+ WPI_DEPRECATED("Use EnableDigital() instead")
void Start();
/**
* Stops closed-loop control. Note that closed loop control is enabled by
* default.
+ *
+ * @deprecated Use Disable() instead.
*/
+ WPI_DEPRECATED("Use Disable() instead")
void Stop();
/**
@@ -87,25 +95,58 @@
*
* @return The current through the compressor, in amps
*/
- double GetCompressorCurrent() const;
+ units::ampere_t GetCurrent() const;
/**
- * Enables or disables automatically turning the compressor on when the
- * pressure is low.
+ * Query the analog input voltage (on channel 0) (if supported).
*
- * @param on Set to true to enable closed loop control of the compressor.
- * False to disable.
+ * @return The analog input voltage, in volts
*/
- void SetClosedLoopControl(bool on);
+ units::volt_t GetAnalogVoltage() const;
/**
- * Returns true if the compressor will automatically turn on when the
- * pressure is low.
+ * Query the analog sensor pressure (on channel 0) (if supported). Note this
+ * is only for use with the REV Analog Pressure Sensor.
*
- * @return True if closed loop control of the compressor is enabled. False if
- * disabled.
+ * @return The analog sensor pressure, in PSI
*/
- bool GetClosedLoopControl() const;
+ units::pounds_per_square_inch_t GetPressure() const;
+
+ /**
+ * Disable the compressor.
+ */
+ void Disable();
+
+ /**
+ * Enable compressor closed loop control using digital input.
+ */
+ void EnableDigital();
+
+ /**
+ * Enable compressor closed loop control using analog input. Note this is only
+ * for use with the REV Analog Pressure Sensor.
+ *
+ * <p>On CTRE PCM, this will enable digital control.
+ *
+ * @param minPressure The minimum pressure in PSI to enable compressor
+ * @param maxPressure The maximum pressure in PSI to disable compressor
+ */
+ void EnableAnalog(units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure);
+
+ /**
+ * Enable compressor closed loop control using hybrid input. Note this is only
+ * for use with the REV Analog Pressure Sensor.
+ *
+ * On CTRE PCM, this will enable digital control.
+ *
+ * @param minPressure The minimum pressure in PSI to enable compressor
+ * @param maxPressure The maximum pressure in PSI to disable compressor
+ */
+ void EnableHybrid(units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure);
+
+ CompressorConfigType GetConfigType() const;
void InitSendable(wpi::SendableBuilder& builder) override;
diff --git a/wpilibc/src/main/native/include/frc/CompressorConfigType.h b/wpilibc/src/main/native/include/frc/CompressorConfigType.h
new file mode 100644
index 0000000..5a5a1c1
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/CompressorConfigType.h
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace frc {
+enum class CompressorConfigType {
+ Disabled = 0,
+ Digital = 1,
+ Analog = 2,
+ Hybrid = 3
+};
+
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Debouncer.h b/wpilibc/src/main/native/include/frc/Debouncer.h
deleted file mode 100644
index 8f583f0..0000000
--- a/wpilibc/src/main/native/include/frc/Debouncer.h
+++ /dev/null
@@ -1,46 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-#pragma once
-
-#include <units/time.h>
-
-#include "frc/Timer.h"
-
-namespace frc {
-/**
- * A simple debounce filter for boolean streams. Requires that the boolean
- * change value from baseline for a specified period of time before the filtered
- * value changes.
- */
-class Debouncer {
- public:
- enum DebounceType { kRising, kFalling, kBoth };
-
- /**
- * Creates a new Debouncer.
- *
- * @param debounceTime The number of seconds the value must change from
- * baseline for the filtered value to change.
- * @param type Which type of state change the debouncing will be
- * performed on.
- */
- explicit Debouncer(units::second_t debounceTime,
- DebounceType type = DebounceType::kRising);
-
- /**
- * Applies the debouncer to the input stream.
- *
- * @param input The current value of the input stream.
- * @return The debounced value of the input stream.
- */
- bool Calculate(bool input);
-
- private:
- frc::Timer m_timer;
- units::second_t m_debounceTime;
- bool m_baseline;
- DebounceType m_debounceType;
-};
-} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
index ccf56ad..ab7ded9 100644
--- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
+++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
@@ -122,6 +122,21 @@
units::turn_t Get() const;
/**
+ * Set the encoder duty cycle range. As the encoder needs to maintain a duty
+ * cycle, the duty cycle cannot go all the way to 0% or all the way to 100%.
+ * For example, an encoder with a 4096 us period might have a minimum duty
+ * cycle of 1 us / 4096 us and a maximum duty cycle of 4095 / 4096 us. Setting
+ * the range will result in an encoder duty cycle less than or equal to the
+ * minimum being output as 0 rotation, the duty cycle greater than or equal to
+ * the maximum being output as 1 rotation, and values in between linearly
+ * scaled from 0 to 1.
+ *
+ * @param min minimum duty cycle (0-1 range)
+ * @param max maximum duty cycle (0-1 range)
+ */
+ void SetDutyCycleRange(double min, double max);
+
+ /**
* Set the distance per rotation of the encoder. This sets the multiplier used
* to determine the distance driven based on the rotation value from the
* encoder. Set this value based on the how far the mechanism travels in 1
@@ -175,6 +190,8 @@
double m_positionOffset = 0;
double m_distancePerRotation = 1.0;
mutable units::turn_t m_lastPosition{0.0};
+ double m_sensorMin = 0;
+ double m_sensorMax = 1;
hal::SimDevice m_simDevice;
hal::SimDouble m_simPosition;
diff --git a/wpilibc/src/main/native/include/frc/MotorSafety.h b/wpilibc/src/main/native/include/frc/MotorSafety.h
index f84af17..1bfd34a 100644
--- a/wpilibc/src/main/native/include/frc/MotorSafety.h
+++ b/wpilibc/src/main/native/include/frc/MotorSafety.h
@@ -91,6 +91,12 @@
static void CheckMotors();
virtual void StopMotor() = 0;
+
+ /**
+ * The return value from this method is printed out when an error occurs
+ *
+ * This method must not throw!
+ */
virtual std::string GetDescription() const = 0;
private:
diff --git a/wpilibc/src/main/native/include/frc/PneumaticHub.h b/wpilibc/src/main/native/include/frc/PneumaticHub.h
index 857ed51..d412bb6 100644
--- a/wpilibc/src/main/native/include/frc/PneumaticHub.h
+++ b/wpilibc/src/main/native/include/frc/PneumaticHub.h
@@ -22,13 +22,23 @@
bool GetCompressor() const override;
- void SetClosedLoopControl(bool enabled) override;
+ void DisableCompressor() override;
- bool GetClosedLoopControl() const override;
+ void EnableCompressorDigital() override;
+
+ void EnableCompressorAnalog(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) override;
+
+ void EnableCompressorHybrid(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) override;
+
+ CompressorConfigType GetCompressorConfigType() const override;
bool GetPressureSwitch() const override;
- double GetCompressorCurrent() const override;
+ units::ampere_t GetCompressorCurrent() const override;
void SetSolenoids(int mask, int values) override;
@@ -57,6 +67,70 @@
int reverseChannel) override;
Compressor MakeCompressor() override;
+ struct Version {
+ uint32_t FirmwareMajor;
+ uint32_t FirmwareMinor;
+ uint32_t FirmwareFix;
+ uint32_t HardwareMinor;
+ uint32_t HardwareMajor;
+ uint32_t UniqueId;
+ };
+
+ Version GetVersion() const;
+
+ struct Faults {
+ uint32_t Channel0Fault : 1;
+ uint32_t Channel1Fault : 1;
+ uint32_t Channel2Fault : 1;
+ uint32_t Channel3Fault : 1;
+ uint32_t Channel4Fault : 1;
+ uint32_t Channel5Fault : 1;
+ uint32_t Channel6Fault : 1;
+ uint32_t Channel7Fault : 1;
+ uint32_t Channel8Fault : 1;
+ uint32_t Channel9Fault : 1;
+ uint32_t Channel10Fault : 1;
+ uint32_t Channel11Fault : 1;
+ uint32_t Channel12Fault : 1;
+ uint32_t Channel13Fault : 1;
+ uint32_t Channel14Fault : 1;
+ uint32_t Channel15Fault : 1;
+ uint32_t CompressorOverCurrent : 1;
+ uint32_t CompressorOpen : 1;
+ uint32_t SolenoidOverCurrent : 1;
+ uint32_t Brownout : 1;
+ uint32_t CanWarning : 1;
+ uint32_t HardwareFault : 1;
+ };
+
+ Faults GetFaults() const;
+
+ struct StickyFaults {
+ uint32_t CompressorOverCurrent : 1;
+ uint32_t CompressorOpen : 1;
+ uint32_t SolenoidOverCurrent : 1;
+ uint32_t Brownout : 1;
+ uint32_t CanWarning : 1;
+ uint32_t CanBusOff : 1;
+ uint32_t HasReset : 1;
+ };
+
+ StickyFaults GetStickyFaults() const;
+
+ void ClearStickyFaults();
+
+ units::volt_t GetInputVoltage() const;
+
+ units::volt_t Get5VRegulatedVoltage() const;
+
+ units::ampere_t GetSolenoidsTotalCurrent() const;
+
+ units::volt_t GetSolenoidsVoltage() const;
+
+ units::volt_t GetAnalogVoltage(int channel) const override;
+
+ units::pounds_per_square_inch_t GetPressure(int channel) const override;
+
private:
class DataStore;
friend class DataStore;
diff --git a/wpilibc/src/main/native/include/frc/PneumaticsBase.h b/wpilibc/src/main/native/include/frc/PneumaticsBase.h
index 06cad5d..50ceb87 100644
--- a/wpilibc/src/main/native/include/frc/PneumaticsBase.h
+++ b/wpilibc/src/main/native/include/frc/PneumaticsBase.h
@@ -6,8 +6,12 @@
#include <memory>
+#include <units/current.h>
+#include <units/pressure.h>
#include <units/time.h>
+#include <units/voltage.h>
+#include "frc/CompressorConfigType.h"
#include "frc/PneumaticsModuleType.h"
namespace frc {
@@ -22,11 +26,21 @@
virtual bool GetPressureSwitch() const = 0;
- virtual double GetCompressorCurrent() const = 0;
+ virtual units::ampere_t GetCompressorCurrent() const = 0;
- virtual void SetClosedLoopControl(bool on) = 0;
+ virtual void DisableCompressor() = 0;
- virtual bool GetClosedLoopControl() const = 0;
+ virtual void EnableCompressorDigital() = 0;
+
+ virtual void EnableCompressorAnalog(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) = 0;
+
+ virtual void EnableCompressorHybrid(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) = 0;
+
+ virtual CompressorConfigType GetCompressorConfigType() const = 0;
virtual void SetSolenoids(int mask, int values) = 0;
@@ -50,6 +64,10 @@
virtual void UnreserveCompressor() = 0;
+ virtual units::volt_t GetAnalogVoltage(int channel) const = 0;
+
+ virtual units::pounds_per_square_inch_t GetPressure(int channel) const = 0;
+
virtual Solenoid MakeSolenoid(int channel) = 0;
virtual DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
int reverseChannel) = 0;
diff --git a/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h b/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h
index 64686d6..ea4517b 100644
--- a/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h
+++ b/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h
@@ -22,13 +22,23 @@
bool GetCompressor() const override;
- void SetClosedLoopControl(bool enabled) override;
+ void DisableCompressor() override;
- bool GetClosedLoopControl() const override;
+ void EnableCompressorDigital() override;
+
+ void EnableCompressorAnalog(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) override;
+
+ void EnableCompressorHybrid(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) override;
+
+ CompressorConfigType GetCompressorConfigType() const override;
bool GetPressureSwitch() const override;
- double GetCompressorCurrent() const override;
+ units::ampere_t GetCompressorCurrent() const override;
bool GetCompressorCurrentTooHighFault() const;
bool GetCompressorCurrentTooHighStickyFault() const;
@@ -64,6 +74,10 @@
void UnreserveCompressor() override;
+ units::volt_t GetAnalogVoltage(int channel) const override;
+
+ units::pounds_per_square_inch_t GetPressure(int channel) const override;
+
Solenoid MakeSolenoid(int channel) override;
DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
int reverseChannel) override;
diff --git a/wpilibc/src/main/native/include/frc/PowerDistribution.h b/wpilibc/src/main/native/include/frc/PowerDistribution.h
index 2863daf..763e6bf 100644
--- a/wpilibc/src/main/native/include/frc/PowerDistribution.h
+++ b/wpilibc/src/main/native/include/frc/PowerDistribution.h
@@ -12,25 +12,26 @@
/**
* Class for getting voltage, current, temperature, power and energy from the
- * CAN PDP.
+ * CTRE Power Distribution Panel (PDP) or REV Power Distribution Hub (PDH).
*/
class PowerDistribution : public wpi::Sendable,
public wpi::SendableHelper<PowerDistribution> {
public:
static constexpr int kDefaultModule = -1;
- enum class ModuleType { kAutomatic = 0, kCTRE = 1, kRev = 2 };
+ enum class ModuleType { kCTRE = 1, kRev = 2 };
/**
- * Constructs a PowerDistribution.
+ * Constructs a PowerDistribution object.
*
- * Uses the default CAN ID.
+ * Detects the connected PDP/PDH using the default CAN ID (0 for CTRE and 1
+ * for REV).
*/
PowerDistribution();
/**
- * Constructs a PowerDistribution.
+ * Constructs a PowerDistribution object.
*
- * @param module The CAN ID of the PDP
+ * @param module The CAN ID of the PDP/PDH
* @param moduleType The type of module
*/
PowerDistribution(int module, ModuleType moduleType);
@@ -40,56 +41,57 @@
PowerDistribution& operator=(PowerDistribution&&) = default;
/**
- * Query the input voltage of the PDP.
+ * Query the input voltage of the PDP/PDH.
*
- * @return The voltage of the PDP in volts
+ * @return The input voltage in volts
*/
double GetVoltage() const;
/**
- * Query the temperature of the PDP.
+ * Query the temperature of the PDP/PDH.
*
- * @return The temperature of the PDP in degrees Celsius
+ * @return The temperature in degrees Celsius
*/
double GetTemperature() const;
/**
- * Query the current of a single channel of the PDP.
+ * Query the current of a single channel of the PDP/PDH.
*
- * @return The current of one of the PDP channels (channels 0-15) in Amperes
+ * @param channel the channel to query (0-15 for PDP, 0-23 for PDH)
+ * @return The current of the channel in Amperes
*/
double GetCurrent(int channel) const;
/**
- * Query the total current of all monitored PDP channels (0-15).
+ * Query the total current of all monitored PDP/PDH channels.
*
- * @return The the total current drawn from the PDP channels in Amperes
+ * @return The total current drawn from all channels in Amperes
*/
double GetTotalCurrent() const;
/**
- * Query the total power drawn from the monitored PDP channels.
+ * Query the total power drawn from all monitored PDP/PDH channels.
*
- * @return The the total power drawn from the PDP channels in Watts
+ * @return The total power drawn in Watts
*/
double GetTotalPower() const;
/**
- * Query the total energy drawn from the monitored PDP channels.
+ * Query the total energy drawn from the monitored PDP/PDH channels.
*
- * @return The the total energy drawn from the PDP channels in Joules
+ * @return The total energy drawn in Joules
*/
double GetTotalEnergy() const;
/**
- * Reset the total energy drawn from the PDP.
+ * Reset the total energy drawn from the PDP/PDH.
*
* @see PowerDistribution#GetTotalEnergy
*/
void ResetTotalEnergy();
/**
- * Remove all of the fault flags on the PDP.
+ * Remove all of the fault flags on the PDP/PDH.
*/
void ClearStickyFaults();
@@ -98,10 +100,102 @@
*/
int GetModule() const;
+ /**
+ * Gets module type.
+ */
+ ModuleType GetType() const;
+
+ /**
+ * Gets whether the PDH switchable channel is turned on or off. Returns false
+ * with the CTRE PDP.
+ *
+ * @return The output state of the PDH switchable channel
+ */
bool GetSwitchableChannel() const;
+ /**
+ * Sets the PDH switchable channel on or off. Does nothing with the CTRE PDP.
+ *
+ * @param enabled Whether to turn the PDH switchable channel on or off
+ */
void SetSwitchableChannel(bool enabled);
+ struct Version {
+ uint32_t FirmwareMajor;
+ uint32_t FirmwareMinor;
+ uint32_t FirmwareFix;
+ uint32_t HardwareMinor;
+ uint32_t HardwareMajor;
+ uint32_t UniqueId;
+ };
+
+ Version GetVersion() const;
+
+ struct Faults {
+ uint32_t Channel0BreakerFault : 1;
+ uint32_t Channel1BreakerFault : 1;
+ uint32_t Channel2BreakerFault : 1;
+ uint32_t Channel3BreakerFault : 1;
+ uint32_t Channel4BreakerFault : 1;
+ uint32_t Channel5BreakerFault : 1;
+ uint32_t Channel6BreakerFault : 1;
+ uint32_t Channel7BreakerFault : 1;
+ uint32_t Channel8BreakerFault : 1;
+ uint32_t Channel9BreakerFault : 1;
+ uint32_t Channel10BreakerFault : 1;
+ uint32_t Channel11BreakerFault : 1;
+ uint32_t Channel12BreakerFault : 1;
+ uint32_t Channel13BreakerFault : 1;
+ uint32_t Channel14BreakerFault : 1;
+ uint32_t Channel15BreakerFault : 1;
+ uint32_t Channel16BreakerFault : 1;
+ uint32_t Channel17BreakerFault : 1;
+ uint32_t Channel18BreakerFault : 1;
+ uint32_t Channel19BreakerFault : 1;
+ uint32_t Channel20BreakerFault : 1;
+ uint32_t Channel21BreakerFault : 1;
+ uint32_t Channel22BreakerFault : 1;
+ uint32_t Channel23BreakerFault : 1;
+ uint32_t Brownout : 1;
+ uint32_t CanWarning : 1;
+ uint32_t HardwareFault : 1;
+ };
+
+ Faults GetFaults() const;
+
+ struct StickyFaults {
+ uint32_t Channel0BreakerFault : 1;
+ uint32_t Channel1BreakerFault : 1;
+ uint32_t Channel2BreakerFault : 1;
+ uint32_t Channel3BreakerFault : 1;
+ uint32_t Channel4BreakerFault : 1;
+ uint32_t Channel5BreakerFault : 1;
+ uint32_t Channel6BreakerFault : 1;
+ uint32_t Channel7BreakerFault : 1;
+ uint32_t Channel8BreakerFault : 1;
+ uint32_t Channel9BreakerFault : 1;
+ uint32_t Channel10BreakerFault : 1;
+ uint32_t Channel11BreakerFault : 1;
+ uint32_t Channel12BreakerFault : 1;
+ uint32_t Channel13BreakerFault : 1;
+ uint32_t Channel14BreakerFault : 1;
+ uint32_t Channel15BreakerFault : 1;
+ uint32_t Channel16BreakerFault : 1;
+ uint32_t Channel17BreakerFault : 1;
+ uint32_t Channel18BreakerFault : 1;
+ uint32_t Channel19BreakerFault : 1;
+ uint32_t Channel20BreakerFault : 1;
+ uint32_t Channel21BreakerFault : 1;
+ uint32_t Channel22BreakerFault : 1;
+ uint32_t Channel23BreakerFault : 1;
+ uint32_t Brownout : 1;
+ uint32_t CanWarning : 1;
+ uint32_t CanBusOff : 1;
+ uint32_t HasReset : 1;
+ };
+
+ StickyFaults GetStickyFaults() const;
+
void InitSendable(wpi::SendableBuilder& builder) override;
private:
diff --git a/wpilibc/src/main/native/include/frc/SPI.h b/wpilibc/src/main/native/include/frc/SPI.h
index 36c82e4..4f12cbe 100644
--- a/wpilibc/src/main/native/include/frc/SPI.h
+++ b/wpilibc/src/main/native/include/frc/SPI.h
@@ -35,7 +35,7 @@
*/
explicit SPI(Port port);
- ~SPI();
+ virtual ~SPI();
SPI(SPI&&) = default;
SPI& operator=(SPI&&) = default;
diff --git a/wpilibc/src/main/native/include/frc/Servo.h b/wpilibc/src/main/native/include/frc/Servo.h
index b1df655..96cbfa4 100644
--- a/wpilibc/src/main/native/include/frc/Servo.h
+++ b/wpilibc/src/main/native/include/frc/Servo.h
@@ -46,7 +46,8 @@
* Get the servo position.
*
* Servo values range from 0.0 to 1.0 corresponding to the range of full left
- * to full right.
+ * to full right. This returns the commanded position, not the position that
+ * the servo is actually at, as the servo does not report its own position.
*
* @return Position from 0.0 to 1.0.
*/
@@ -55,8 +56,8 @@
/**
* Set the servo angle.
*
- * Assume that the servo angle is linear with respect to the PWM value (big
- * assumption, need to test).
+ * The angles are based on the HS-322HD Servo, and have a range of 0 to 180
+ * degrees.
*
* Servo angles that are out of the supported range of the servo simply
* "saturate" in that direction. In other words, if the servo has a range of
@@ -71,8 +72,8 @@
/**
* Get the servo angle.
*
- * Assume that the servo angle is linear with respect to the PWM value (big
- * assumption, need to test).
+ * This returns the commanded angle, not the angle that the servo is actually
+ * at, as the servo does not report its own angle.
*
* @return The angle in degrees to which the servo is set.
*/
diff --git a/wpilibc/src/main/native/include/frc/WPIWarnings.mac b/wpilibc/src/main/native/include/frc/WPIWarnings.mac
index fa63494..6ac2c11 100644
--- a/wpilibc/src/main/native/include/frc/WPIWarnings.mac
+++ b/wpilibc/src/main/native/include/frc/WPIWarnings.mac
@@ -10,7 +10,7 @@
S(NonBinaryDigitalValue, 5, "Digital output value is not 0 or 1")
S(IncorrectBatteryChannel, 6,
"Battery measurement channel is not correct value")
-S(BadJoystickIndex, 7, "Joystick index is out of range, should be 0-3")
+S(BadJoystickIndex, 7, "Joystick index is out of range, should be 0-5")
S(BadJoystickAxis, 8, "Joystick axis or POV is out of range")
S(InvalidMotorIndex, 9, "Motor index is out of range, should be 0-3")
S(DriverStationTaskError, 10, "Driver Station task won't start")
diff --git a/wpilibc/src/main/native/include/frc/counter/EdgeConfiguration.h b/wpilibc/src/main/native/include/frc/counter/EdgeConfiguration.h
new file mode 100644
index 0000000..508117d
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/counter/EdgeConfiguration.h
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace frc {
+enum class EdgeConfiguration {
+ kNone = 0,
+ kRisingEdge = 0x1,
+ kFallingEdge = 0x2,
+ kBoth = 0x3
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/counter/ExternalDirectionCounter.h b/wpilibc/src/main/native/include/frc/counter/ExternalDirectionCounter.h
new file mode 100644
index 0000000..4d9faf2
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/counter/ExternalDirectionCounter.h
@@ -0,0 +1,84 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "EdgeConfiguration.h"
+
+namespace frc {
+class DigitalSource;
+
+/** Counter using external direction.
+ *
+ * <p>This counts on an edge from one digital input and the whether it counts
+ * up or down based on the state of a second digital input.
+ *
+ */
+class ExternalDirectionCounter
+ : public wpi::Sendable,
+ public wpi::SendableHelper<ExternalDirectionCounter> {
+ public:
+ /**
+ * Constructs a new ExternalDirectionCounter.
+ *
+ * @param countSource The source for counting.
+ * @param directionSource The source for selecting count direction.
+ */
+ ExternalDirectionCounter(DigitalSource& countSource,
+ DigitalSource& directionSource);
+
+ /**
+ * Constructs a new ExternalDirectionCounter.
+ *
+ * @param countSource The source for counting.
+ * @param directionSource The source for selecting count direction.
+ */
+ ExternalDirectionCounter(std::shared_ptr<DigitalSource> countSource,
+ std::shared_ptr<DigitalSource> directionSource);
+
+ ~ExternalDirectionCounter() override;
+
+ ExternalDirectionCounter(ExternalDirectionCounter&&) = default;
+ ExternalDirectionCounter& operator=(ExternalDirectionCounter&&) = default;
+
+ /**
+ * Gets the current count.
+ *
+ * @return The current count.
+ */
+ int GetCount() const;
+
+ /**
+ * Sets to reverse the counter direction.
+ *
+ * @param reverseDirection True to reverse counting direction.
+ */
+ void SetReverseDirection(bool reverseDirection);
+
+ /** Resets the current count. */
+ void Reset();
+
+ /**
+ * Sets the edge configuration for counting.
+ *
+ * @param configuration The counting edge configuration.
+ */
+ void SetEdgeConfiguration(EdgeConfiguration configuration);
+
+ protected:
+ void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+ std::shared_ptr<DigitalSource> m_countSource;
+ std::shared_ptr<DigitalSource> m_directionSource;
+ hal::Handle<HAL_CounterHandle> m_handle;
+ int32_t m_index = 0;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/counter/Tachometer.h b/wpilibc/src/main/native/include/frc/counter/Tachometer.h
new file mode 100644
index 0000000..dba1575
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/counter/Tachometer.h
@@ -0,0 +1,140 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+#include <units/angular_velocity.h>
+#include <units/frequency.h>
+#include <units/time.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+class DigitalSource;
+
+/**
+ * Tachometer for getting rotational speed from a device.
+ *
+ * <p>The Tachometer class measures the time between digital pulses to
+ * determine the rotation speed of a mechanism. Examples of devices that could
+ * be used with the tachometer class are a hall effect sensor, break beam
+ * sensor, or optical sensor detecting tape on a shooter wheel. Unlike
+ * encoders, this class only needs a single digital input.
+ */
+class Tachometer : public wpi::Sendable,
+ public wpi::SendableHelper<Tachometer> {
+ public:
+ /**
+ * Constructs a new tachometer.
+ *
+ * @param source The source.
+ */
+ explicit Tachometer(DigitalSource& source);
+
+ /**
+ * Constructs a new tachometer.
+ *
+ * @param source The source.
+ */
+ explicit Tachometer(std::shared_ptr<DigitalSource> source);
+
+ ~Tachometer() override;
+
+ Tachometer(Tachometer&&) = default;
+ Tachometer& operator=(Tachometer&&) = default;
+
+ /**
+ * Gets the tachometer frequency.
+ *
+ * @return Current frequency.
+ */
+ units::hertz_t GetFrequency() const;
+
+ /**
+ * Gets the tachometer period.
+ *
+ * @return Current period.
+ */
+ units::second_t GetPeriod() const;
+
+ /**
+ * Gets the number of edges per revolution.
+ *
+ * @return Edges per revolution.
+ */
+ int GetEdgesPerRevolution() const;
+
+ /**
+ * Sets the number of edges per revolution.
+ *
+ * @param edges Edges per revolution.
+ */
+ void SetEdgesPerRevolution(int edges);
+
+ /**
+ * Gets the current tachometer revolutions per second.
+ *
+ * SetEdgesPerRevolution must be set with a non 0 value for this to work.
+ *
+ * @return Current RPS.
+ */
+ units::turns_per_second_t GetRevolutionsPerSecond() const;
+
+ /**
+ * Gets the current tachometer revolutions per minute.
+ *
+ * SetEdgesPerRevolution must be set with a non 0 value for this to work.
+ *
+ * @return Current RPM.
+ */
+ units::revolutions_per_minute_t GetRevolutionsPerMinute() const;
+
+ /**
+ * Gets if the tachometer is stopped.
+ *
+ * @return True if the tachometer is stopped.
+ */
+ bool GetStopped() const;
+
+ /**
+ * Gets the number of sample to average.
+ *
+ * @return Samples to average.
+ */
+ int GetSamplesToAverage() const;
+
+ /**
+ * Sets the number of samples to average.
+ *
+ * @param samples Samples to average.
+ */
+ void SetSamplesToAverage(int samples);
+
+ /**
+ * Sets the maximum period before the tachometer is considered stopped.
+ *
+ * @param maxPeriod The max period.
+ */
+ void SetMaxPeriod(units::second_t maxPeriod);
+
+ /**
+ * Sets if to update when empty.
+ *
+ * @param updateWhenEmpty True to update when empty.
+ */
+ void SetUpdateWhenEmpty(bool updateWhenEmpty);
+
+ protected:
+ void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+ std::shared_ptr<DigitalSource> m_source;
+ hal::Handle<HAL_CounterHandle> m_handle;
+ int m_edgesPerRevolution;
+ int32_t m_index;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/counter/UpDownCounter.h b/wpilibc/src/main/native/include/frc/counter/UpDownCounter.h
new file mode 100644
index 0000000..b01ce42
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/counter/UpDownCounter.h
@@ -0,0 +1,90 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "EdgeConfiguration.h"
+
+namespace frc {
+class DigitalSource;
+
+/** Up Down Counter.
+ *
+ * This class can count edges on a single digital input or count up based on an
+ * edge from one digital input and down on an edge from another digital input.
+ *
+ */
+class UpDownCounter : public wpi::Sendable,
+ public wpi::SendableHelper<UpDownCounter> {
+ public:
+ /**
+ * Constructs a new UpDown Counter.
+ *
+ * @param upSource The up count source (can be null).
+ * @param downSource The down count source (can be null).
+ */
+ UpDownCounter(DigitalSource& upSource, DigitalSource& downSource);
+
+ /**
+ * Constructs a new UpDown Counter.
+ *
+ * @param upSource The up count source (can be null).
+ * @param downSource The down count source (can be null).
+ */
+ UpDownCounter(std::shared_ptr<DigitalSource> upSource,
+ std::shared_ptr<DigitalSource> downSource);
+
+ ~UpDownCounter() override;
+
+ UpDownCounter(UpDownCounter&&) = default;
+ UpDownCounter& operator=(UpDownCounter&&) = default;
+
+ /**
+ * Gets the current count.
+ *
+ * @return The current count.
+ */
+ int GetCount() const;
+
+ /**
+ * Sets to revert the counter direction.
+ *
+ * @param reverseDirection True to reverse counting direction.
+ */
+ void SetReverseDirection(bool reverseDirection);
+
+ /** Resets the current count. */
+ void Reset();
+
+ /**
+ * Sets the configuration for the up source.
+ *
+ * @param configuration The up source configuration.
+ */
+ void SetUpEdgeConfiguration(EdgeConfiguration configuration);
+
+ /**
+ * Sets the configuration for the down source.
+ *
+ * @param configuration The down source configuration.
+ */
+ void SetDownEdgeConfiguration(EdgeConfiguration configuration);
+
+ protected:
+ void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+ void InitUpDownCounter();
+ std::shared_ptr<DigitalSource> m_upSource;
+ std::shared_ptr<DigitalSource> m_downSource;
+ hal::Handle<HAL_CounterHandle> m_handle;
+ int32_t m_index = 0;
+};
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
index 50a7a3f..7009eb9 100644
--- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
@@ -101,6 +101,11 @@
public wpi::Sendable,
public wpi::SendableHelper<DifferentialDrive> {
public:
+ /**
+ * Wheel speeds for a differential drive.
+ *
+ * Uses normalized voltage [-1.0..1.0].
+ */
struct WheelSpeeds {
double left = 0.0;
double right = 0.0;
@@ -142,10 +147,11 @@
*
* @param xSpeed The robot's speed along the X axis [-1.0..1.0].
* Forward is positive.
- * @param zRotation The robot's rotation rate around the Z axis
- * [-1.0..1.0]. Clockwise is positive.
+ * @param zRotation The normalized curvature [-1.0..1.0]. Clockwise is
+ * positive.
* @param allowTurnInPlace If set, overrides constant-curvature turning for
- * turn-in-place maneuvers.
+ * turn-in-place maneuvers. zRotation will control
+ * turning rate instead of curvature.
*/
void CurvatureDrive(double xSpeed, double zRotation, bool allowTurnInPlace);
@@ -171,6 +177,7 @@
* @param zRotation The rotation rate of the robot around the Z axis
* [-1.0..1.0]. Clockwise is positive.
* @param squareInputs If set, decreases the input sensitivity at low speeds.
+ * @return Wheel speeds [-1.0..1.0].
*/
static WheelSpeeds ArcadeDriveIK(double xSpeed, double zRotation,
bool squareInputs = true);
@@ -184,10 +191,12 @@
*
* @param xSpeed The robot's speed along the X axis [-1.0..1.0].
* Forward is positive.
- * @param zRotation The robot's rotation rate around the Z axis
- * [-1.0..1.0]. Clockwise is positive.
+ * @param zRotation The normalized curvature [-1.0..1.0]. Clockwise is
+ * positive.
* @param allowTurnInPlace If set, overrides constant-curvature turning for
- * turn-in-place maneuvers.
+ * turn-in-place maneuvers. zRotation will control
+ * turning rate instead of curvature.
+ * @return Wheel speeds [-1.0..1.0].
*/
static WheelSpeeds CurvatureDriveIK(double xSpeed, double zRotation,
bool allowTurnInPlace);
@@ -200,6 +209,7 @@
* @param rightSpeed The robot right side's speed along the X axis
* [-1.0..1.0]. Forward is positive.
* @param squareInputs If set, decreases the input sensitivity at low speeds.
+ * @return Wheel speeds [-1.0..1.0].
*/
static WheelSpeeds TankDriveIK(double leftSpeed, double rightSpeed,
bool squareInputs = true);
diff --git a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h b/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
index 1acc69f..d4c5c5c 100644
--- a/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
@@ -62,6 +62,11 @@
static constexpr double kDefaultRightMotorAngle = 120.0;
static constexpr double kDefaultBackMotorAngle = 270.0;
+ /**
+ * Wheel speeds for a Killough drive.
+ *
+ * Uses normalized voltage [-1.0..1.0].
+ */
struct WheelSpeeds {
double left = 0.0;
double right = 0.0;
@@ -154,6 +159,7 @@
* Clockwise is positive.
* @param gyroAngle The current angle reading from the gyro in degrees around
* the Z axis. Use this to implement field-oriented controls.
+ * @return Wheel speeds [-1.0..1.0].
*/
WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed, double zRotation,
double gyroAngle = 0.0);
diff --git a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
index 809e7ea..757ae67 100644
--- a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
@@ -44,25 +44,20 @@
* </pre>
*
* Each Drive() function provides different inverse kinematic relations for a
- * Mecanum drive robot. Motor outputs for the right side are negated, so motor
- * direction inversion by the user is usually unnecessary.
+ * Mecanum drive robot.
*
- * This library uses the NED axes convention (North-East-Down as external
- * reference in the world frame):
- * http://www.nuclearprojects.com/ins/images/axis_big.png.
- *
- * The positive X axis points ahead, the positive Y axis points to the right,
+ * The positive Y axis points ahead, the positive X axis points to the right,
* and the positive Z axis points down. Rotations follow the right-hand rule, so
* clockwise rotation around the Z axis is positive.
*
+ * Note: the axis conventions used in this class differ from DifferentialDrive.
+ * This may change in a future year's WPILib release.
+ *
* Inputs smaller then 0.02 will be set to 0, and larger values will be scaled
* so that the full range is still used. This deadband value can be changed
* with SetDeadband().
*
* RobotDrive porting guide:
- * <br>In MecanumDrive, the right side motor controllers are automatically
- * inverted, while in RobotDrive, no motor controllers are automatically
- * inverted.
* <br>DriveCartesian(double, double, double, double) is equivalent to
* RobotDrive's MecanumDrive_Cartesian(double, double, double, double)
* if a deadband of 0 is used, and the ySpeed and gyroAngle values are inverted
@@ -76,6 +71,11 @@
public wpi::Sendable,
public wpi::SendableHelper<MecanumDrive> {
public:
+ /**
+ * Wheel speeds for a mecanum drive.
+ *
+ * Uses normalized voltage [-1.0..1.0].
+ */
struct WheelSpeeds {
double frontLeft = 0.0;
double frontRight = 0.0;
@@ -103,9 +103,9 @@
* Angles are measured clockwise from the positive X axis. The robot's speed
* is independent from its angle or rotation rate.
*
- * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is
+ * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Forward is
* positive.
- * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is
* positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
* Clockwise is positive.
@@ -136,14 +136,15 @@
* Angles are measured clockwise from the positive X axis. The robot's speed
* is independent from its angle or rotation rate.
*
- * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is
+ * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Forward is
* positive.
- * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is
+ * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Right is
* positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
* Clockwise is positive.
* @param gyroAngle The current angle reading from the gyro in degrees around
* the Z axis. Use this to implement field-oriented controls.
+ * @return Wheel speeds [-1.0..1.0].
*/
static WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed,
double zRotation, double gyroAngle = 0.0);
diff --git a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
index 389d67b..8ce5636 100644
--- a/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
+++ b/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
@@ -82,10 +82,10 @@
static double ApplyDeadband(double value, double deadband);
/**
- * Normalize all wheel speeds if the magnitude of any wheel is greater than
+ * Renormalize all wheel speeds if the magnitude of any wheel is greater than
* 1.0.
*/
- static void Normalize(wpi::span<double> wheelSpeeds);
+ static void Desaturate(wpi::span<double> wheelSpeeds);
double m_deadband = 0.02;
double m_maxOutput = 1.0;
diff --git a/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h b/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h
index ac6ba4b..7bb63fb 100644
--- a/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h
+++ b/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h
@@ -61,6 +61,16 @@
int GetChannel() const;
+ /**
+ * Optionally eliminate the deadband from a motor controller.
+ *
+ * @param eliminateDeadband If true, set the motor curve on the speed
+ * controller to eliminate the deadband in the middle
+ * of the range. Otherwise, keep the full range
+ * without modifying any values.
+ */
+ void EnableDeadbandElimination(bool eliminateDeadband);
+
protected:
/**
* Constructor for a PWM Motor %Controller connected via PWM.
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h b/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
index 9ac315f..d7ed1d5 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
@@ -370,7 +370,16 @@
* </td></tr>
* </table>
*/
- kCameraStream
+ kCameraStream,
+ /**
+ * Displays a field2d object.<br>
+ * Supported types:
+ *
+ * <ul>
+ * <li>Field2d
+ * </ul>
+ */
+ kField,
};
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h b/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
index e7c9a81..4792ce3 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
@@ -7,9 +7,13 @@
#include <functional>
#include <memory>
#include <string>
+#include <string_view>
#ifndef DYNAMIC_CAMERA_SERVER
#include <cscore_oo.h>
+#include <fmt/format.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
#else
namespace cs {
class VideoSource;
@@ -20,6 +24,7 @@
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
+#include <wpi/span.h>
namespace frc {
@@ -28,8 +33,8 @@
namespace detail {
constexpr const char* kProtocol = "camera_server://";
std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
- CS_Source source);
-void AddToSendableRegistry(wpi::Sendable* sendable, std::string name);
+ std::string_view cameraName);
+void AddToSendableRegistry(wpi::Sendable* sendable, std::string_view name);
} // namespace detail
/**
@@ -46,9 +51,9 @@
* Creates a new sendable wrapper. Private constructor to avoid direct
* instantiation with multiple wrappers floating around for the same camera.
*
- * @param source the source to wrap
+ * @param cameraName the name of the camera to wrap
*/
- SendableCameraWrapper(CS_Source source, const private_init&);
+ SendableCameraWrapper(std::string_view cameraName, const private_init&);
/**
* Gets a sendable wrapper object for the given video source, creating the
@@ -61,6 +66,9 @@
static SendableCameraWrapper& Wrap(const cs::VideoSource& source);
static SendableCameraWrapper& Wrap(CS_Source source);
+ static SendableCameraWrapper& Wrap(std::string_view cameraName,
+ wpi::span<const std::string> cameraUrls);
+
void InitSendable(wpi::SendableBuilder& builder) override;
private:
@@ -68,11 +76,9 @@
};
#ifndef DYNAMIC_CAMERA_SERVER
-inline SendableCameraWrapper::SendableCameraWrapper(CS_Source source,
+inline SendableCameraWrapper::SendableCameraWrapper(std::string_view name,
const private_init&)
: m_uri(detail::kProtocol) {
- CS_Status status = 0;
- auto name = cs::GetSourceName(source, &status);
detail::AddToSendableRegistry(this, name);
m_uri += name;
}
@@ -83,12 +89,27 @@
}
inline SendableCameraWrapper& SendableCameraWrapper::Wrap(CS_Source source) {
- auto& wrapper = detail::GetSendableCameraWrapper(source);
+ CS_Status status = 0;
+ auto name = cs::GetSourceName(source, &status);
+ auto& wrapper = detail::GetSendableCameraWrapper(name);
if (!wrapper) {
- wrapper = std::make_shared<SendableCameraWrapper>(source, private_init{});
+ wrapper = std::make_shared<SendableCameraWrapper>(name, private_init{});
}
return *wrapper;
}
+
+inline SendableCameraWrapper& SendableCameraWrapper::Wrap(
+ std::string_view cameraName, wpi::span<const std::string> cameraUrls) {
+ auto& wrapper = detail::GetSendableCameraWrapper(cameraName);
+ if (!wrapper) {
+ wrapper =
+ std::make_shared<SendableCameraWrapper>(cameraName, private_init{});
+ }
+ auto streams = fmt::format("/CameraPublisher/{}/streams", cameraName);
+ nt::NetworkTableInstance::GetDefault().GetEntry(streams).SetStringArray(
+ cameraUrls);
+ return *wrapper;
+}
#endif
} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
index db58a21..13a5cee 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
@@ -128,6 +128,18 @@
ComplexWidget& Add(std::string_view title, const cs::VideoSource& video);
/**
+ * Adds a widget to this container to display a video stream.
+ *
+ * @param title the title of the widget
+ * @param cameraName the name of the streamed camera
+ * @param cameraUrls the URLs with which the dashboard can access the camera
+ * stream
+ * @return a widget to display the camera stream
+ */
+ ComplexWidget& AddCamera(std::string_view title, std::string_view cameraName,
+ wpi::span<const std::string> cameraUrls);
+
+ /**
* Adds a widget to this container to display the given sendable.
*
* @param sendable the sendable to display
@@ -526,4 +538,10 @@
std::string_view title, const cs::VideoSource& video) {
return Add(title, frc::SendableCameraWrapper::Wrap(video));
}
+
+inline frc::ComplexWidget& frc::ShuffleboardContainer::AddCamera(
+ std::string_view title, std::string_view cameraName,
+ wpi::span<const std::string> cameraUrls) {
+ return Add(title, frc::SendableCameraWrapper::Wrap(cameraName, cameraUrls));
+}
#endif
diff --git a/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h b/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h
new file mode 100644
index 0000000..9618695
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h
@@ -0,0 +1,106 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+namespace frc {
+
+class ADIS16448_IMU;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADIS16448 IMU.
+ */
+class ADIS16448_IMUSim {
+ public:
+ /**
+ * Constructs from a ADIS16448_IMU object.
+ *
+ * @param imu ADIS16448_IMU to simulate
+ */
+ explicit ADIS16448_IMUSim(const ADIS16448_IMU& imu);
+
+ /**
+ * Sets the X axis angle (CCW positive).
+ *
+ * @param angle The angle.
+ */
+ void SetGyroAngleX(units::degree_t angle);
+
+ /**
+ * Sets the Y axis angle (CCW positive).
+ *
+ * @param angle The angle.
+ */
+ void SetGyroAngleY(units::degree_t angle);
+
+ /**
+ * Sets the Z axis angle (CCW positive).
+ *
+ * @param angle The angle.
+ */
+ void SetGyroAngleZ(units::degree_t angle);
+
+ /**
+ * Sets the X axis angular rate (CCW positive).
+ *
+ * @param angularRate The angular rate.
+ */
+ void SetGyroRateX(units::degrees_per_second_t angularRate);
+
+ /**
+ * Sets the Y axis angular rate (CCW positive).
+ *
+ * @param angularRate The angular rate.
+ */
+ void SetGyroRateY(units::degrees_per_second_t angularRate);
+
+ /**
+ * Sets the Z axis angular rate (CCW positive).
+ *
+ * @param angularRate The angular rate.
+ */
+ void SetGyroRateZ(units::degrees_per_second_t angularRate);
+
+ /**
+ * Sets the X axis acceleration.
+ *
+ * @param accel The acceleration.
+ */
+ void SetAccelX(units::meters_per_second_squared_t accel);
+
+ /**
+ * Sets the Y axis acceleration.
+ *
+ * @param accel The acceleration.
+ */
+ void SetAccelY(units::meters_per_second_squared_t accel);
+
+ /**
+ * Sets the Z axis acceleration.
+ *
+ * @param accel The acceleration.
+ */
+ void SetAccelZ(units::meters_per_second_squared_t accel);
+
+ private:
+ hal::SimDouble m_simGyroAngleX;
+ hal::SimDouble m_simGyroAngleY;
+ hal::SimDouble m_simGyroAngleZ;
+ hal::SimDouble m_simGyroRateX;
+ hal::SimDouble m_simGyroRateY;
+ hal::SimDouble m_simGyroRateZ;
+ hal::SimDouble m_simAccelX;
+ hal::SimDouble m_simAccelY;
+ hal::SimDouble m_simAccelZ;
+};
+
+} // namespace sim
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h b/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h
new file mode 100644
index 0000000..7b43762
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h
@@ -0,0 +1,106 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+namespace frc {
+
+class ADIS16470_IMU;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADIS16470 IMU.
+ */
+class ADIS16470_IMUSim {
+ public:
+ /**
+ * Constructs from a ADIS16470_IMU object.
+ *
+ * @param imu ADIS16470_IMU to simulate
+ */
+ explicit ADIS16470_IMUSim(const ADIS16470_IMU& imu);
+
+ /**
+ * Sets the X axis angle (CCW positive).
+ *
+ * @param angle The angle.
+ */
+ void SetGyroAngleX(units::degree_t angle);
+
+ /**
+ * Sets the Y axis angle (CCW positive).
+ *
+ * @param angle The angle.
+ */
+ void SetGyroAngleY(units::degree_t angle);
+
+ /**
+ * Sets the Z axis angle (CCW positive).
+ *
+ * @param angle The angle.
+ */
+ void SetGyroAngleZ(units::degree_t angle);
+
+ /**
+ * Sets the X axis angular rate (CCW positive).
+ *
+ * @param angularRate The angular rate.
+ */
+ void SetGyroRateX(units::degrees_per_second_t angularRate);
+
+ /**
+ * Sets the Y axis angular rate (CCW positive).
+ *
+ * @param angularRate The angular rate.
+ */
+ void SetGyroRateY(units::degrees_per_second_t angularRate);
+
+ /**
+ * Sets the Z axis angular rate (CCW positive).
+ *
+ * @param angularRate The angular rate.
+ */
+ void SetGyroRateZ(units::degrees_per_second_t angularRate);
+
+ /**
+ * Sets the X axis acceleration.
+ *
+ * @param accel The acceleration.
+ */
+ void SetAccelX(units::meters_per_second_squared_t accel);
+
+ /**
+ * Sets the Y axis acceleration.
+ *
+ * @param accel The acceleration.
+ */
+ void SetAccelY(units::meters_per_second_squared_t accel);
+
+ /**
+ * Sets the Z axis acceleration.
+ *
+ * @param accel The acceleration.
+ */
+ void SetAccelZ(units::meters_per_second_squared_t accel);
+
+ private:
+ hal::SimDouble m_simGyroAngleX;
+ hal::SimDouble m_simGyroAngleY;
+ hal::SimDouble m_simGyroAngleZ;
+ hal::SimDouble m_simGyroRateX;
+ hal::SimDouble m_simGyroRateY;
+ hal::SimDouble m_simGyroRateZ;
+ hal::SimDouble m_simAccelX;
+ hal::SimDouble m_simAccelY;
+ hal::SimDouble m_simAccelZ;
+};
+
+} // namespace sim
+} // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h
new file mode 100644
index 0000000..b1388bd
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h
@@ -0,0 +1,81 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/moment_of_inertia.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated DC motor mechanism.
+ */
+class DCMotorSim : public LinearSystemSim<2, 1, 2> {
+ public:
+ /**
+ * Creates a simulated DC motor mechanism.
+ *
+ * @param plant The linear system representing the DC motor.
+ * @param gearbox The type of and number of motors in the DC motor
+ * gearbox.
+ * @param gearing The gearing of the DC motor (numbers greater than
+ * 1 represent reductions).
+ * @param measurementStdDevs The standard deviation of the measurement noise.
+ */
+ DCMotorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox,
+ double gearing,
+ const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
+
+ /**
+ * Creates a simulated DC motor mechanism.
+ *
+ * @param gearbox The type of and number of motors in the DC motor
+ * gearbox.
+ * @param gearing The gearing of the DC motor (numbers greater than
+ * 1 represent reductions).
+ * @param moi The moment of inertia of the DC motor.
+ * @param measurementStdDevs The standard deviation of the measurement noise.
+ */
+ DCMotorSim(const DCMotor& gearbox, double gearing,
+ units::kilogram_square_meter_t moi,
+ const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
+
+ /**
+ * Returns the DC motor position.
+ *
+ * @return The DC motor position.
+ */
+ units::radian_t GetAngularPosition() const;
+
+ /**
+ * Returns the DC motor velocity.
+ *
+ * @return The DC motor velocity.
+ */
+ units::radians_per_second_t GetAngularVelocity() const;
+
+ /**
+ * Returns the DC motor current draw.
+ *
+ * @return The DC motor current draw.
+ */
+ units::ampere_t GetCurrentDraw() const override;
+
+ /**
+ * Sets the input voltage for the DC motor.
+ *
+ * @param voltage The input voltage.
+ */
+ void SetInputVoltage(units::volt_t voltage);
+
+ private:
+ DCMotor m_gearbox;
+ double m_gearing;
+};
+} // namespace frc::sim
diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
index e64a652..7c53eb7 100644
--- a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
@@ -225,6 +225,12 @@
static constexpr frc::DCMotor SingleMiniCIMPerSide =
frc::DCMotor::MiniCIM(1);
static constexpr frc::DCMotor DualMiniCIMPerSide = frc::DCMotor::MiniCIM(2);
+ static constexpr frc::DCMotor SingleFalcon500PerSide =
+ frc::DCMotor::Falcon500(1);
+ static constexpr frc::DCMotor DualFalcon500PerSide =
+ frc::DCMotor::Falcon500(2);
+ static constexpr frc::DCMotor SingleNEOPerSide = frc::DCMotor::NEO(1);
+ static constexpr frc::DCMotor DualNEOPerSide = frc::DCMotor::NEO(2);
};
class KitbotWheelSize {
diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
index 61cbd46..731fad3 100644
--- a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
@@ -45,6 +45,8 @@
m_u = Eigen::Vector<double, Inputs>::Zero();
}
+ virtual ~LinearSystemSim() = default;
+
/**
* Updates the simulation.
*
@@ -136,7 +138,7 @@
* @return The normalized input.
*/
Eigen::Vector<double, Inputs> ClampInput(Eigen::Vector<double, Inputs> u) {
- return frc::NormalizeInputVector<Inputs>(
+ return frc::DesaturateInputVector<Inputs>(
u, frc::RobotController::GetInputVoltage());
}
diff --git a/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h b/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h
index 97e31bf..e5f4efe 100644
--- a/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h
@@ -119,22 +119,22 @@
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]] std::unique_ptr<CallbackStore>
- RegisterClosedLoopEnabledCallback(NotifyCallback callback,
- bool initialNotify);
+ RegisterCompressorConfigTypeCallback(NotifyCallback callback,
+ bool initialNotify);
/**
* Check whether the closed loop compressor control is active.
*
- * @return true if active
+ * @return compressor config type
*/
- bool GetClosedLoopEnabled() const;
+ int GetCompressorConfigType() const;
/**
* Turn on/off the closed loop control of the compressor.
*
- * @param closedLoopEnabled whether the control loop is active
+ * @param compressorConfigType compressor config type
*/
- void SetClosedLoopEnabled(bool closedLoopEnabled);
+ void SetCompressorConfigType(int compressorConfigType);
/**
* Register a callback to be run whenever the pressure switch value changes.
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h
index 70abaa6..f1bebb9 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h
@@ -34,7 +34,14 @@
*
* @param color the color of the line
*/
- void SetColor(const frc::Color8Bit& color);
+ void SetColor(const Color8Bit& color);
+
+ /**
+ * Get the ligament color.
+ *
+ * @return the color of the line
+ */
+ Color8Bit GetColor();
/**
* Set the ligament's length.
@@ -71,6 +78,13 @@
*/
void SetLineWeight(double lineWidth);
+ /**
+ * Get the line thickness.
+ *
+ * @return the line thickness
+ */
+ double GetLineWeight();
+
protected:
void UpdateEntries(std::shared_ptr<nt::NetworkTable> table) override;
diff --git a/wpilibc/src/test/native/cpp/DebouncerTest.cpp b/wpilibc/src/test/native/cpp/DebouncerTest.cpp
deleted file mode 100644
index 379a6d5..0000000
--- a/wpilibc/src/test/native/cpp/DebouncerTest.cpp
+++ /dev/null
@@ -1,48 +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/Debouncer.h" // NOLINT(build/include_order)
-
-#include "frc/simulation/SimHooks.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-TEST(DebouncerTest, DebounceRising) {
- Debouncer debouncer{20_ms};
-
- debouncer.Calculate(false);
- EXPECT_FALSE(debouncer.Calculate(true));
-
- frc::sim::StepTiming(100_ms);
-
- EXPECT_TRUE(debouncer.Calculate(true));
-}
-
-TEST(DebouncerTest, DebounceFalling) {
- Debouncer debouncer{20_ms, Debouncer::DebounceType::kFalling};
-
- debouncer.Calculate(true);
- EXPECT_TRUE(debouncer.Calculate(false));
-
- frc::sim::StepTiming(100_ms);
-
- EXPECT_FALSE(debouncer.Calculate(false));
-}
-
-TEST(DebouncerTest, DebounceBoth) {
- Debouncer debouncer{20_ms, Debouncer::DebounceType::kBoth};
-
- debouncer.Calculate(false);
- EXPECT_FALSE(debouncer.Calculate(true));
-
- frc::sim::StepTiming(100_ms);
-
- EXPECT_TRUE(debouncer.Calculate(true));
- EXPECT_TRUE(debouncer.Calculate(false));
-
- frc::sim::StepTiming(100_ms);
-
- EXPECT_FALSE(debouncer.Calculate(false));
-}
diff --git a/wpilibc/src/test/native/cpp/TestCallbackHelpers.cpp b/wpilibc/src/test/native/cpp/TestCallbackHelpers.cpp
index d98f539..12ae5d2 100644
--- a/wpilibc/src/test/native/cpp/TestCallbackHelpers.cpp
+++ b/wpilibc/src/test/native/cpp/TestCallbackHelpers.cpp
@@ -4,8 +4,6 @@
#include "callback_helpers/TestCallbackHelpers.h"
-#include <iostream>
-
#include <fmt/format.h>
namespace frc::sim {
diff --git a/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
index 1d13a77..b930346 100644
--- a/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
+++ b/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
@@ -6,6 +6,238 @@
#include "frc/drive/DifferentialDrive.h"
#include "gtest/gtest.h"
+TEST(DifferentialDriveTest, ArcadeDriveIK) {
+ // Forward
+ auto speeds = frc::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, false);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Forward left turn
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, false);
+ EXPECT_DOUBLE_EQ(0.0, speeds.left);
+ EXPECT_DOUBLE_EQ(0.5, speeds.right);
+
+ // Forward right turn
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, false);
+ EXPECT_DOUBLE_EQ(0.5, speeds.left);
+ EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+ // Backward
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, false);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+ // Backward left turn
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, false);
+ EXPECT_DOUBLE_EQ(-0.5, speeds.left);
+ EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+ // Backward right turn
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, false);
+ EXPECT_DOUBLE_EQ(0.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-0.5, speeds.right);
+
+ // Left turn (xSpeed with negative sign)
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Left turn (xSpeed with positive sign)
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Right turn (xSpeed with negative sign)
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+ // Right turn (xSpeed with positive sign)
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+}
+
+TEST(DifferentialDriveTest, ArcadeDriveIKSquared) {
+ // Forward
+ auto speeds = frc::DifferentialDrive::ArcadeDriveIK(1.0, 0.0, true);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Forward left turn
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, -0.5, true);
+ EXPECT_DOUBLE_EQ(0.0, speeds.left);
+ EXPECT_DOUBLE_EQ(0.25, speeds.right);
+
+ // Forward right turn
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(0.5, 0.5, true);
+ EXPECT_DOUBLE_EQ(0.25, speeds.left);
+ EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+ // Backward
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(-1.0, 0.0, true);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+ // Backward left turn
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, -0.5, true);
+ EXPECT_DOUBLE_EQ(-0.25, speeds.left);
+ EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+ // Backward right turn
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.5, 0.5, true);
+ EXPECT_DOUBLE_EQ(0.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-0.25, speeds.right);
+
+ // Left turn (xSpeed with negative sign)
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, -1.0, false);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Left turn (xSpeed with positive sign)
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, -1.0, false);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Right turn (xSpeed with negative sign)
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(-0.0, 1.0, false);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+ // Right turn (xSpeed with positive sign)
+ speeds = frc::DifferentialDrive::ArcadeDriveIK(0.0, 1.0, false);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+}
+
+TEST(DifferentialDriveTest, CurvatureDriveIK) {
+ // Forward
+ auto speeds = frc::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, false);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Forward left turn
+ speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, false);
+ EXPECT_DOUBLE_EQ(0.25, speeds.left);
+ EXPECT_DOUBLE_EQ(0.75, speeds.right);
+
+ // Forward right turn
+ speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, false);
+ EXPECT_DOUBLE_EQ(0.75, speeds.left);
+ EXPECT_DOUBLE_EQ(0.25, speeds.right);
+
+ // Backward
+ speeds = frc::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, false);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+ // Backward left turn
+ speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, false);
+ EXPECT_DOUBLE_EQ(-0.75, speeds.left);
+ EXPECT_DOUBLE_EQ(-0.25, speeds.right);
+
+ // Backward right turn
+ speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, false);
+ EXPECT_DOUBLE_EQ(-0.25, speeds.left);
+ EXPECT_DOUBLE_EQ(-0.75, speeds.right);
+}
+
+TEST(DifferentialDriveTest, CurvatureDriveIKTurnInPlace) {
+ // Forward
+ auto speeds = frc::DifferentialDrive::CurvatureDriveIK(1.0, 0.0, true);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Forward left turn
+ speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, -0.5, true);
+ EXPECT_DOUBLE_EQ(0.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Forward right turn
+ speeds = frc::DifferentialDrive::CurvatureDriveIK(0.5, 0.5, true);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+ // Backward
+ speeds = frc::DifferentialDrive::CurvatureDriveIK(-1.0, 0.0, true);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+ // Backward left turn
+ speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, -0.5, true);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(0.0, speeds.right);
+
+ // Backward right turn
+ speeds = frc::DifferentialDrive::CurvatureDriveIK(-0.5, 0.5, true);
+ EXPECT_DOUBLE_EQ(0.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+}
+
+TEST(DifferentialDriveTest, TankDriveIK) {
+ // Forward
+ auto speeds = frc::DifferentialDrive::TankDriveIK(1.0, 1.0, false);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Forward left turn
+ speeds = frc::DifferentialDrive::TankDriveIK(0.5, 1.0, false);
+ EXPECT_DOUBLE_EQ(0.5, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Forward right turn
+ speeds = frc::DifferentialDrive::TankDriveIK(1.0, 0.5, false);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(0.5, speeds.right);
+
+ // Backward
+ speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -1.0, false);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+ // Backward left turn
+ speeds = frc::DifferentialDrive::TankDriveIK(-0.5, -1.0, false);
+ EXPECT_DOUBLE_EQ(-0.5, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+ // Backward right turn
+ speeds = frc::DifferentialDrive::TankDriveIK(-0.5, 1.0, false);
+ EXPECT_DOUBLE_EQ(-0.5, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+}
+
+TEST(DifferentialDriveTest, TankDriveIKSquared) {
+ // Forward
+ auto speeds = frc::DifferentialDrive::TankDriveIK(1.0, 1.0, true);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Forward left turn
+ speeds = frc::DifferentialDrive::TankDriveIK(0.5, 1.0, true);
+ EXPECT_DOUBLE_EQ(0.25, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+
+ // Forward right turn
+ speeds = frc::DifferentialDrive::TankDriveIK(1.0, 0.5, true);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(0.25, speeds.right);
+
+ // Backward
+ speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -1.0, true);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+ // Backward left turn
+ speeds = frc::DifferentialDrive::TankDriveIK(-0.5, -1.0, true);
+ EXPECT_DOUBLE_EQ(-0.25, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+
+ // Backward right turn
+ speeds = frc::DifferentialDrive::TankDriveIK(-1.0, -0.5, true);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-0.25, speeds.right);
+}
+
TEST(DifferentialDriveTest, ArcadeDrive) {
frc::MockMotorController left;
frc::MockMotorController right;
diff --git a/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp
index 510b377..c23c7b0 100644
--- a/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp
+++ b/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp
@@ -8,6 +8,80 @@
#include "frc/drive/KilloughDrive.h"
#include "gtest/gtest.h"
+TEST(KilloughDriveTest, CartesianIK) {
+ frc::MockMotorController left;
+ frc::MockMotorController right;
+ frc::MockMotorController back;
+ frc::KilloughDrive drive{left, right, back};
+
+ // Forward
+ auto speeds = drive.DriveCartesianIK(1.0, 0.0, 0.0);
+ EXPECT_DOUBLE_EQ(0.5, speeds.left);
+ EXPECT_DOUBLE_EQ(-0.5, speeds.right);
+ EXPECT_NEAR(0.0, speeds.back, 1e-9);
+
+ // Left
+ speeds = drive.DriveCartesianIK(0.0, -1.0, 0.0);
+ EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.left);
+ EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.right);
+ EXPECT_DOUBLE_EQ(1.0, speeds.back);
+
+ // Right
+ speeds = drive.DriveCartesianIK(0.0, 1.0, 0.0);
+ EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, speeds.left);
+ EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, speeds.right);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.back);
+
+ // Rotate CCW
+ speeds = drive.DriveCartesianIK(0.0, 0.0, -1.0);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.back);
+
+ // Rotate CW
+ speeds = drive.DriveCartesianIK(0.0, 0.0, 1.0);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+ EXPECT_DOUBLE_EQ(1.0, speeds.back);
+}
+
+TEST(KilloughDriveTest, CartesianIKGyro90CW) {
+ frc::MockMotorController left;
+ frc::MockMotorController right;
+ frc::MockMotorController back;
+ frc::KilloughDrive drive{left, right, back};
+
+ // Forward in global frame; left in robot frame
+ auto speeds = drive.DriveCartesianIK(1.0, 0.0, 0.0, 90.0);
+ EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.left);
+ EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, speeds.right);
+ EXPECT_DOUBLE_EQ(1.0, speeds.back);
+
+ // Left in global frame; backward in robot frame
+ speeds = drive.DriveCartesianIK(0.0, -1.0, 0.0, 90.0);
+ EXPECT_DOUBLE_EQ(-0.5, speeds.left);
+ EXPECT_NEAR(0.5, speeds.right, 1e-9);
+ EXPECT_NEAR(0.0, speeds.back, 1e-9);
+
+ // Right in global frame; forward in robot frame
+ speeds = drive.DriveCartesianIK(0.0, 1.0, 0.0, 90.0);
+ EXPECT_DOUBLE_EQ(0.5, speeds.left);
+ EXPECT_NEAR(-0.5, speeds.right, 1e-9);
+ EXPECT_NEAR(0.0, speeds.back, 1e-9);
+
+ // Rotate CCW
+ speeds = drive.DriveCartesianIK(0.0, 0.0, -1.0, 90.0);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.right);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.back);
+
+ // Rotate CW
+ speeds = drive.DriveCartesianIK(0.0, 0.0, 1.0, 90.0);
+ EXPECT_DOUBLE_EQ(1.0, speeds.left);
+ EXPECT_DOUBLE_EQ(1.0, speeds.right);
+ EXPECT_DOUBLE_EQ(1.0, speeds.back);
+}
+
TEST(KilloughDriveTest, Cartesian) {
frc::MockMotorController left;
frc::MockMotorController right;
diff --git a/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp b/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
index fe0a73b..2990848 100644
--- a/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
+++ b/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
@@ -6,12 +6,86 @@
#include "frc/drive/MecanumDrive.h"
#include "gtest/gtest.h"
+TEST(MecanumDriveTest, CartesianIK) {
+ // Forward
+ auto speeds = frc::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0);
+ EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
+ EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+ EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+ EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
+
+ // Left
+ speeds = frc::MecanumDrive::DriveCartesianIK(0.0, -1.0, 0.0);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
+ EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+ EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
+
+ // Right
+ speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 1.0, 0.0);
+ EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
+ EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
+
+ // Rotate CCW
+ speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, -1.0);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
+ EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
+ EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
+
+ // Rotate CW
+ speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, 1.0);
+ EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
+ EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
+}
+
+TEST(MecanumDriveTest, CartesianIKGyro90CW) {
+ // Forward in global frame; left in robot frame
+ auto speeds = frc::MecanumDrive::DriveCartesianIK(1.0, 0.0, 0.0, 90.0);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
+ EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+ EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
+
+ // Left in global frame; backward in robot frame
+ speeds = frc::MecanumDrive::DriveCartesianIK(0.0, -1.0, 0.0, 90.0);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
+
+ // Right in global frame; forward in robot frame
+ speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 1.0, 0.0, 90.0);
+ EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
+ EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+ EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+ EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
+
+ // Rotate CCW
+ speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, -1.0, 90.0);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.frontLeft);
+ EXPECT_DOUBLE_EQ(1.0, speeds.frontRight);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.rearLeft);
+ EXPECT_DOUBLE_EQ(1.0, speeds.rearRight);
+
+ // Rotate CW
+ speeds = frc::MecanumDrive::DriveCartesianIK(0.0, 0.0, 1.0, 90.0);
+ EXPECT_DOUBLE_EQ(1.0, speeds.frontLeft);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.frontRight);
+ EXPECT_DOUBLE_EQ(1.0, speeds.rearLeft);
+ EXPECT_DOUBLE_EQ(-1.0, speeds.rearRight);
+}
+
TEST(MecanumDriveTest, Cartesian) {
frc::MockMotorController fl;
- frc::MockMotorController fr;
frc::MockMotorController rl;
+ frc::MockMotorController fr;
frc::MockMotorController rr;
- frc::MecanumDrive drive{fl, fr, rl, rr};
+ frc::MecanumDrive drive{fl, rl, fr, rr};
drive.SetDeadband(0.0);
// Forward
@@ -38,15 +112,15 @@
// Rotate CCW
drive.DriveCartesian(0.0, 0.0, -1.0);
EXPECT_DOUBLE_EQ(-1.0, fl.Get());
- EXPECT_DOUBLE_EQ(-1.0, fr.Get());
- EXPECT_DOUBLE_EQ(1.0, rl.Get());
+ EXPECT_DOUBLE_EQ(1.0, fr.Get());
+ EXPECT_DOUBLE_EQ(-1.0, rl.Get());
EXPECT_DOUBLE_EQ(1.0, rr.Get());
// Rotate CW
drive.DriveCartesian(0.0, 0.0, 1.0);
EXPECT_DOUBLE_EQ(1.0, fl.Get());
- EXPECT_DOUBLE_EQ(1.0, fr.Get());
- EXPECT_DOUBLE_EQ(-1.0, rl.Get());
+ EXPECT_DOUBLE_EQ(-1.0, fr.Get());
+ EXPECT_DOUBLE_EQ(1.0, rl.Get());
EXPECT_DOUBLE_EQ(-1.0, rr.Get());
}
@@ -55,7 +129,7 @@
frc::MockMotorController fr;
frc::MockMotorController rl;
frc::MockMotorController rr;
- frc::MecanumDrive drive{fl, fr, rl, rr};
+ frc::MecanumDrive drive{fl, rl, fr, rr};
drive.SetDeadband(0.0);
// Forward in global frame; left in robot frame
@@ -82,15 +156,15 @@
// Rotate CCW
drive.DriveCartesian(0.0, 0.0, -1.0, 90.0);
EXPECT_DOUBLE_EQ(-1.0, fl.Get());
- EXPECT_DOUBLE_EQ(-1.0, fr.Get());
- EXPECT_DOUBLE_EQ(1.0, rl.Get());
+ EXPECT_DOUBLE_EQ(1.0, fr.Get());
+ EXPECT_DOUBLE_EQ(-1.0, rl.Get());
EXPECT_DOUBLE_EQ(1.0, rr.Get());
// Rotate CW
drive.DriveCartesian(0.0, 0.0, 1.0, 90.0);
EXPECT_DOUBLE_EQ(1.0, fl.Get());
- EXPECT_DOUBLE_EQ(1.0, fr.Get());
- EXPECT_DOUBLE_EQ(-1.0, rl.Get());
+ EXPECT_DOUBLE_EQ(-1.0, fr.Get());
+ EXPECT_DOUBLE_EQ(1.0, rl.Get());
EXPECT_DOUBLE_EQ(-1.0, rr.Get());
}
@@ -99,7 +173,7 @@
frc::MockMotorController fr;
frc::MockMotorController rl;
frc::MockMotorController rr;
- frc::MecanumDrive drive{fl, fr, rl, rr};
+ frc::MecanumDrive drive{fl, rl, fr, rr};
drive.SetDeadband(0.0);
// Forward
@@ -126,14 +200,14 @@
// Rotate CCW
drive.DrivePolar(0.0, 0.0, -1.0);
EXPECT_DOUBLE_EQ(-1.0, fl.Get());
- EXPECT_DOUBLE_EQ(-1.0, fr.Get());
- EXPECT_DOUBLE_EQ(1.0, rl.Get());
+ EXPECT_DOUBLE_EQ(1.0, fr.Get());
+ EXPECT_DOUBLE_EQ(-1.0, rl.Get());
EXPECT_DOUBLE_EQ(1.0, rr.Get());
// Rotate CW
drive.DrivePolar(0.0, 0.0, 1.0);
EXPECT_DOUBLE_EQ(1.0, fl.Get());
- EXPECT_DOUBLE_EQ(1.0, fr.Get());
- EXPECT_DOUBLE_EQ(-1.0, rl.Get());
+ EXPECT_DOUBLE_EQ(-1.0, fr.Get());
+ EXPECT_DOUBLE_EQ(1.0, rl.Get());
EXPECT_DOUBLE_EQ(-1.0, rr.Get());
}
diff --git a/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp
index a8f95da..8082ed5 100644
--- a/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp
@@ -96,7 +96,7 @@
EXPECT_TRUE(callback.GetLastValue());
}
-TEST(CTREPCMSimTest, SetClosedLoopEnabled) {
+TEST(CTREPCMSimTest, SetEnableDigital) {
PneumaticsControlModule pcm;
CTREPCMSim sim(pcm);
sim.ResetData();
@@ -105,12 +105,12 @@
auto cb =
sim.RegisterClosedLoopEnabledCallback(callback.GetCallback(), false);
- pcm.SetClosedLoopControl(false);
- EXPECT_FALSE(pcm.GetClosedLoopControl());
+ pcm.DisableCompressor();
+ EXPECT_EQ(pcm.GetCompressorConfigType(), CompressorConfigType::Disabled);
- pcm.SetClosedLoopControl(true);
+ pcm.EnableCompressorDigital();
EXPECT_TRUE(sim.GetClosedLoopEnabled());
- EXPECT_TRUE(pcm.GetClosedLoopControl());
+ EXPECT_EQ(pcm.GetCompressorConfigType(), CompressorConfigType::Digital);
EXPECT_TRUE(callback.WasTriggered());
EXPECT_TRUE(callback.GetLastValue());
}
@@ -143,7 +143,7 @@
sim.SetCompressorCurrent(35.04);
EXPECT_EQ(35.04, sim.GetCompressorCurrent());
- EXPECT_EQ(35.04, pcm.GetCompressorCurrent());
+ EXPECT_EQ(35.04_A, pcm.GetCompressorCurrent());
EXPECT_TRUE(callback.WasTriggered());
EXPECT_EQ(35.04, callback.GetLastValue());
}
diff --git a/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp
new file mode 100644
index 0000000..efed939
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp
@@ -0,0 +1,90 @@
+// 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/Encoder.h"
+#include "frc/RobotController.h"
+#include "frc/controller/PIDController.h"
+#include "frc/motorcontrol/PWMVictorSPX.h"
+#include "frc/simulation/BatterySim.h"
+#include "frc/simulation/DCMotorSim.h"
+#include "frc/simulation/EncoderSim.h"
+#include "frc/simulation/RoboRioSim.h"
+#include "gtest/gtest.h"
+
+TEST(DCMotorSimTest, VoltageSteadyState) {
+ frc::DCMotor gearbox = frc::DCMotor::NEO(1);
+ frc::sim::DCMotorSim sim{gearbox, 1.0,
+ units::kilogram_square_meter_t{0.0005}};
+
+ frc::Encoder encoder{0, 1};
+ frc::sim::EncoderSim encoderSim{encoder};
+ frc::PWMVictorSPX motor{0};
+
+ frc::sim::RoboRioSim::ResetData();
+ encoderSim.ResetData();
+
+ // Spin-up
+ for (int i = 0; i < 100; i++) {
+ // RobotPeriodic runs first
+ motor.SetVoltage(12_V);
+
+ // Then, SimulationPeriodic runs
+ frc::sim::RoboRioSim::SetVInVoltage(
+ frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
+ sim.SetInputVoltage(motor.Get() *
+ frc::RobotController::GetBatteryVoltage());
+ sim.Update(20_ms);
+ encoderSim.SetRate(sim.GetAngularVelocity().value());
+ }
+
+ EXPECT_NEAR((gearbox.Kv * 12_V).value(), encoder.GetRate(), 0.1);
+
+ // Decay
+ for (int i = 0; i < 100; i++) {
+ // RobotPeriodic runs first
+ motor.SetVoltage(0_V);
+
+ // Then, SimulationPeriodic runs
+ frc::sim::RoboRioSim::SetVInVoltage(
+ frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
+ sim.SetInputVoltage(motor.Get() *
+ frc::RobotController::GetBatteryVoltage());
+ sim.Update(20_ms);
+ encoderSim.SetRate(sim.GetAngularVelocity().value());
+ }
+
+ EXPECT_NEAR(0, encoder.GetRate(), 0.1);
+}
+
+TEST(DCMotorSimTest, PositionFeedbackControl) {
+ frc::DCMotor gearbox = frc::DCMotor::NEO(1);
+ frc::sim::DCMotorSim sim{gearbox, 1.0,
+ units::kilogram_square_meter_t{0.0005}};
+
+ frc2::PIDController controller{0.04, 0.0, 0.001};
+
+ frc::Encoder encoder{0, 1};
+ frc::sim::EncoderSim encoderSim{encoder};
+ frc::PWMVictorSPX motor{0};
+
+ frc::sim::RoboRioSim::ResetData();
+ encoderSim.ResetData();
+
+ for (int i = 0; i < 140; i++) {
+ // RobotPeriodic runs first
+ motor.Set(controller.Calculate(encoder.GetDistance(), 750));
+
+ // Then, SimulationPeriodic runs
+ frc::sim::RoboRioSim::SetVInVoltage(
+ frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
+ sim.SetInputVoltage(motor.Get() *
+ frc::RobotController::GetBatteryVoltage());
+ sim.Update(20_ms);
+ encoderSim.SetDistance(sim.GetAngularPosition().value());
+ encoderSim.SetRate(sim.GetAngularVelocity().value());
+ }
+
+ EXPECT_NEAR(encoder.GetDistance(), 750, 1.0);
+ EXPECT_NEAR(encoder.GetRate(), 0, 0.1);
+}
diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
similarity index 93%
rename from wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
rename to wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
index eabbecb..8ccfcd6 100644
--- a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp
@@ -17,7 +17,7 @@
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
#include "gtest/gtest.h"
-TEST(DifferentialDriveSimTest, Convergence) {
+TEST(DifferentialDrivetrainSimTest, Convergence) {
auto motor = frc::DCMotor::NEO(2);
auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
@@ -42,9 +42,8 @@
auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
frc::Pose2d(), {}, frc::Pose2d(2_m, 2_m, 0_rad), config);
- // NOLINTNEXTLINE
- for (double t = 0; t < trajectory.TotalTime().value(); t += 0.02) {
- auto state = trajectory.Sample(20_ms);
+ for (auto t = 0_s; t < trajectory.TotalTime(); t += 20_ms) {
+ auto state = trajectory.Sample(t);
auto ramseteOut = ramsete.Calculate(sim.GetPose(), state);
auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut);
@@ -70,7 +69,7 @@
EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01);
}
-TEST(DifferentialDriveSimTest, Current) {
+TEST(DifferentialDrivetrainSimTest, Current) {
auto motor = frc::DCMotor::NEO(2);
auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
@@ -97,7 +96,7 @@
EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
}
-TEST(DifferentialDriveSimTest, ModelStability) {
+TEST(DifferentialDrivetrainSimTest, ModelStability) {
auto motor = frc::DCMotor::NEO(2);
auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
motor, 50_kg, 2_in, 12_in, 2_kg_sq_m, 5.0);
diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
index bfc50e1..03022ab 100644
--- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
@@ -2,8 +2,6 @@
// 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 <iostream>
-
#include <units/time.h>
#include "frc/Encoder.h"
diff --git a/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp
index 7786b29..688f37e 100644
--- a/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp
@@ -96,23 +96,67 @@
EXPECT_TRUE(callback.GetLastValue());
}
-TEST(REVPHSimTest, SetClosedLoopEnabled) {
+TEST(REVPHSimTest, SetEnableDigital) {
PneumaticHub ph;
REVPHSim sim(ph);
sim.ResetData();
- BooleanCallback callback;
+ EnumCallback callback;
auto cb =
- sim.RegisterClosedLoopEnabledCallback(callback.GetCallback(), false);
+ sim.RegisterCompressorConfigTypeCallback(callback.GetCallback(), false);
- ph.SetClosedLoopControl(false);
- EXPECT_FALSE(ph.GetClosedLoopControl());
+ ph.DisableCompressor();
+ EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Disabled);
- ph.SetClosedLoopControl(true);
- EXPECT_TRUE(sim.GetClosedLoopEnabled());
- EXPECT_TRUE(ph.GetClosedLoopControl());
+ ph.EnableCompressorDigital();
+ EXPECT_EQ(sim.GetCompressorConfigType(),
+ static_cast<int>(CompressorConfigType::Digital));
+ EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Digital);
EXPECT_TRUE(callback.WasTriggered());
- EXPECT_TRUE(callback.GetLastValue());
+ EXPECT_EQ(callback.GetLastValue(),
+ static_cast<int>(CompressorConfigType::Digital));
+}
+
+TEST(REVPHSimTest, SetEnableAnalog) {
+ PneumaticHub ph;
+ REVPHSim sim(ph);
+ sim.ResetData();
+
+ EnumCallback callback;
+ auto cb =
+ sim.RegisterCompressorConfigTypeCallback(callback.GetCallback(), false);
+
+ ph.DisableCompressor();
+ EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Disabled);
+
+ ph.EnableCompressorAnalog(1_psi, 2_psi);
+ EXPECT_EQ(sim.GetCompressorConfigType(),
+ static_cast<int>(CompressorConfigType::Analog));
+ EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Analog);
+ EXPECT_TRUE(callback.WasTriggered());
+ EXPECT_EQ(callback.GetLastValue(),
+ static_cast<int>(CompressorConfigType::Analog));
+}
+
+TEST(REVPHSimTest, SetEnableHybrid) {
+ PneumaticHub ph;
+ REVPHSim sim(ph);
+ sim.ResetData();
+
+ EnumCallback callback;
+ auto cb =
+ sim.RegisterCompressorConfigTypeCallback(callback.GetCallback(), false);
+
+ ph.DisableCompressor();
+ EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Disabled);
+
+ ph.EnableCompressorHybrid(1_psi, 2_psi);
+ EXPECT_EQ(sim.GetCompressorConfigType(),
+ static_cast<int>(CompressorConfigType::Hybrid));
+ EXPECT_EQ(ph.GetCompressorConfigType(), CompressorConfigType::Hybrid);
+ EXPECT_TRUE(callback.WasTriggered());
+ EXPECT_EQ(callback.GetLastValue(),
+ static_cast<int>(CompressorConfigType::Hybrid));
}
TEST(REVPHSimTest, SetPressureSwitchEnabled) {
@@ -143,7 +187,7 @@
sim.SetCompressorCurrent(35.04);
EXPECT_EQ(35.04, sim.GetCompressorCurrent());
- EXPECT_EQ(35.04, ph.GetCompressorCurrent());
+ EXPECT_EQ(35.04_A, ph.GetCompressorCurrent());
EXPECT_TRUE(callback.WasTriggered());
EXPECT_EQ(35.04, callback.GetLastValue());
}
diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
index e7e0405..23af83d 100644
--- a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
+++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
@@ -2,8 +2,6 @@
// 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 <iostream>
-
#include <units/angular_acceleration.h>
#include <units/angular_velocity.h>