diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java index 4eb023cdc2..8408b0b37a 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -9,13 +9,8 @@ import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; -import edu.wpi.first.networktables.IntegerArrayEntry; -import edu.wpi.first.networktables.IntegerArrayPublisher; -import edu.wpi.first.networktables.IntegerArrayTopic; -import edu.wpi.first.networktables.NTSendable; -import edu.wpi.first.networktables.NTSendableBuilder; -import edu.wpi.first.networktables.StringArrayPublisher; -import edu.wpi.first.networktables.StringArrayTopic; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; @@ -46,7 +41,7 @@ import java.util.function.Consumer; * *

This class is provided by the NewCommands VendorDep */ -public final class CommandScheduler implements NTSendable, AutoCloseable { +public final class CommandScheduler implements Sendable, AutoCloseable { /** The Singleton Instance. */ private static CommandScheduler instance; @@ -624,45 +619,46 @@ public final class CommandScheduler implements NTSendable, AutoCloseable { } @Override - public void initSendable(NTSendableBuilder builder) { + public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Scheduler"); - final StringArrayPublisher namesPub = new StringArrayTopic(builder.getTopic("Names")).publish(); - final IntegerArrayPublisher idsPub = new IntegerArrayTopic(builder.getTopic("Ids")).publish(); - final IntegerArrayEntry cancelEntry = - new IntegerArrayTopic(builder.getTopic("Cancel")).getEntry(new long[] {}); - builder.addCloseable(namesPub); - builder.addCloseable(idsPub); - builder.addCloseable(cancelEntry); - builder.setUpdateTable( + builder.addStringArrayProperty( + "Names", () -> { - if (namesPub == null || idsPub == null || cancelEntry == null) { - return; - } - - Map ids = new LinkedHashMap<>(); - List names = new ArrayList<>(); - long[] ids2 = new long[m_scheduledCommands.size()]; - + String[] names = new String[m_scheduledCommands.size()]; int i = 0; + for (Command command : m_scheduledCommands) { + names[i] = command.getName(); + i++; + } + return names; + }, + null); + builder.addIntegerArrayProperty( + "Ids", + () -> { + long[] ids = new long[m_scheduledCommands.size()]; + int i = 0; + for (Command command : m_scheduledCommands) { + ids[i] = command.hashCode(); + i++; + } + return ids; + }, + null); + builder.addIntegerArrayProperty( + "Cancel", + () -> { + return new long[] {}; + }, + toCancel -> { + Map ids = new LinkedHashMap<>(); for (Command command : m_scheduledCommands) { long id = command.hashCode(); ids.put(id, command); - names.add(command.getName()); - ids2[i] = id; - i++; } - - long[] toCancel = cancelEntry.get(); - if (toCancel.length > 0) { - for (long hash : toCancel) { - cancel(ids.get(hash)); - ids.remove(hash); - } - cancelEntry.set(new long[] {}); + for (long hash : toCancel) { + cancel(ids.get(hash)); } - - namesPub.set(names.toArray(new String[] {})); - idsPub.set(ids2); }); } } diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp index 900f7bddd1..41dcacdba0 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp @@ -13,10 +13,10 @@ #include #include #include -#include #include #include #include +#include #include #include "frc2/command/CommandPtr.h" @@ -454,36 +454,40 @@ void CommandScheduler::RequireUngrouped( } } -void CommandScheduler::InitSendable(nt::NTSendableBuilder& builder) { +void CommandScheduler::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("Scheduler"); - builder.SetUpdateTable( - [this, - namesPub = nt::StringArrayTopic{builder.GetTopic("Names")}.Publish(), - idsPub = nt::IntegerArrayTopic{builder.GetTopic("Ids")}.Publish(), - cancelEntry = nt::IntegerArrayTopic{builder.GetTopic("Cancel")}.GetEntry( - {})]() mutable { - auto toCancel = cancelEntry.Get(); - if (!toCancel.empty()) { - for (auto cancel : cancelEntry.Get()) { - uintptr_t ptrTmp = static_cast(cancel); - Command* command = reinterpret_cast(ptrTmp); - if (m_impl->scheduledCommands.find(command) != - m_impl->scheduledCommands.end()) { - Cancel(command); - } - } - cancelEntry.Set({}); - } - - wpi::SmallVector names; - wpi::SmallVector ids; + builder.AddStringArrayProperty( + "Names", + [this]() mutable { + std::vector names; for (Command* command : m_impl->scheduledCommands) { names.emplace_back(command->GetName()); + } + return names; + }, + nullptr); + builder.AddIntegerArrayProperty( + "Ids", + [this]() mutable { + std::vector ids; + for (Command* command : m_impl->scheduledCommands) { uintptr_t ptrTmp = reinterpret_cast(command); ids.emplace_back(static_cast(ptrTmp)); } - namesPub.Set(names); - idsPub.Set(ids); + return ids; + }, + nullptr); + builder.AddIntegerArrayProperty( + "Cancel", []() { return std::vector{}; }, + [this](std::span toCancel) mutable { + for (auto cancel : toCancel) { + uintptr_t ptrTmp = static_cast(cancel); + Command* command = reinterpret_cast(ptrTmp); + if (m_impl->scheduledCommands.find(command) != + m_impl->scheduledCommands.end()) { + Cancel(command); + } + } }); } diff --git a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h index 986a4c30fb..188d273aa0 100644 --- a/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h +++ b/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h @@ -14,9 +14,9 @@ #include #include #include -#include #include #include +#include #include namespace frc2 { @@ -33,7 +33,7 @@ class Subsystem; * * This class is provided by the NewCommands VendorDep */ -class CommandScheduler final : public nt::NTSendable, +class CommandScheduler final : public wpi::Sendable, public wpi::SendableHelper { public: /** @@ -388,7 +388,7 @@ class CommandScheduler final : public nt::NTSendable, */ void RequireUngrouped(std::initializer_list commands); - void InitSendable(nt::NTSendableBuilder& builder) override; + void InitSendable(wpi::SendableBuilder& builder) override; private: // Constructor; private as this is a singleton diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java index c0295f51f3..793ad95f1e 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java @@ -11,6 +11,8 @@ import static org.mockito.Mockito.never; import static org.mockito.Mockito.times; import static org.mockito.Mockito.verify; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.junit.jupiter.api.Test; class CommandScheduleTest extends CommandTestBase { @@ -116,4 +118,27 @@ class CommandScheduleTest extends CommandTestBase { assertDoesNotThrow(() -> scheduler.cancel(mockCommand)); } } + + @Test + void smartDashboardCancelTest() { + try (CommandScheduler scheduler = new CommandScheduler(); + var inst = NetworkTableInstance.create()) { + SmartDashboard.setNetworkTableInstance(inst); + SmartDashboard.putData("Scheduler", scheduler); + SmartDashboard.updateValues(); + + MockCommandHolder holder = new MockCommandHolder(true); + Command mockCommand = holder.getMock(); + scheduler.schedule(mockCommand); + scheduler.run(); + SmartDashboard.updateValues(); + assertTrue(scheduler.isScheduled(mockCommand)); + + var table = inst.getTable("SmartDashboard"); + table.getEntry("Scheduler/Cancel").setIntegerArray(new long[] {mockCommand.hashCode()}); + SmartDashboard.updateValues(); + scheduler.run(); + assertFalse(scheduler.isScheduled(mockCommand)); + } + } } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp index cb16b48433..91786770e0 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp @@ -2,6 +2,9 @@ // 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 +#include + #include "CommandTestBase.h" using namespace frc2; @@ -98,3 +101,24 @@ TEST_F(CommandScheduleTest, NotScheduledCancel) { EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command)); } + +TEST_F(CommandScheduleTest, SmartDashboardCancel) { + CommandScheduler scheduler = GetScheduler(); + frc::SmartDashboard::PutData("Scheduler", &scheduler); + frc::SmartDashboard::UpdateValues(); + + MockCommand command; + scheduler.Schedule(&command); + scheduler.Run(); + frc::SmartDashboard::UpdateValues(); + EXPECT_TRUE(scheduler.IsScheduled(&command)); + + uintptr_t ptrTmp = reinterpret_cast(&command); + nt::NetworkTableInstance::GetDefault() + .GetEntry("/SmartDashboard/Scheduler/Cancel") + .SetIntegerArray( + std::span{{static_cast(ptrTmp)}}); + frc::SmartDashboard::UpdateValues(); + scheduler.Run(); + EXPECT_FALSE(scheduler.IsScheduled(&command)); +} diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index cac2d08613..14fd37fedc 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include "frc/Errors.h" @@ -882,7 +882,7 @@ int ADIS16448_IMU::GetPort() const { * This function pushes the most recent angle estimates for all axes to the *driver station. **/ -void ADIS16448_IMU::InitSendable(nt::NTSendableBuilder& builder) { +void ADIS16448_IMU::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("ADIS16448 IMU"); builder.AddDoubleProperty( "Yaw Angle", [=, this] { return GetAngle().value(); }, nullptr); diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index c987de6b99..05dac7893d 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include "frc/Errors.h" @@ -816,7 +816,7 @@ int ADIS16470_IMU::GetPort() const { * This function pushes the most recent angle estimates for all axes to the *driver station. **/ -void ADIS16470_IMU::InitSendable(nt::NTSendableBuilder& builder) { +void ADIS16470_IMU::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("ADIS16470 IMU"); builder.AddDoubleProperty( "Yaw Angle", [=, this] { return GetAngle().value(); }, nullptr); diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h index 60d57a9f1a..3f4a1c2ebd 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h @@ -20,7 +20,6 @@ #include #include -#include #include #include #include @@ -29,6 +28,7 @@ #include #include #include +#include #include #include "frc/DigitalInput.h" @@ -53,7 +53,7 @@ namespace frc { * the RoboRIO MXP port. */ -class ADIS16448_IMU : public nt::NTSendable, +class ADIS16448_IMU : public wpi::Sendable, public wpi::SendableHelper { public: /* ADIS16448 Calibration Time Enum Class */ @@ -216,7 +216,7 @@ class ADIS16448_IMU : public nt::NTSendable, */ int GetPort() const; - void InitSendable(nt::NTSendableBuilder& builder) override; + void InitSendable(wpi::SendableBuilder& builder) override; private: /** @brief ADIS16448 Register Map Declaration */ diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index 4619819a2b..e3b521c2b3 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -20,12 +20,12 @@ #include #include -#include #include #include #include #include #include +#include #include #include "frc/DigitalInput.h" @@ -50,7 +50,7 @@ namespace frc { * available on the RoboRIO. */ -class ADIS16470_IMU : public nt::NTSendable, +class ADIS16470_IMU : public wpi::Sendable, public wpi::SendableHelper { public: /* ADIS16470 Calibration Time Enum Class */ @@ -172,7 +172,7 @@ class ADIS16470_IMU : public nt::NTSendable, */ int GetPort() const; - void InitSendable(nt::NTSendableBuilder& builder) override; + void InitSendable(wpi::SendableBuilder& builder) override; private: /* ADIS16470 Register Map Declaration */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java index 7f9446b830..d53f4ccc7d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java @@ -18,8 +18,8 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.SimBoolean; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.networktables.NTSendable; -import edu.wpi.first.networktables.NTSendableBuilder; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; // CHECKSTYLE.OFF: TypeName // CHECKSTYLE.OFF: MemberName @@ -50,7 +50,7 @@ import edu.wpi.first.networktables.NTSendableBuilder; "PMD.EmptyIfStmt", "PMD.EmptyStatementNotInLoop" }) -public class ADIS16448_IMU implements AutoCloseable, NTSendable { +public class ADIS16448_IMU implements AutoCloseable, Sendable { /** ADIS16448 Register Map Declaration */ private static final int FLASH_CNT = 0x00; // Flash memory write count @@ -1157,7 +1157,7 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { } @Override - public void initSendable(NTSendableBuilder builder) { + public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Gyro"); builder.addDoubleProperty("Value", this::getAngle, null); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java index 56c2641cd4..8ca783098d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java @@ -17,8 +17,8 @@ import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.SimBoolean; import edu.wpi.first.hal.SimDevice; import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.networktables.NTSendable; -import edu.wpi.first.networktables.NTSendableBuilder; +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; import java.nio.ByteBuffer; import java.nio.ByteOrder; @@ -51,7 +51,7 @@ import java.nio.ByteOrder; "PMD.EmptyIfStmt", "PMD.EmptyStatementNotInLoop" }) -public class ADIS16470_IMU implements AutoCloseable, NTSendable { +public class ADIS16470_IMU implements AutoCloseable, Sendable { /* ADIS16470 Register Map Declaration */ private static final int FLASH_CNT = 0x00; // Flash memory write count private static final int DIAG_STAT = 0x02; // Diagnostic and operational status @@ -1042,7 +1042,7 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { } @Override - public void initSendable(NTSendableBuilder builder) { + public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Gyro"); builder.addDoubleProperty("Value", this::getAngle, null); }