From f9bece2ffbf7a77ccd11038b24d3d82a9118de0a Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Mon, 4 Dec 2017 23:28:33 -0800 Subject: [PATCH] Update LiveWindow to provide continuous telemetry. (#771) LiveWindow.updateValues() is now called from IterativeRobotBase on every loop iteration. Telemetry for all WPILib classes is enabled by default; it can be disabled for specific classes using LiveWindow.disableTelemetry(), or all telemetry can be disabled using LiveWindow.disableAllTelemetry(). This necessitated changing the hook methodology into other classes to be more property-based rather than each class providing multiple functions. This had the benefit of reducing boilerplate and increasing consistency. - Remove NamedSendable, add name to Sendable. - Provide SendableBase abstract class. - Deprecate LiveWindow addSensor/addActuator interfaces. - Add LiveWindow support to drive classes. - Add addChild() helper functions to Subsystem. - Fix inheritance hierarchy. Now only sensors inherit from SensorBase. Other devices inherit from some combination of SendableBase, ErrorBase, or nothing. --- wpilibc/src/main/native/cpp/ADXL345_I2C.cpp | 37 +- wpilibc/src/main/native/cpp/ADXL345_SPI.cpp | 38 +- wpilibc/src/main/native/cpp/ADXL362.cpp | 64 ++- wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp | 3 +- .../main/native/cpp/AnalogAccelerometer.cpp | 35 +- wpilibc/src/main/native/cpp/AnalogGyro.cpp | 23 +- wpilibc/src/main/native/cpp/AnalogInput.cpp | 27 +- wpilibc/src/main/native/cpp/AnalogOutput.cpp | 28 +- .../main/native/cpp/AnalogPotentiometer.cpp | 28 +- wpilibc/src/main/native/cpp/AnalogTrigger.cpp | 6 + .../main/native/cpp/AnalogTriggerOutput.cpp | 2 + .../main/native/cpp/BuiltInAccelerometer.cpp | 31 +- .../src/main/native/cpp/Buttons/Trigger.cpp | 19 +- .../src/main/native/cpp/Commands/Command.cpp | 69 ++-- .../main/native/cpp/Commands/PIDCommand.cpp | 11 +- .../main/native/cpp/Commands/PIDSubsystem.cpp | 13 +- .../main/native/cpp/Commands/Scheduler.cpp | 43 +-- .../main/native/cpp/Commands/Subsystem.cpp | 150 ++++--- wpilibc/src/main/native/cpp/Compressor.cpp | 58 +-- wpilibc/src/main/native/cpp/Counter.cpp | 24 +- .../main/native/cpp/DigitalGlitchFilter.cpp | 3 + wpilibc/src/main/native/cpp/DigitalInput.cpp | 26 +- wpilibc/src/main/native/cpp/DigitalOutput.cpp | 61 +-- .../src/main/native/cpp/DoubleSolenoid.cpp | 90 ++--- .../native/cpp/Drive/DifferentialDrive.cpp | 19 +- .../main/native/cpp/Drive/KilloughDrive.cpp | 20 + .../main/native/cpp/Drive/MecanumDrive.cpp | 27 +- wpilibc/src/main/native/cpp/Encoder.cpp | 59 ++- wpilibc/src/main/native/cpp/GearTooth.cpp | 11 +- wpilibc/src/main/native/cpp/GyroBase.cpp | 22 +- .../main/native/cpp/IterativeRobotBase.cpp | 2 + wpilibc/src/main/native/cpp/Jaguar.cpp | 4 +- .../main/native/cpp/LiveWindow/LiveWindow.cpp | 348 +++++++++++------ .../src/main/native/cpp/NidecBrushless.cpp | 51 +-- wpilibc/src/main/native/cpp/PIDController.cpp | 205 ++++------ wpilibc/src/main/native/cpp/PWM.cpp | 45 +-- wpilibc/src/main/native/cpp/PWMTalonSRX.cpp | 4 +- .../native/cpp/PowerDistributionPanel.cpp | 45 +-- wpilibc/src/main/native/cpp/Relay.cpp | 82 ++-- wpilibc/src/main/native/cpp/SD540.cpp | 4 +- wpilibc/src/main/native/cpp/Servo.cpp | 42 +- .../cpp/SmartDashboard/SendableBase.cpp | 91 +++++ .../SmartDashboard/SendableBuilderImpl.cpp | 365 ++++++++++++++++++ .../SmartDashboard/SendableChooserBase.cpp | 4 - .../cpp/SmartDashboard/SmartDashboard.cpp | 42 +- wpilibc/src/main/native/cpp/Solenoid.cpp | 52 +-- wpilibc/src/main/native/cpp/Spark.cpp | 4 +- .../main/native/cpp/SpeedControllerGroup.cpp | 9 + wpilibc/src/main/native/cpp/Talon.cpp | 4 +- wpilibc/src/main/native/cpp/Ultrasonic.cpp | 30 +- wpilibc/src/main/native/cpp/Victor.cpp | 4 +- wpilibc/src/main/native/cpp/VictorSP.cpp | 4 +- wpilibc/src/main/native/include/ADXL345_I2C.h | 21 +- wpilibc/src/main/native/include/ADXL345_SPI.h | 23 +- wpilibc/src/main/native/include/ADXL362.h | 20 +- .../main/native/include/AnalogAccelerometer.h | 17 +- wpilibc/src/main/native/include/AnalogInput.h | 19 +- .../src/main/native/include/AnalogOutput.h | 20 +- .../main/native/include/AnalogPotentiometer.h | 28 +- .../src/main/native/include/AnalogTrigger.h | 4 +- .../main/native/include/AnalogTriggerOutput.h | 4 +- .../native/include/BuiltInAccelerometer.h | 21 +- .../src/main/native/include/Buttons/Trigger.h | 17 +- .../main/native/include/Commands/Command.h | 26 +- .../main/native/include/Commands/PIDCommand.h | 3 +- .../native/include/Commands/PIDSubsystem.h | 6 +- .../main/native/include/Commands/Scheduler.h | 14 +- .../main/native/include/Commands/Subsystem.h | 33 +- wpilibc/src/main/native/include/Compressor.h | 23 +- wpilibc/src/main/native/include/Counter.h | 16 +- .../main/native/include/DigitalGlitchFilter.h | 5 +- .../src/main/native/include/DigitalInput.h | 16 +- .../src/main/native/include/DigitalOutput.h | 28 +- .../src/main/native/include/DigitalSource.h | 1 - .../src/main/native/include/DoubleSolenoid.h | 18 +- .../native/include/Drive/DifferentialDrive.h | 4 +- .../main/native/include/Drive/KilloughDrive.h | 4 +- .../main/native/include/Drive/MecanumDrive.h | 4 +- .../native/include/Drive/RobotDriveBase.h | 6 +- .../src/main/native/include/DriverStation.h | 6 +- wpilibc/src/main/native/include/Encoder.h | 20 +- wpilibc/src/main/native/include/GearTooth.h | 3 +- wpilibc/src/main/native/include/GyroBase.h | 21 +- wpilibc/src/main/native/include/I2C.h | 6 +- .../native/include/InterruptableSensorBase.h | 1 - wpilibc/src/main/native/include/Jaguar.h | 1 - .../native/include/LiveWindow/LiveWindow.h | 108 +++--- .../include/LiveWindow/LiveWindowSendable.h | 36 -- .../src/main/native/include/NidecBrushless.h | 25 +- .../src/main/native/include/PIDController.h | 35 +- .../src/main/native/include/PIDInterface.h | 1 - wpilibc/src/main/native/include/PWM.h | 21 +- .../main/native/include/PWMSpeedController.h | 1 - wpilibc/src/main/native/include/PWMTalonSRX.h | 1 - .../native/include/PowerDistributionPanel.h | 16 +- wpilibc/src/main/native/include/Relay.h | 20 +- wpilibc/src/main/native/include/SD540.h | 1 - wpilibc/src/main/native/include/SPI.h | 9 +- wpilibc/src/main/native/include/SensorBase.h | 7 +- wpilibc/src/main/native/include/Servo.h | 15 +- .../include/SmartDashboard/NamedSendable.h | 29 -- .../native/include/SmartDashboard/Sendable.h | 58 ++- .../include/SmartDashboard/SendableBase.h | 44 +++ .../include/SmartDashboard/SendableBuilder.h | 217 +++++++++++ .../SmartDashboard/SendableBuilderImpl.h | 182 +++++++++ .../include/SmartDashboard/SendableChooser.h | 8 +- .../SmartDashboard/SendableChooser.inc | 45 ++- .../SmartDashboard/SendableChooserBase.h | 12 +- .../include/SmartDashboard/SmartDashboard.h | 3 +- wpilibc/src/main/native/include/Solenoid.h | 17 +- .../src/main/native/include/SolenoidBase.h | 6 +- wpilibc/src/main/native/include/Spark.h | 1 - .../native/include/SpeedControllerGroup.h | 6 +- .../native/include/SpeedControllerGroup.inc | 8 +- wpilibc/src/main/native/include/Talon.h | 1 - wpilibc/src/main/native/include/Ultrasonic.h | 17 +- wpilibc/src/main/native/include/Victor.h | 1 - wpilibc/src/main/native/include/VictorSP.h | 1 - .../main/cpp/examples/GearsBot/src/Robot.cpp | 4 +- .../main/cpp/examples/GearsBot/src/Robot.h | 2 + .../examples/GearsBot/src/Subsystems/Claw.cpp | 4 +- .../GearsBot/src/Subsystems/DriveTrain.cpp | 29 +- .../GearsBot/src/Subsystems/Elevator.cpp | 7 +- .../GearsBot/src/Subsystems/Wrist.cpp | 8 +- .../cpp/examples/GettingStarted/src/Robot.cpp | 2 +- .../src/main/cpp/examples/PacGoat/src/OI.cpp | 1 + .../main/cpp/examples/PacGoat/src/Robot.cpp | 6 +- .../PacGoat/src/Subsystems/Collector.cpp | 12 +- .../PacGoat/src/Subsystems/DriveTrain.cpp | 21 +- .../examples/PacGoat/src/Subsystems/Pivot.cpp | 16 +- .../PacGoat/src/Subsystems/Pneumatics.cpp | 5 +- .../PacGoat/src/Subsystems/Shooter.cpp | 15 +- .../main/cpp/templates/commandbased/Robot.cpp | 2 +- .../main/cpp/templates/iterative/Robot.cpp | 2 +- .../src/main/cpp/templates/timed/Robot.cpp | 2 +- .../edu/wpi/first/wpilibj/ADXL345_I2C.java | 68 +--- .../edu/wpi/first/wpilibj/ADXL345_SPI.java | 65 +--- .../java/edu/wpi/first/wpilibj/ADXL362.java | 76 ++-- .../edu/wpi/first/wpilibj/ADXRS450_Gyro.java | 8 +- .../first/wpilibj/AnalogAccelerometer.java | 58 +-- .../edu/wpi/first/wpilibj/AnalogGyro.java | 9 +- .../edu/wpi/first/wpilibj/AnalogInput.java | 52 +-- .../edu/wpi/first/wpilibj/AnalogOutput.java | 54 +-- .../first/wpilibj/AnalogPotentiometer.java | 54 +-- .../edu/wpi/first/wpilibj/AnalogTrigger.java | 14 +- .../first/wpilibj/AnalogTriggerOutput.java | 14 +- .../first/wpilibj/BuiltInAccelerometer.java | 58 +-- .../edu/wpi/first/wpilibj/Compressor.java | 69 +--- .../java/edu/wpi/first/wpilibj/Counter.java | 42 +- .../first/wpilibj/DigitalGlitchFilter.java | 7 + .../edu/wpi/first/wpilibj/DigitalInput.java | 45 +-- .../edu/wpi/first/wpilibj/DigitalOutput.java | 87 +---- .../edu/wpi/first/wpilibj/DoubleSolenoid.java | 70 +--- .../java/edu/wpi/first/wpilibj/Encoder.java | 108 ++---- .../java/edu/wpi/first/wpilibj/GearTooth.java | 19 +- .../java/edu/wpi/first/wpilibj/GyroBase.java | 41 +- .../main/java/edu/wpi/first/wpilibj/I2C.java | 3 +- .../wpilibj/InterruptableSensorBase.java | 11 + .../wpi/first/wpilibj/IterativeRobotBase.java | 1 + .../java/edu/wpi/first/wpilibj/Jaguar.java | 3 +- .../edu/wpi/first/wpilibj/NamedSendable.java | 23 -- .../edu/wpi/first/wpilibj/NidecBrushless.java | 65 +--- .../edu/wpi/first/wpilibj/PIDController.java | 254 ++++-------- .../main/java/edu/wpi/first/wpilibj/PWM.java | 55 +-- .../edu/wpi/first/wpilibj/PWMTalonSRX.java | 3 +- .../first/wpilibj/PowerDistributionPanel.java | 69 +--- .../java/edu/wpi/first/wpilibj/Relay.java | 60 +-- .../java/edu/wpi/first/wpilibj/SD540.java | 3 +- .../main/java/edu/wpi/first/wpilibj/SPI.java | 2 +- .../java/edu/wpi/first/wpilibj/Sendable.java | 47 ++- .../edu/wpi/first/wpilibj/SendableBase.java | 94 +++++ .../edu/wpi/first/wpilibj/SensorBase.java | 32 +- .../java/edu/wpi/first/wpilibj/Servo.java | 48 +-- .../java/edu/wpi/first/wpilibj/Solenoid.java | 64 +-- .../edu/wpi/first/wpilibj/SolenoidBase.java | 2 +- .../java/edu/wpi/first/wpilibj/Spark.java | 3 +- .../first/wpilibj/SpeedControllerGroup.java | 16 +- .../java/edu/wpi/first/wpilibj/Talon.java | 3 +- .../edu/wpi/first/wpilibj/Ultrasonic.java | 48 +-- .../java/edu/wpi/first/wpilibj/Victor.java | 3 +- .../java/edu/wpi/first/wpilibj/VictorSP.java | 3 +- .../wpi/first/wpilibj/buttons/Trigger.java | 38 +- .../wpi/first/wpilibj/command/Command.java | 89 ++--- .../wpi/first/wpilibj/command/PIDCommand.java | 14 +- .../first/wpilibj/command/PIDSubsystem.java | 18 +- .../wpi/first/wpilibj/command/Scheduler.java | 99 ++--- .../wpi/first/wpilibj/command/Subsystem.java | 122 +++--- .../wpilibj/drive/DifferentialDrive.java | 14 + .../first/wpilibj/drive/KilloughDrive.java | 16 + .../wpi/first/wpilibj/drive/MecanumDrive.java | 22 ++ .../first/wpilibj/drive/RobotDriveBase.java | 4 +- .../first/wpilibj/livewindow/LiveWindow.java | 257 ++++++++---- .../livewindow/LiveWindowComponent.java | 41 -- .../livewindow/LiveWindowSendable.java | 31 -- .../smartdashboard/SendableBuilder.java | 143 +++++++ .../smartdashboard/SendableBuilderImpl.java | 346 +++++++++++++++++ .../smartdashboard/SendableChooser.java | 54 +-- .../smartdashboard/SmartDashboard.java | 43 ++- .../wpilibj/examples/gearsbot/Robot.java | 2 - .../examples/gearsbot/subsystems/Claw.java | 10 +- .../gearsbot/subsystems/DriveTrain.java | 12 +- .../gearsbot/subsystems/Elevator.java | 16 +- .../examples/gearsbot/subsystems/Wrist.java | 14 +- .../examples/gettingstarted/Robot.java | 2 - .../first/wpilibj/examples/pacgoat/Robot.java | 2 - .../pacgoat/subsystems/Collector.java | 9 +- .../pacgoat/subsystems/DriveTrain.java | 19 +- .../examples/pacgoat/subsystems/Pivot.java | 12 +- .../pacgoat/subsystems/Pneumatics.java | 3 +- .../examples/pacgoat/subsystems/Shooter.java | 11 +- .../wpilibj/templates/commandbased/Robot.java | 2 - .../java/edu/wpi/first/wpilibj/PIDTest.java | 10 +- .../first/wpilibj/RelayCrossConnectTest.java | 17 +- 213 files changed, 3704 insertions(+), 3758 deletions(-) create mode 100644 wpilibc/src/main/native/cpp/SmartDashboard/SendableBase.cpp create mode 100644 wpilibc/src/main/native/cpp/SmartDashboard/SendableBuilderImpl.cpp delete mode 100644 wpilibc/src/main/native/include/LiveWindow/LiveWindowSendable.h delete mode 100644 wpilibc/src/main/native/include/SmartDashboard/NamedSendable.h create mode 100644 wpilibc/src/main/native/include/SmartDashboard/SendableBase.h create mode 100644 wpilibc/src/main/native/include/SmartDashboard/SendableBuilder.h create mode 100644 wpilibc/src/main/native/include/SmartDashboard/SendableBuilderImpl.h delete mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/NamedSendable.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java delete mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowComponent.java delete mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowSendable.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java diff --git a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp index 64011ce68c..24d01ec702 100644 --- a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp +++ b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp @@ -9,8 +9,7 @@ #include -#include "I2C.h" -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" using namespace frc; @@ -30,7 +29,7 @@ ADXL345_I2C::ADXL345_I2C(I2C::Port port, Range range, int deviceAddress) HAL_Report(HALUsageReporting::kResourceType_ADXL345, HALUsageReporting::kADXL345_I2C, 0); - LiveWindow::GetInstance()->AddSensor("ADXL345_I2C", port, this); + SetName("ADXL345_I2C", port); } void ADXL345_I2C::SetRange(Range range) { @@ -75,25 +74,15 @@ ADXL345_I2C::AllAxes ADXL345_I2C::GetAccelerations() { return data; } -std::string ADXL345_I2C::GetSmartDashboardType() const { - return "3AxisAccelerometer"; -} - -void ADXL345_I2C::InitTable(std::shared_ptr subtable) { - if (subtable) { - m_xEntry = subtable->GetEntry("X"); - m_yEntry = subtable->GetEntry("Y"); - m_zEntry = subtable->GetEntry("Z"); - UpdateTable(); - } else { - m_xEntry = nt::NetworkTableEntry(); - m_yEntry = nt::NetworkTableEntry(); - m_zEntry = nt::NetworkTableEntry(); - } -} - -void ADXL345_I2C::UpdateTable() { - if (m_xEntry) m_xEntry.SetDouble(GetX()); - if (m_yEntry) m_yEntry.SetDouble(GetY()); - if (m_zEntry) m_zEntry.SetDouble(GetZ()); +void ADXL345_I2C::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("3AxisAccelerometer"); + auto x = builder.GetEntry("X").GetHandle(); + auto y = builder.GetEntry("Y").GetHandle(); + auto z = builder.GetEntry("Z").GetHandle(); + builder.SetUpdateTable([=]() { + auto data = GetAccelerations(); + nt::NetworkTableEntry(x).SetDouble(data.XAxis); + nt::NetworkTableEntry(y).SetDouble(data.YAxis); + nt::NetworkTableEntry(z).SetDouble(data.ZAxis); + }); } diff --git a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp index 03ae558a65..99caad7749 100644 --- a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp +++ b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp @@ -9,9 +9,7 @@ #include -#include "DigitalInput.h" -#include "DigitalOutput.h" -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" using namespace frc; @@ -40,7 +38,7 @@ ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range) HAL_Report(HALUsageReporting::kResourceType_ADXL345, HALUsageReporting::kADXL345_SPI); - LiveWindow::GetInstance()->AddSensor("ADXL345_SPI", port, this); + SetName("ADXL345_SPI", port); } void ADXL345_SPI::SetRange(Range range) { @@ -103,25 +101,15 @@ ADXL345_SPI::AllAxes ADXL345_SPI::GetAccelerations() { return data; } -std::string ADXL345_SPI::GetSmartDashboardType() const { - return "3AxisAccelerometer"; -} - -void ADXL345_SPI::InitTable(std::shared_ptr subtable) { - if (subtable) { - m_xEntry = subtable->GetEntry("X"); - m_yEntry = subtable->GetEntry("Y"); - m_zEntry = subtable->GetEntry("Z"); - UpdateTable(); - } else { - m_xEntry = nt::NetworkTableEntry(); - m_yEntry = nt::NetworkTableEntry(); - m_zEntry = nt::NetworkTableEntry(); - } -} - -void ADXL345_SPI::UpdateTable() { - if (m_xEntry) m_xEntry.SetDouble(GetX()); - if (m_yEntry) m_yEntry.SetDouble(GetY()); - if (m_zEntry) m_zEntry.SetDouble(GetZ()); +void ADXL345_SPI::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("3AxisAccelerometer"); + auto x = builder.GetEntry("X").GetHandle(); + auto y = builder.GetEntry("Y").GetHandle(); + auto z = builder.GetEntry("Z").GetHandle(); + builder.SetUpdateTable([=]() { + auto data = GetAccelerations(); + nt::NetworkTableEntry(x).SetDouble(data.XAxis); + nt::NetworkTableEntry(y).SetDouble(data.YAxis); + nt::NetworkTableEntry(z).SetDouble(data.ZAxis); + }); } diff --git a/wpilibc/src/main/native/cpp/ADXL362.cpp b/wpilibc/src/main/native/cpp/ADXL362.cpp index cc319e4a49..ebda50c468 100644 --- a/wpilibc/src/main/native/cpp/ADXL362.cpp +++ b/wpilibc/src/main/native/cpp/ADXL362.cpp @@ -9,29 +9,27 @@ #include -#include "DigitalInput.h" -#include "DigitalOutput.h" #include "DriverStation.h" -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" using namespace frc; -static int kRegWrite = 0x0A; -static int kRegRead = 0x0B; +static constexpr int kRegWrite = 0x0A; +static constexpr int kRegRead = 0x0B; -static int kPartIdRegister = 0x02; -static int kDataRegister = 0x0E; -static int kFilterCtlRegister = 0x2C; -static int kPowerCtlRegister = 0x2D; +static constexpr int kPartIdRegister = 0x02; +static constexpr int kDataRegister = 0x0E; +static constexpr int kFilterCtlRegister = 0x2C; +static constexpr int kPowerCtlRegister = 0x2D; -// static int kFilterCtl_Range2G = 0x00; -// static int kFilterCtl_Range4G = 0x40; -// static int kFilterCtl_Range8G = 0x80; -static int kFilterCtl_ODR_100Hz = 0x03; +// static constexpr int kFilterCtl_Range2G = 0x00; +// static constexpr int kFilterCtl_Range4G = 0x40; +// static constexpr int kFilterCtl_Range8G = 0x80; +static constexpr int kFilterCtl_ODR_100Hz = 0x03; -static int kPowerCtl_UltraLowNoise = 0x20; -// static int kPowerCtl_AutoSleep = 0x04; -static int kPowerCtl_Measure = 0x02; +static constexpr int kPowerCtl_UltraLowNoise = 0x20; +// static constexpr int kPowerCtl_AutoSleep = 0x04; +static constexpr int kPowerCtl_Measure = 0x02; /** * Constructor. Uses the onboard CS1. @@ -75,7 +73,7 @@ ADXL362::ADXL362(SPI::Port port, Range range) : m_spi(port) { HAL_Report(HALUsageReporting::kResourceType_ADXL362, port); - LiveWindow::GetInstance()->AddSensor("ADXL362", port, this); + SetName("ADXL362", port); } void ADXL362::SetRange(Range range) { @@ -163,25 +161,15 @@ ADXL362::AllAxes ADXL362::GetAccelerations() { return data; } -std::string ADXL362::GetSmartDashboardType() const { - return "3AxisAccelerometer"; -} - -void ADXL362::InitTable(std::shared_ptr subtable) { - if (subtable) { - m_xEntry = subtable->GetEntry("X"); - m_yEntry = subtable->GetEntry("Y"); - m_zEntry = subtable->GetEntry("Z"); - UpdateTable(); - } else { - m_xEntry = nt::NetworkTableEntry(); - m_yEntry = nt::NetworkTableEntry(); - m_zEntry = nt::NetworkTableEntry(); - } -} - -void ADXL362::UpdateTable() { - if (m_xEntry) m_xEntry.SetDouble(GetX()); - if (m_yEntry) m_yEntry.SetDouble(GetY()); - if (m_zEntry) m_zEntry.SetDouble(GetZ()); +void ADXL362::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("3AxisAccelerometer"); + auto x = builder.GetEntry("X").GetHandle(); + auto y = builder.GetEntry("Y").GetHandle(); + auto z = builder.GetEntry("Z").GetHandle(); + builder.SetUpdateTable([=]() { + auto data = GetAccelerations(); + nt::NetworkTableEntry(x).SetDouble(data.XAxis); + nt::NetworkTableEntry(y).SetDouble(data.YAxis); + nt::NetworkTableEntry(z).SetDouble(data.ZAxis); + }); } diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp index ec5d8dce59..bf96388249 100644 --- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp +++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp @@ -10,7 +10,6 @@ #include #include "DriverStation.h" -#include "LiveWindow/LiveWindow.h" #include "Timer.h" using namespace frc; @@ -81,7 +80,7 @@ ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) : m_spi(port) { Calibrate(); HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port); - LiveWindow::GetInstance()->AddSensor("ADXRS450_Gyro", port, this); + SetName("ADXRS450_Gyro", port); } static bool CalcParity(int v) { diff --git a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp index 5ffab6b108..bfe2373474 100644 --- a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp +++ b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp @@ -9,7 +9,7 @@ #include -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -20,8 +20,7 @@ using namespace frc; void AnalogAccelerometer::InitAccelerometer() { HAL_Report(HALUsageReporting::kResourceType_Accelerometer, m_analogInput->GetChannel()); - LiveWindow::GetInstance()->AddSensor("Accelerometer", - m_analogInput->GetChannel(), this); + SetName("Accelerometer", m_analogInput->GetChannel()); } /** @@ -32,9 +31,9 @@ void AnalogAccelerometer::InitAccelerometer() { * @param channel The channel number for the analog input the accelerometer is * connected to */ -AnalogAccelerometer::AnalogAccelerometer(int channel) { - m_analogInput = std::make_shared(channel); - InitAccelerometer(); +AnalogAccelerometer::AnalogAccelerometer(int channel) + : AnalogAccelerometer(std::make_shared(channel)) { + AddChild(m_analogInput); } /** @@ -116,24 +115,8 @@ void AnalogAccelerometer::SetZero(double zero) { m_zeroGVoltage = zero; } */ double AnalogAccelerometer::PIDGet() { return GetAcceleration(); } -void AnalogAccelerometer::UpdateTable() { - if (m_valueEntry) m_valueEntry.SetDouble(GetAcceleration()); -} - -void AnalogAccelerometer::StartLiveWindowMode() {} - -void AnalogAccelerometer::StopLiveWindowMode() {} - -std::string AnalogAccelerometer::GetSmartDashboardType() const { - return "Accelerometer"; -} - -void AnalogAccelerometer::InitTable( - std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void AnalogAccelerometer::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Accelerometer"); + builder.AddDoubleProperty("Value", [=]() { return GetAcceleration(); }, + nullptr); } diff --git a/wpilibc/src/main/native/cpp/AnalogGyro.cpp b/wpilibc/src/main/native/cpp/AnalogGyro.cpp index 45ae83ed6b..beadfb29b1 100644 --- a/wpilibc/src/main/native/cpp/AnalogGyro.cpp +++ b/wpilibc/src/main/native/cpp/AnalogGyro.cpp @@ -14,7 +14,6 @@ #include #include "AnalogInput.h" -#include "LiveWindow/LiveWindow.h" #include "Timer.h" #include "WPIErrors.h" @@ -27,7 +26,9 @@ using namespace frc; * be used on on-board Analog Inputs 0-1. */ AnalogGyro::AnalogGyro(int channel) - : AnalogGyro(std::make_shared(channel)) {} + : AnalogGyro(std::make_shared(channel)) { + AddChild(m_analog); +} /** * Gyro constructor with a precreated AnalogInput object. @@ -75,18 +76,9 @@ AnalogGyro::AnalogGyro(std::shared_ptr channel) * value. * @param offset Preset uncalibrated value to use as the gyro offset. */ -AnalogGyro::AnalogGyro(int channel, int center, double offset) { - m_analog = std::make_shared(channel); - InitGyro(); - int32_t status = 0; - HAL_SetAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond, - offset, center, &status); - if (status != 0) { - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - m_gyroHandle = HAL_kInvalidHandle; - return; - } - Reset(); +AnalogGyro::AnalogGyro(int channel, int center, double offset) + : AnalogGyro(std::make_shared(channel), center, offset) { + AddChild(m_analog); } /** @@ -172,8 +164,7 @@ void AnalogGyro::InitGyro() { } HAL_Report(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel()); - LiveWindow::GetInstance()->AddSensor("AnalogGyro", m_analog->GetChannel(), - this); + SetName("AnalogGyro", m_analog->GetChannel()); } void AnalogGyro::Calibrate() { diff --git a/wpilibc/src/main/native/cpp/AnalogInput.cpp b/wpilibc/src/main/native/cpp/AnalogInput.cpp index 11668c780a..20f8499e09 100644 --- a/wpilibc/src/main/native/cpp/AnalogInput.cpp +++ b/wpilibc/src/main/native/cpp/AnalogInput.cpp @@ -14,7 +14,7 @@ #include #include -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" #include "Timer.h" #include "WPIErrors.h" @@ -49,8 +49,8 @@ AnalogInput::AnalogInput(int channel) { return; } - LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this); HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel); + SetName("AnalogInput", channel); } /** @@ -414,23 +414,8 @@ double AnalogInput::PIDGet() { return GetAverageVoltage(); } -void AnalogInput::UpdateTable() { - if (m_valueEntry) m_valueEntry.SetDouble(GetAverageVoltage()); -} - -void AnalogInput::StartLiveWindowMode() {} - -void AnalogInput::StopLiveWindowMode() {} - -std::string AnalogInput::GetSmartDashboardType() const { - return "Analog Input"; -} - -void AnalogInput::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void AnalogInput::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Analog Input"); + builder.AddDoubleProperty("Value", [=]() { return GetAverageVoltage(); }, + nullptr); } diff --git a/wpilibc/src/main/native/cpp/AnalogOutput.cpp b/wpilibc/src/main/native/cpp/AnalogOutput.cpp index e877b350d1..b2b05acc96 100644 --- a/wpilibc/src/main/native/cpp/AnalogOutput.cpp +++ b/wpilibc/src/main/native/cpp/AnalogOutput.cpp @@ -14,7 +14,8 @@ #include #include -#include "LiveWindow/LiveWindow.h" +#include "SensorBase.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -51,8 +52,8 @@ AnalogOutput::AnalogOutput(int channel) { return; } - LiveWindow::GetInstance()->AddActuator("AnalogOutput", m_channel, this); HAL_Report(HALUsageReporting::kResourceType_AnalogOutput, m_channel); + SetName("AnalogOutput", m_channel); } /** @@ -93,23 +94,8 @@ double AnalogOutput::GetVoltage() const { return voltage; } -void AnalogOutput::UpdateTable() { - if (m_valueEntry) m_valueEntry.SetDouble(GetVoltage()); -} - -void AnalogOutput::StartLiveWindowMode() {} - -void AnalogOutput::StopLiveWindowMode() {} - -std::string AnalogOutput::GetSmartDashboardType() const { - return "Analog Output"; -} - -void AnalogOutput::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void AnalogOutput::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Analog Output"); + builder.AddDoubleProperty("Value", [=]() { return GetVoltage(); }, + [=](double value) { SetVoltage(value); }); } diff --git a/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp b/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp index f89d919165..e7dc559c02 100644 --- a/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp +++ b/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp @@ -8,6 +8,7 @@ #include "AnalogPotentiometer.h" #include "ControllerPower.h" +#include "SmartDashboard/SendableBuilder.h" using namespace frc; @@ -23,9 +24,11 @@ using namespace frc; */ AnalogPotentiometer::AnalogPotentiometer(int channel, double fullRange, double offset) - : m_analog_input(std::make_unique(channel)), + : m_analog_input(std::make_shared(channel)), m_fullRange(fullRange), - m_offset(offset) {} + m_offset(offset) { + AddChild(m_analog_input); +} /** * Construct an Analog Potentiometer object from an existing Analog Input @@ -76,26 +79,9 @@ double AnalogPotentiometer::Get() const { */ double AnalogPotentiometer::PIDGet() { return Get(); } -/** - * @return the Smart Dashboard Type - */ -std::string AnalogPotentiometer::GetSmartDashboardType() const { - return "Analog Input"; -} - /** * Live Window code, only does anything if live window is activated. */ -void AnalogPotentiometer::InitTable( - std::shared_ptr subtable) { - if (subtable) { - m_valueEntry = subtable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } -} - -void AnalogPotentiometer::UpdateTable() { - if (m_valueEntry) m_valueEntry.SetDouble(Get()); +void AnalogPotentiometer::InitSendable(SendableBuilder& builder) { + m_analog_input->InitSendable(builder); } diff --git a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp index 11af78e80a..b794b9bbf1 100644 --- a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp +++ b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp @@ -25,6 +25,7 @@ using namespace frc; AnalogTrigger::AnalogTrigger(int channel) : AnalogTrigger(new AnalogInput(channel)) { m_ownsAnalog = true; + AddChild(m_analogInput); } /** @@ -49,6 +50,7 @@ AnalogTrigger::AnalogTrigger(AnalogInput* input) { m_index = index; HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, input->m_channel); + SetName("AnalogTrigger", input->GetChannel()); } AnalogTrigger::~AnalogTrigger() { @@ -184,3 +186,7 @@ std::shared_ptr AnalogTrigger::CreateOutput( return std::shared_ptr( new AnalogTriggerOutput(*this, type), NullDeleter()); } + +void AnalogTrigger::InitSendable(SendableBuilder& builder) { + if (m_ownsAnalog) m_analogInput->InitSendable(builder); +} diff --git a/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp index 68cceea986..dc1d453862 100644 --- a/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp +++ b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp @@ -78,3 +78,5 @@ AnalogTriggerType AnalogTriggerOutput::GetAnalogTriggerTypeForRouting() const { * @return The channel of the source. */ int AnalogTriggerOutput::GetChannel() const { return m_trigger.m_index; } + +void AnalogTriggerOutput::InitSendable(SendableBuilder&) {} diff --git a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp index ce7f120e97..74f3afa4aa 100644 --- a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp +++ b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp @@ -10,7 +10,7 @@ #include #include -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -25,7 +25,7 @@ BuiltInAccelerometer::BuiltInAccelerometer(Range range) { HAL_Report(HALUsageReporting::kResourceType_Accelerometer, 0, 0, "Built-in accelerometer"); - LiveWindow::GetInstance()->AddSensor((std::string) "BuiltInAccel", 0, this); + SetName("BuiltInAccel", 0); } void BuiltInAccelerometer::SetRange(Range range) { @@ -54,26 +54,9 @@ double BuiltInAccelerometer::GetY() { return HAL_GetAccelerometerY(); } */ double BuiltInAccelerometer::GetZ() { return HAL_GetAccelerometerZ(); } -std::string BuiltInAccelerometer::GetSmartDashboardType() const { - return "3AxisAccelerometer"; -} - -void BuiltInAccelerometer::InitTable( - std::shared_ptr subtable) { - if (subtable != nullptr) { - m_xEntry = subtable->GetEntry("X"); - m_yEntry = subtable->GetEntry("Y"); - m_zEntry = subtable->GetEntry("Z"); - UpdateTable(); - } else { - m_xEntry = nt::NetworkTableEntry(); - m_yEntry = nt::NetworkTableEntry(); - m_zEntry = nt::NetworkTableEntry(); - } -} - -void BuiltInAccelerometer::UpdateTable() { - if (m_xEntry) m_xEntry.SetDouble(GetX()); - if (m_yEntry) m_yEntry.SetDouble(GetY()); - if (m_zEntry) m_zEntry.SetDouble(GetZ()); +void BuiltInAccelerometer::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("3AxisAccelerometer"); + builder.AddDoubleProperty("X", [=]() { return GetX(); }, nullptr); + builder.AddDoubleProperty("Y", [=]() { return GetY(); }, nullptr); + builder.AddDoubleProperty("Z", [=]() { return GetZ(); }, nullptr); } diff --git a/wpilibc/src/main/native/cpp/Buttons/Trigger.cpp b/wpilibc/src/main/native/cpp/Buttons/Trigger.cpp index 3f2d8631c4..f71e51b6b4 100644 --- a/wpilibc/src/main/native/cpp/Buttons/Trigger.cpp +++ b/wpilibc/src/main/native/cpp/Buttons/Trigger.cpp @@ -11,12 +11,11 @@ #include "Buttons/PressedButtonScheduler.h" #include "Buttons/ReleasedButtonScheduler.h" #include "Buttons/ToggleButtonScheduler.h" +#include "SmartDashboard/SendableBuilder.h" using namespace frc; -bool Trigger::Grab() { - return Get() || (m_pressedEntry && m_pressedEntry.GetBoolean(false)); -} +bool Trigger::Grab() { return Get() || m_sendablePressed; } void Trigger::WhenActive(Command* command) { auto pbs = new PressedButtonScheduler(Grab(), this, command); @@ -43,13 +42,9 @@ void Trigger::ToggleWhenActive(Command* command) { tbs->Start(); } -std::string Trigger::GetSmartDashboardType() const { return "Button"; } - -void Trigger::InitTable(std::shared_ptr subtable) { - if (subtable) { - m_pressedEntry = subtable->GetEntry("pressed"); - m_pressedEntry.SetBoolean(Get()); - } else { - m_pressedEntry = nt::NetworkTableEntry(); - } +void Trigger::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Button"); + builder.SetSafeState([=]() { m_sendablePressed = false; }); + builder.AddBooleanProperty("pressed", [=]() { return Grab(); }, + [=](bool value) { m_sendablePressed = value; }); } diff --git a/wpilibc/src/main/native/cpp/Commands/Command.cpp b/wpilibc/src/main/native/cpp/Commands/Command.cpp index 621f5a2faa..acb48f7399 100644 --- a/wpilibc/src/main/native/cpp/Commands/Command.cpp +++ b/wpilibc/src/main/native/cpp/Commands/Command.cpp @@ -11,16 +11,14 @@ #include "Commands/CommandGroup.h" #include "Commands/Scheduler.h" +#include "LiveWindow/LiveWindow.h" #include "RobotState.h" +#include "SmartDashboard/SendableBuilder.h" #include "Timer.h" #include "WPIErrors.h" using namespace frc; -static const std::string kName = ".name"; -static const std::string kRunning = "running"; -static const std::string kIsParented = ".isParented"; - int Command::m_commandCounter = 0; /** @@ -35,7 +33,7 @@ Command::Command() : Command("", -1.0) {} * * @param name the name for this command */ -Command::Command(const std::string& name) : Command(name, -1.0) {} +Command::Command(const llvm::Twine& name) : Command(name, -1.0) {} /** * Creates a new command with the given timeout and a default name. @@ -52,7 +50,8 @@ Command::Command(double timeout) : Command("", timeout) {} * @param timeout the time (in seconds) before this command "times out" * @see IsTimedOut() */ -Command::Command(const std::string& name, double timeout) { +Command::Command(const llvm::Twine& name, double timeout) + : SendableBase(false) { // We use -1.0 to indicate no timeout. if (timeout < 0.0 && timeout != -1.0) wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0"); @@ -60,17 +59,14 @@ Command::Command(const std::string& name, double timeout) { m_timeout = timeout; // If name contains an empty string - if (name.length() == 0) { - m_name = std::string("Command_") + std::string(typeid(*this).name()); + if (name.isTriviallyEmpty() || + (name.isSingleStringRef() && name.getSingleStringRef().empty())) { + SetName("Command_" + llvm::Twine(typeid(*this).name())); } else { - m_name = name; + SetName(name); } } -Command::~Command() { - if (m_runningListener != 0) m_runningEntry.RemoveListener(m_runningListener); -} - /** * Get the ID (sequence number) for this command. * @@ -144,7 +140,6 @@ void Command::Removed() { m_initialized = false; m_canceled = false; m_running = false; - if (m_runningEntry) m_runningEntry.SetBoolean(false); } /** @@ -291,10 +286,16 @@ void Command::SetParent(CommandGroup* parent) { } else { LockChanges(); m_parent = parent; - if (m_isParentedEntry) m_isParentedEntry.SetBoolean(true); } } +/** + * Returns whether the command has a parent. + * + * @param True if the command has a parent. + */ +bool Command::IsParented() const { return m_parent != nullptr; } + /** * Clears list of subsystem requirements. * @@ -317,7 +318,6 @@ void Command::ClearRequirements() { m_requirements.clear(); } void Command::StartRunning() { m_running = true; m_startTime = -1; - if (m_runningEntry) m_runningEntry.SetBoolean(true); } /** @@ -422,28 +422,17 @@ void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; } */ bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; } -std::string Command::GetName() const { return m_name; } - -std::string Command::GetSmartDashboardType() const { return "Command"; } - -void Command::InitTable(std::shared_ptr subtable) { - if (m_runningListener != 0) m_runningEntry.RemoveListener(m_runningListener); - if (subtable) { - subtable->GetEntry(kName).SetString(GetName()); - m_runningEntry = subtable->GetEntry(kRunning); - m_runningEntry.SetBoolean(IsRunning()); - m_isParentedEntry = subtable->GetEntry(kIsParented); - m_isParentedEntry.SetBoolean(m_parent != nullptr); - - m_runningListener = m_runningEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsBoolean()) return; - if (event.value->GetBoolean()) { - if (!IsRunning()) Start(); - } else { - if (IsRunning()) Cancel(); - } - }, - NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - } +void Command::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Command"); + builder.AddStringProperty(".name", [=]() { return GetName(); }, nullptr); + builder.AddBooleanProperty("running", [=]() { return IsRunning(); }, + [=](bool value) { + if (value) { + if (!IsRunning()) Start(); + } else { + if (IsRunning()) Cancel(); + } + }); + builder.AddBooleanProperty(".isParented", [=]() { return IsParented(); }, + nullptr); } diff --git a/wpilibc/src/main/native/cpp/Commands/PIDCommand.cpp b/wpilibc/src/main/native/cpp/Commands/PIDCommand.cpp index 1e7fd10aa0..28eb5f2bcc 100644 --- a/wpilibc/src/main/native/cpp/Commands/PIDCommand.cpp +++ b/wpilibc/src/main/native/cpp/Commands/PIDCommand.cpp @@ -7,6 +7,8 @@ #include "Commands/PIDCommand.h" +#include "SmartDashboard/SendableBuilder.h" + using namespace frc; PIDCommand::PIDCommand(const std::string& name, double p, double i, double d, @@ -65,9 +67,8 @@ double PIDCommand::GetSetpoint() const { return m_controller->GetSetpoint(); } double PIDCommand::GetPosition() { return ReturnPIDInput(); } -std::string PIDCommand::GetSmartDashboardType() const { return "PIDCommand"; } - -void PIDCommand::InitTable(std::shared_ptr subtable) { - m_controller->InitTable(subtable); - Command::InitTable(subtable); +void PIDCommand::InitSendable(SendableBuilder& builder) { + m_controller->InitSendable(builder); + Command::InitSendable(builder); + builder.SetSmartDashboardType("PIDCommand"); } diff --git a/wpilibc/src/main/native/cpp/Commands/PIDSubsystem.cpp b/wpilibc/src/main/native/cpp/Commands/PIDSubsystem.cpp index 5b08ed5f84..eeabf7fbf6 100644 --- a/wpilibc/src/main/native/cpp/Commands/PIDSubsystem.cpp +++ b/wpilibc/src/main/native/cpp/Commands/PIDSubsystem.cpp @@ -23,6 +23,7 @@ PIDSubsystem::PIDSubsystem(const std::string& name, double p, double i, double d) : Subsystem(name) { m_controller = std::make_shared(p, i, d, this, this); + AddChild("PIDController", m_controller); } /** @@ -38,6 +39,7 @@ PIDSubsystem::PIDSubsystem(const std::string& name, double p, double i, double d, double f) : Subsystem(name) { m_controller = std::make_shared(p, i, d, f, this, this); + AddChild("PIDController", m_controller); } /** @@ -58,6 +60,7 @@ PIDSubsystem::PIDSubsystem(const std::string& name, double p, double i, : Subsystem(name) { m_controller = std::make_shared(p, i, d, f, this, this, period); + AddChild("PIDController", m_controller); } /** @@ -72,6 +75,7 @@ PIDSubsystem::PIDSubsystem(const std::string& name, double p, double i, PIDSubsystem::PIDSubsystem(double p, double i, double d) : Subsystem("PIDSubsystem") { m_controller = std::make_shared(p, i, d, this, this); + AddChild("PIDController", m_controller); } /** @@ -87,6 +91,7 @@ PIDSubsystem::PIDSubsystem(double p, double i, double d) PIDSubsystem::PIDSubsystem(double p, double i, double d, double f) : Subsystem("PIDSubsystem") { m_controller = std::make_shared(p, i, d, f, this, this); + AddChild("PIDController", m_controller); } /** @@ -106,6 +111,7 @@ PIDSubsystem::PIDSubsystem(double p, double i, double d, double f, : Subsystem("PIDSubsystem") { m_controller = std::make_shared(p, i, d, f, this, this, period); + AddChild("PIDController", m_controller); } /** @@ -233,10 +239,3 @@ double PIDSubsystem::GetRate() { return ReturnPIDInput(); } void PIDSubsystem::PIDWrite(double output) { UsePIDOutput(output); } double PIDSubsystem::PIDGet() { return ReturnPIDInput(); } - -std::string PIDSubsystem::GetSmartDashboardType() const { return "PIDCommand"; } - -void PIDSubsystem::InitTable(std::shared_ptr subtable) { - m_controller->InitTable(subtable); - Subsystem::InitTable(subtable); -} diff --git a/wpilibc/src/main/native/cpp/Commands/Scheduler.cpp b/wpilibc/src/main/native/cpp/Commands/Scheduler.cpp index a3d42dcfcf..bb3614204b 100644 --- a/wpilibc/src/main/native/cpp/Commands/Scheduler.cpp +++ b/wpilibc/src/main/native/cpp/Commands/Scheduler.cpp @@ -13,11 +13,15 @@ #include "Buttons/ButtonScheduler.h" #include "Commands/Subsystem.h" #include "HLUsageReporting.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; -Scheduler::Scheduler() { HLUsageReporting::ReportScheduler(); } +Scheduler::Scheduler() { + HLUsageReporting::ReportScheduler(); + SetName("Scheduler"); +} /** * Returns the Scheduler, creating it if one does not exist. @@ -161,8 +165,6 @@ void Scheduler::Run() { } lock->ConfirmCommand(); } - - UpdateTable(); } /** @@ -223,12 +225,12 @@ void Scheduler::ResetAll() { m_cancelEntry = nt::NetworkTableEntry(); } -/** - * Update the network tables associated with the Scheduler object on the - * SmartDashboard. - */ -void Scheduler::UpdateTable() { - if (m_cancelEntry && m_namesEntry && m_idsEntry) { +void Scheduler::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Scheduler"); + m_namesEntry = builder.GetEntry("Names"); + m_idsEntry = builder.GetEntry("Ids"); + m_cancelEntry = builder.GetEntry("Cancel"); + builder.SetUpdateTable([=]() { // Get the list of possible commands to cancel auto new_toCancel = m_cancelEntry.GetValue(); if (new_toCancel) @@ -264,26 +266,5 @@ void Scheduler::UpdateTable() { m_namesEntry.SetStringArray(commands); m_idsEntry.SetDoubleArray(ids); } - } -} - -std::string Scheduler::GetName() const { return "Scheduler"; } - -std::string Scheduler::GetType() const { return "Scheduler"; } - -std::string Scheduler::GetSmartDashboardType() const { return "Scheduler"; } - -void Scheduler::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_namesEntry = subTable->GetEntry("Names"); - m_idsEntry = subTable->GetEntry("Ids"); - m_cancelEntry = subTable->GetEntry("Cancel"); - m_namesEntry.SetStringArray(commands); - m_idsEntry.SetDoubleArray(ids); - m_cancelEntry.SetDoubleArray(toCancel); - } else { - m_namesEntry = nt::NetworkTableEntry(); - m_idsEntry = nt::NetworkTableEntry(); - m_cancelEntry = nt::NetworkTableEntry(); - } + }); } diff --git a/wpilibc/src/main/native/cpp/Commands/Subsystem.cpp b/wpilibc/src/main/native/cpp/Commands/Subsystem.cpp index 3dcbacafed..af788e8018 100644 --- a/wpilibc/src/main/native/cpp/Commands/Subsystem.cpp +++ b/wpilibc/src/main/native/cpp/Commands/Subsystem.cpp @@ -9,6 +9,8 @@ #include "Commands/Command.h" #include "Commands/Scheduler.h" +#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -18,10 +20,11 @@ using namespace frc; * * @param name the name of the subsystem */ -Subsystem::Subsystem(const std::string& name) { - m_name = name; +Subsystem::Subsystem(const llvm::Twine& name) { + SetName(name, name); Scheduler::GetInstance()->RegisterSubsystem(this); } + /** * Initialize the default command for this subsystem. * @@ -64,14 +67,6 @@ void Subsystem::SetDefaultCommand(Command* command) { m_defaultCommand = command; } - if (m_hasDefaultEntry && m_defaultEntry) { - if (m_defaultCommand != nullptr) { - m_hasDefaultEntry.SetBoolean(true); - m_defaultEntry.SetString(m_defaultCommand->GetName()); - } else { - m_hasDefaultEntry.SetBoolean(false); - } - } } /** @@ -87,6 +82,20 @@ Command* Subsystem::GetDefaultCommand() { return m_defaultCommand; } +/** + * Returns the default command name, or empty string is there is none. + * + * @return the default command name + */ +llvm::StringRef Subsystem::GetDefaultCommandName() { + Command* defaultCommand = GetDefaultCommand(); + if (defaultCommand) { + return defaultCommand->GetName(); + } else { + return llvm::StringRef(); + } +} + /** * Sets the current command. * @@ -104,6 +113,20 @@ void Subsystem::SetCurrentCommand(Command* command) { */ Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; } +/** + * Returns the current command name, or empty string if no current command. + * + * @return the current command name + */ +llvm::StringRef Subsystem::GetCurrentCommandName() const { + Command* currentCommand = GetCurrentCommand(); + if (currentCommand) { + return currentCommand->GetName(); + } else { + return llvm::StringRef(); + } +} + /** * When the run method of the scheduler is called this method will be called. */ @@ -118,41 +141,78 @@ void Subsystem::Periodic() {} * avoid that situation. */ void Subsystem::ConfirmCommand() { - if (m_currentCommandChanged) { - if (m_hasCommandEntry && m_commandEntry) { - if (m_currentCommand != nullptr) { - m_hasCommandEntry.SetBoolean(true); - m_commandEntry.SetString(m_currentCommand->GetName()); - } else { - m_hasCommandEntry.SetBoolean(false); - } - } - m_currentCommandChanged = false; - } + if (m_currentCommandChanged) m_currentCommandChanged = false; } -std::string Subsystem::GetName() const { return m_name; } - -std::string Subsystem::GetSmartDashboardType() const { return "Subsystem"; } - -void Subsystem::InitTable(std::shared_ptr subtable) { - if (subtable != nullptr) { - m_hasDefaultEntry = subtable->GetEntry("hasDefault"); - m_defaultEntry = subtable->GetEntry("default"); - m_hasCommandEntry = subtable->GetEntry("hasCommand"); - m_commandEntry = subtable->GetEntry("command"); - - if (m_defaultCommand != nullptr) { - m_hasDefaultEntry.SetBoolean(true); - m_defaultEntry.SetString(m_defaultCommand->GetName()); - } else { - m_hasDefaultEntry.SetBoolean(false); - } - if (m_currentCommand != nullptr) { - m_hasCommandEntry.SetBoolean(true); - m_commandEntry.SetString(m_currentCommand->GetName()); - } else { - m_hasCommandEntry.SetBoolean(false); - } - } +/** + * Associate a Sendable with this Subsystem. + * Also update the child's name. + * + * @param name name to give child + * @param child sendable + */ +void Subsystem::AddChild(const llvm::Twine& name, + std::shared_ptr child) { + AddChild(name, *child); +} + +/** + * Associate a Sendable with this Subsystem. + * Also update the child's name. + * + * @param name name to give child + * @param child sendable + */ +void Subsystem::AddChild(const llvm::Twine& name, Sendable* child) { + AddChild(name, *child); +} + +/** + * Associate a Sendable with this Subsystem. + * Also update the child's name. + * + * @param name name to give child + * @param child sendable + */ +void Subsystem::AddChild(const llvm::Twine& name, Sendable& child) { + child.SetName(GetSubsystem(), name); + LiveWindow::GetInstance()->Add(&child); +} + +/** + * Associate a {@link Sendable} with this Subsystem. + * + * @param child sendable + */ +void Subsystem::AddChild(std::shared_ptr child) { AddChild(*child); } + +/** + * Associate a {@link Sendable} with this Subsystem. + * + * @param child sendable + */ +void Subsystem::AddChild(Sendable* child) { AddChild(*child); } + +/** + * Associate a {@link Sendable} with this Subsystem. + * + * @param child sendable + */ +void Subsystem::AddChild(Sendable& child) { + child.SetSubsystem(GetSubsystem()); + LiveWindow::GetInstance()->Add(&child); +} + +void Subsystem::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Subsystem"); + + builder.AddBooleanProperty( + "hasDefault", [=]() { return m_defaultCommand != nullptr; }, nullptr); + builder.AddStringProperty("default", + [=]() { return GetDefaultCommandName(); }, nullptr); + + builder.AddBooleanProperty( + "hasCommand", [=]() { return m_currentCommand != nullptr; }, nullptr); + builder.AddStringProperty("command", + [=]() { return GetCurrentCommandName(); }, nullptr); } diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp index 5df90b2917..b301453e27 100644 --- a/wpilibc/src/main/native/cpp/Compressor.cpp +++ b/wpilibc/src/main/native/cpp/Compressor.cpp @@ -12,6 +12,7 @@ #include #include +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -32,13 +33,7 @@ Compressor::Compressor(int pcmID) : m_module(pcmID) { SetClosedLoopControl(true); HAL_Report(HALUsageReporting::kResourceType_Compressor, pcmID); -} - -/** - * Destructor. - */ -Compressor::~Compressor() { - if (m_enabledListener != 0) m_enabledEntry.RemoveListener(m_enabledListener); + SetName("Compressor", pcmID); } /** @@ -308,42 +303,15 @@ void Compressor::ClearAllPCMStickyFaults() { } } -void Compressor::UpdateTable() { - if (m_enabledEntry) m_enabledEntry.SetBoolean(Enabled()); - if (m_pressureSwitchEntry) - m_pressureSwitchEntry.SetBoolean(GetPressureSwitchValue()); -} - -void Compressor::StartLiveWindowMode() { - if (m_enabledEntry) { - m_enabledListener = m_enabledEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsBoolean()) return; - if (event.value->GetBoolean()) - Start(); - else - Stop(); - }, - NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - } -} - -void Compressor::StopLiveWindowMode() { - if (m_enabledListener != 0) { - m_enabledEntry.RemoveListener(m_enabledListener); - m_enabledListener = 0; - } -} - -std::string Compressor::GetSmartDashboardType() const { return "Compressor"; } - -void Compressor::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_enabledEntry = subTable->GetEntry("Enabled"); - m_pressureSwitchEntry = subTable->GetEntry("Pressure switch"); - UpdateTable(); - } else { - m_enabledEntry = nt::NetworkTableEntry(); - m_pressureSwitchEntry = nt::NetworkTableEntry(); - } +void Compressor::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Compressor"); + builder.AddBooleanProperty("Enabled", [=]() { return Enabled(); }, + [=](bool value) { + if (value) + Start(); + else + Stop(); + }); + builder.AddBooleanProperty( + "Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr); } diff --git a/wpilibc/src/main/native/cpp/Counter.cpp b/wpilibc/src/main/native/cpp/Counter.cpp index f482ac4fde..75be52f5a0 100644 --- a/wpilibc/src/main/native/cpp/Counter.cpp +++ b/wpilibc/src/main/native/cpp/Counter.cpp @@ -11,6 +11,7 @@ #include "AnalogTrigger.h" #include "DigitalInput.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -36,6 +37,7 @@ Counter::Counter(Mode mode) { SetMaxPeriod(.5); HAL_Report(HALUsageReporting::kResourceType_Counter, m_index, mode); + SetName("Counter", m_index); } /** @@ -181,6 +183,7 @@ Counter::~Counter() { void Counter::SetUpSource(int channel) { if (StatusIsFatal()) return; SetUpSource(std::make_shared(channel)); + AddChild(m_upSource); } /** @@ -287,6 +290,7 @@ void Counter::ClearUpSource() { void Counter::SetDownSource(int channel) { if (StatusIsFatal()) return; SetDownSource(std::make_shared(channel)); + AddChild(m_downSource); } /** @@ -603,21 +607,7 @@ void Counter::SetReverseDirection(bool reverseDirection) { wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); } -void Counter::UpdateTable() { - if (m_valueEntry) m_valueEntry.SetDouble(Get()); -} - -void Counter::StartLiveWindowMode() {} - -void Counter::StopLiveWindowMode() {} - -std::string Counter::GetSmartDashboardType() const { return "Counter"; } - -void Counter::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void Counter::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Counter"); + builder.AddDoubleProperty("Value", [=]() { return Get(); }, nullptr); } diff --git a/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp index 71cf59840c..0879724a82 100644 --- a/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp +++ b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp @@ -35,6 +35,7 @@ DigitalGlitchFilter::DigitalGlitchFilter() { *index = true; HAL_Report(HALUsageReporting::kResourceType_DigitalFilter, m_channelIndex); + SetName("DigitalGlitchFilter", m_channelIndex); } DigitalGlitchFilter::~DigitalGlitchFilter() { @@ -199,3 +200,5 @@ uint64_t DigitalGlitchFilter::GetPeriodNanoSeconds() { return static_cast(fpgaCycles) * 1000L / static_cast(HAL_GetSystemClockTicksPerMicrosecond() / 4); } + +void DigitalGlitchFilter::InitSendable(SendableBuilder&) {} diff --git a/wpilibc/src/main/native/cpp/DigitalInput.cpp b/wpilibc/src/main/native/cpp/DigitalInput.cpp index f2cfedf92d..407fa8ee46 100644 --- a/wpilibc/src/main/native/cpp/DigitalInput.cpp +++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp @@ -15,7 +15,7 @@ #include #include -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -49,8 +49,8 @@ DigitalInput::DigitalInput(int channel) { return; } - LiveWindow::GetInstance()->AddSensor("DigitalInput", channel, this); HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel); + SetName("DigitalInput", channel); } /** @@ -103,23 +103,7 @@ AnalogTriggerType DigitalInput::GetAnalogTriggerTypeForRouting() const { return (AnalogTriggerType)0; } -void DigitalInput::UpdateTable() { - if (m_valueEntry) m_valueEntry.SetBoolean(Get()); -} - -void DigitalInput::StartLiveWindowMode() {} - -void DigitalInput::StopLiveWindowMode() {} - -std::string DigitalInput::GetSmartDashboardType() const { - return "DigitalInput"; -} - -void DigitalInput::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void DigitalInput::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Digital Input"); + builder.AddBooleanProperty("Value", [=]() { return Get(); }, nullptr); } diff --git a/wpilibc/src/main/native/cpp/DigitalOutput.cpp b/wpilibc/src/main/native/cpp/DigitalOutput.cpp index 2e0c00d910..99f25f0001 100644 --- a/wpilibc/src/main/native/cpp/DigitalOutput.cpp +++ b/wpilibc/src/main/native/cpp/DigitalOutput.cpp @@ -15,6 +15,8 @@ #include #include +#include "SensorBase.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -32,7 +34,7 @@ DigitalOutput::DigitalOutput(int channel) { llvm::raw_svector_ostream buf(str); m_pwmGenerator = HAL_kInvalidHandle; - if (!CheckDigitalChannel(channel)) { + if (!SensorBase::CheckDigitalChannel(channel)) { buf << "Digital Channel " << channel; wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str()); m_channel = std::numeric_limits::max(); @@ -51,13 +53,13 @@ DigitalOutput::DigitalOutput(int channel) { } HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel); + SetName("DigitalOutput", channel); } /** * Free the resources associated with a digital output. */ DigitalOutput::~DigitalOutput() { - if (m_valueListener != 0) m_valueEntry.RemoveListener(m_valueListener); if (StatusIsFatal()) return; // Disable the PWM in case it was running. DisablePWM(); @@ -190,7 +192,8 @@ void DigitalOutput::DisablePWM() { int32_t status = 0; // Disable the output by routing to a dead bit. - HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, kDigitalChannels, &status); + HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, SensorBase::kDigitalChannels, + &status); wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); HAL_FreeDigitalPWM(m_pwmGenerator, &status); @@ -215,52 +218,8 @@ void DigitalOutput::UpdateDutyCycle(double dutyCycle) { wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); } -/** - * @return The HAL Handle to the specified source. - */ -HAL_Handle DigitalOutput::GetPortHandleForRouting() const { return m_handle; } - -/** - * Is source an AnalogTrigger - */ -bool DigitalOutput::IsAnalogTrigger() const { return false; } - -/** - * @return The type of analog trigger output to be used. 0 for Digitals - */ -AnalogTriggerType DigitalOutput::GetAnalogTriggerTypeForRouting() const { - return (AnalogTriggerType)0; -} - -void DigitalOutput::UpdateTable() {} - -void DigitalOutput::StartLiveWindowMode() { - if (m_valueEntry) { - m_valueListener = m_valueEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsBoolean()) return; - Set(event.value->GetBoolean()); - }, - NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - } -} - -void DigitalOutput::StopLiveWindowMode() { - if (m_valueListener != 0) { - m_valueEntry.RemoveListener(m_valueListener); - m_valueListener = 0; - } -} - -std::string DigitalOutput::GetSmartDashboardType() const { - return "Digital Output"; -} - -void DigitalOutput::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void DigitalOutput::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Digital Output"); + builder.AddBooleanProperty("Value", [=]() { return Get(); }, + [=](bool value) { Set(value); }); } diff --git a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp index 6fc412bb82..452d09702e 100644 --- a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp +++ b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp @@ -13,7 +13,8 @@ #include #include -#include "LiveWindow/LiveWindow.h" +#include "SensorBase.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -27,7 +28,7 @@ using namespace frc; * @param reverseChannel The reverse channel number on the PCM (0..7). */ DoubleSolenoid::DoubleSolenoid(int forwardChannel, int reverseChannel) - : DoubleSolenoid(GetDefaultSolenoidModule(), forwardChannel, + : DoubleSolenoid(SensorBase::GetDefaultSolenoidModule(), forwardChannel, reverseChannel) {} /** @@ -44,17 +45,17 @@ DoubleSolenoid::DoubleSolenoid(int moduleNumber, int forwardChannel, m_reverseChannel(reverseChannel) { llvm::SmallString<32> str; llvm::raw_svector_ostream buf(str); - if (!CheckSolenoidModule(m_moduleNumber)) { + if (!SensorBase::CheckSolenoidModule(m_moduleNumber)) { buf << "Solenoid Module " << m_moduleNumber; wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf.str()); return; } - if (!CheckSolenoidChannel(m_forwardChannel)) { + if (!SensorBase::CheckSolenoidChannel(m_forwardChannel)) { buf << "Solenoid Module " << m_forwardChannel; wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str()); return; } - if (!CheckSolenoidChannel(m_reverseChannel)) { + if (!SensorBase::CheckSolenoidChannel(m_reverseChannel)) { buf << "Solenoid Module " << m_reverseChannel; wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str()); return; @@ -89,8 +90,7 @@ DoubleSolenoid::DoubleSolenoid(int moduleNumber, int forwardChannel, m_moduleNumber); HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel, m_moduleNumber); - LiveWindow::GetInstance()->AddActuator("DoubleSolenoid", m_moduleNumber, - m_forwardChannel, this); + SetName("DoubleSolenoid", m_moduleNumber, m_forwardChannel); } /** @@ -99,7 +99,6 @@ DoubleSolenoid::DoubleSolenoid(int moduleNumber, int forwardChannel, DoubleSolenoid::~DoubleSolenoid() { HAL_FreeSolenoidPort(m_forwardHandle); HAL_FreeSolenoidPort(m_reverseHandle); - if (m_valueListener != 0) m_valueEntry.RemoveListener(m_valueListener); } /** @@ -183,56 +182,27 @@ bool DoubleSolenoid::IsRevSolenoidBlackListed() const { return (blackList & m_reverseMask) != 0; } -void DoubleSolenoid::UpdateTable() { - if (m_valueEntry) { - switch (Get()) { - case kForward: - m_valueEntry.SetString("Forward"); - break; - case kReverse: - m_valueEntry.SetString("Reverse"); - break; - default: - m_valueEntry.SetString("Off"); - break; - } - } -} - -void DoubleSolenoid::StartLiveWindowMode() { - Set(kOff); - if (m_valueEntry) { - m_valueListener = m_valueEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsString()) return; - Value lvalue = kOff; - if (event.value->GetString() == "Forward") - lvalue = kForward; - else if (event.value->GetString() == "Reverse") - lvalue = kReverse; - Set(lvalue); - }, - NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - } -} - -void DoubleSolenoid::StopLiveWindowMode() { - Set(kOff); - if (m_valueListener != 0) { - m_valueEntry.RemoveListener(m_valueListener); - m_valueListener = 0; - } -} - -std::string DoubleSolenoid::GetSmartDashboardType() const { - return "Double Solenoid"; -} - -void DoubleSolenoid::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void DoubleSolenoid::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Double Solenoid"); + builder.SetSafeState([=]() { Set(kOff); }); + builder.AddSmallStringProperty( + "Value", + [=](llvm::SmallVectorImpl& buf) -> llvm::StringRef { + switch (Get()) { + case kForward: + return "Forward"; + case kReverse: + return "Reverse"; + default: + return "Off"; + } + }, + [=](llvm::StringRef value) { + Value lvalue = kOff; + if (value == "Forward") + lvalue = kForward; + else if (value == "Reverse") + lvalue = kReverse; + Set(lvalue); + }); } diff --git a/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp index cc071aca85..0e1ffc1221 100644 --- a/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp +++ b/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp @@ -11,6 +11,7 @@ #include +#include "SmartDashboard/SendableBuilder.h" #include "SpeedController.h" using namespace frc; @@ -23,7 +24,13 @@ using namespace frc; */ DifferentialDrive::DifferentialDrive(SpeedController& leftMotor, SpeedController& rightMotor) - : m_leftMotor(leftMotor), m_rightMotor(rightMotor) {} + : m_leftMotor(leftMotor), m_rightMotor(rightMotor) { + AddChild(&m_leftMotor); + AddChild(&m_rightMotor); + static int instances = 0; + ++instances; + SetName("DifferentialDrive", instances); +} /** * Arcade drive method for differential drive platform. @@ -249,3 +256,13 @@ void DifferentialDrive::StopMotor() { void DifferentialDrive::GetDescription(llvm::raw_ostream& desc) const { desc << "DifferentialDrive"; } + +void DifferentialDrive::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("DifferentialDrive"); + builder.AddDoubleProperty("Left Motor Speed", + [=]() { return m_leftMotor.Get(); }, + [=](double value) { m_leftMotor.Set(value); }); + builder.AddDoubleProperty("Right Motor Speed", + [=]() { return m_rightMotor.Get(); }, + [=](double value) { m_rightMotor.Set(value); }); +} diff --git a/wpilibc/src/main/native/cpp/Drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/Drive/KilloughDrive.cpp index 89279b6a09..8ae301ecd9 100644 --- a/wpilibc/src/main/native/cpp/Drive/KilloughDrive.cpp +++ b/wpilibc/src/main/native/cpp/Drive/KilloughDrive.cpp @@ -12,6 +12,7 @@ #include +#include "SmartDashboard/SendableBuilder.h" #include "SpeedController.h" using namespace frc; @@ -62,6 +63,12 @@ KilloughDrive::KilloughDrive(SpeedController& leftMotor, std::sin(rightMotorAngle * (kPi / 180.0))}; m_backVec = {std::cos(backMotorAngle * (kPi / 180.0)), std::sin(backMotorAngle * (kPi / 180.0))}; + AddChild(&m_leftMotor); + AddChild(&m_rightMotor); + AddChild(&m_backMotor); + static int instances = 0; + ++instances; + SetName("KilloughDrive", instances); } /** @@ -146,3 +153,16 @@ void KilloughDrive::StopMotor() { void KilloughDrive::GetDescription(llvm::raw_ostream& desc) const { desc << "KilloughDrive"; } + +void KilloughDrive::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("KilloughDrive"); + builder.AddDoubleProperty("Left Motor Speed", + [=]() { return m_leftMotor.Get(); }, + [=](double value) { m_leftMotor.Set(value); }); + builder.AddDoubleProperty("Right Motor Speed", + [=]() { return m_rightMotor.Get(); }, + [=](double value) { m_rightMotor.Set(value); }); + builder.AddDoubleProperty("Back Motor Speed", + [=]() { return m_backMotor.Get(); }, + [=](double value) { m_backMotor.Set(value); }); +} diff --git a/wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp index 032efb59b6..3ed7ae52f1 100644 --- a/wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp +++ b/wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp @@ -13,6 +13,7 @@ #include #include "Drive/Vector2d.h" +#include "SmartDashboard/SendableBuilder.h" #include "SpeedController.h" using namespace frc; @@ -31,7 +32,15 @@ MecanumDrive::MecanumDrive(SpeedController& frontLeftMotor, : m_frontLeftMotor(frontLeftMotor), m_rearLeftMotor(rearLeftMotor), m_frontRightMotor(frontRightMotor), - m_rearRightMotor(rearRightMotor) {} + m_rearRightMotor(rearRightMotor) { + AddChild(&m_frontLeftMotor); + AddChild(&m_rearLeftMotor); + AddChild(&m_frontRightMotor); + AddChild(&m_rearRightMotor); + static int instances = 0; + ++instances; + SetName("MecanumDrive", instances); +} /** * Drive method for Mecanum platform. @@ -118,3 +127,19 @@ void MecanumDrive::StopMotor() { void MecanumDrive::GetDescription(llvm::raw_ostream& desc) const { desc << "MecanumDrive"; } + +void MecanumDrive::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("MecanumDrive"); + builder.AddDoubleProperty("Front Left Motor Speed", + [=]() { return m_frontLeftMotor.Get(); }, + [=](double value) { m_frontLeftMotor.Set(value); }); + builder.AddDoubleProperty( + "Front Right Motor Speed", [=]() { return m_frontRightMotor.Get(); }, + [=](double value) { m_frontRightMotor.Set(value); }); + builder.AddDoubleProperty("Rear Left Motor Speed", + [=]() { return m_rearLeftMotor.Get(); }, + [=](double value) { m_rearLeftMotor.Set(value); }); + builder.AddDoubleProperty("Rear Right Motor Speed", + [=]() { return m_rearRightMotor.Get(); }, + [=](double value) { m_rearRightMotor.Set(value); }); +} diff --git a/wpilibc/src/main/native/cpp/Encoder.cpp b/wpilibc/src/main/native/cpp/Encoder.cpp index e5a3cbf716..051f87779d 100644 --- a/wpilibc/src/main/native/cpp/Encoder.cpp +++ b/wpilibc/src/main/native/cpp/Encoder.cpp @@ -10,7 +10,7 @@ #include #include "DigitalInput.h" -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -45,8 +45,7 @@ void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) { HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex(), encodingType); - LiveWindow::GetInstance()->AddSensor("Encoder", m_aSource->GetChannel(), - this); + SetName("Encoder", m_aSource->GetChannel()); } /** @@ -77,6 +76,8 @@ Encoder::Encoder(int aChannel, int bChannel, bool reverseDirection, m_aSource = std::make_shared(aChannel); m_bSource = std::make_shared(bChannel); InitEncoder(reverseDirection, encodingType); + AddChild(m_aSource); + AddChild(m_bSource); } /** @@ -371,6 +372,19 @@ void Encoder::SetDistancePerPulse(double distancePerPulse) { wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); } +/** + * Get the distance per pulse for this encoder. + * + * @return The scale factor that will be used to convert pulses to useful units. + */ +double Encoder::GetDistancePerPulse() const { + if (StatusIsFatal()) return 0.0; + int32_t status = 0; + double distancePerPulse = HAL_GetEncoderDistancePerPulse(m_encoder, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + return distancePerPulse; +} + /** * Set the direction sensing for this encoder. * @@ -479,41 +493,18 @@ int Encoder::GetFPGAIndex() const { return val; } -void Encoder::UpdateTable() { - if (m_speedEntry) m_speedEntry.SetDouble(GetRate()); - if (m_distanceEntry) m_distanceEntry.SetDouble(GetDistance()); - if (m_distancePerTickEntry) { - int32_t status = 0; - double distancePerPulse = - HAL_GetEncoderDistancePerPulse(m_encoder, &status); - wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - m_distancePerTickEntry.SetDouble(distancePerPulse); - } -} - -void Encoder::StartLiveWindowMode() {} - -void Encoder::StopLiveWindowMode() {} - -std::string Encoder::GetSmartDashboardType() const { +void Encoder::InitSendable(SendableBuilder& builder) { int32_t status = 0; HAL_EncoderEncodingType type = HAL_GetEncoderEncodingType(m_encoder, &status); wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); if (type == HAL_EncoderEncodingType::HAL_Encoder_k4X) - return "Quadrature Encoder"; + builder.SetSmartDashboardType("Quadrature Encoder"); else - return "Encoder"; -} + builder.SetSmartDashboardType("Encoder"); -void Encoder::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_speedEntry = subTable->GetEntry("Speed"); - m_distanceEntry = subTable->GetEntry("Distance"); - m_distancePerTickEntry = subTable->GetEntry("Distance per Tick"); - UpdateTable(); - } else { - m_speedEntry = nt::NetworkTableEntry(); - m_distanceEntry = nt::NetworkTableEntry(); - m_distancePerTickEntry = nt::NetworkTableEntry(); - } + builder.AddDoubleProperty("Speed", [=]() { return GetRate(); }, nullptr); + builder.AddDoubleProperty("Distance", [=]() { return GetDistance(); }, + nullptr); + builder.AddDoubleProperty("Distance per Tick", + [=]() { return GetDistancePerPulse(); }, nullptr); } diff --git a/wpilibc/src/main/native/cpp/GearTooth.cpp b/wpilibc/src/main/native/cpp/GearTooth.cpp index 4c8d5e4b09..5a3733b5d5 100644 --- a/wpilibc/src/main/native/cpp/GearTooth.cpp +++ b/wpilibc/src/main/native/cpp/GearTooth.cpp @@ -7,7 +7,7 @@ #include "GearTooth.h" -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" using namespace frc; @@ -32,7 +32,7 @@ void GearTooth::EnableDirectionSensing(bool directionSensitive) { */ GearTooth::GearTooth(int channel, bool directionSensitive) : Counter(channel) { EnableDirectionSensing(directionSensitive); - LiveWindow::GetInstance()->AddSensor("GearTooth", channel, this); + SetName("GearTooth", channel); } /** @@ -48,6 +48,7 @@ GearTooth::GearTooth(int channel, bool directionSensitive) : Counter(channel) { GearTooth::GearTooth(DigitalSource* source, bool directionSensitive) : Counter(source) { EnableDirectionSensing(directionSensitive); + SetName("GearTooth", source->GetChannel()); } /** @@ -64,6 +65,10 @@ GearTooth::GearTooth(std::shared_ptr source, bool directionSensitive) : Counter(source) { EnableDirectionSensing(directionSensitive); + SetName("GearTooth", source->GetChannel()); } -std::string GearTooth::GetSmartDashboardType() const { return "GearTooth"; } +void GearTooth::InitSendable(SendableBuilder& builder) { + Counter::InitSendable(builder); + builder.SetSmartDashboardType("Gear Tooth"); +} diff --git a/wpilibc/src/main/native/cpp/GyroBase.cpp b/wpilibc/src/main/native/cpp/GyroBase.cpp index c323589a63..9c6527d70c 100644 --- a/wpilibc/src/main/native/cpp/GyroBase.cpp +++ b/wpilibc/src/main/native/cpp/GyroBase.cpp @@ -7,7 +7,7 @@ #include "GyroBase.h" -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -29,21 +29,7 @@ double GyroBase::PIDGet() { } } -void GyroBase::UpdateTable() { - if (m_valueEntry) m_valueEntry.SetDouble(GetAngle()); -} - -void GyroBase::StartLiveWindowMode() {} - -void GyroBase::StopLiveWindowMode() {} - -std::string GyroBase::GetSmartDashboardType() const { return "Gyro"; } - -void GyroBase::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void GyroBase::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Gyro"); + builder.AddDoubleProperty("Value", [=]() { return GetAngle(); }, nullptr); } diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp index ad42abef4f..57dcc1ea55 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp @@ -12,6 +12,7 @@ #include #include +#include "Commands/Scheduler.h" #include "LiveWindow/LiveWindow.h" using namespace frc; @@ -191,4 +192,5 @@ void IterativeRobotBase::LoopFunc() { TestPeriodic(); } RobotPeriodic(); + LiveWindow::GetInstance()->UpdateValues(); } diff --git a/wpilibc/src/main/native/cpp/Jaguar.cpp b/wpilibc/src/main/native/cpp/Jaguar.cpp index c38fe80533..92324e7d7e 100644 --- a/wpilibc/src/main/native/cpp/Jaguar.cpp +++ b/wpilibc/src/main/native/cpp/Jaguar.cpp @@ -9,8 +9,6 @@ #include -#include "LiveWindow/LiveWindow.h" - using namespace frc; /** @@ -35,5 +33,5 @@ Jaguar::Jaguar(int channel) : PWMSpeedController(channel) { SetZeroLatch(); HAL_Report(HALUsageReporting::kResourceType_Jaguar, GetChannel()); - LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this); + SetName("Jaguar", GetChannel()); } diff --git a/wpilibc/src/main/native/cpp/LiveWindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/LiveWindow/LiveWindow.cpp index 6af46505d6..1a9aea003a 100644 --- a/wpilibc/src/main/native/cpp/LiveWindow/LiveWindow.cpp +++ b/wpilibc/src/main/native/cpp/LiveWindow/LiveWindow.cpp @@ -9,14 +9,52 @@ #include +#include #include #include +#include +#include "Commands/Scheduler.h" +#include "SmartDashboard/SendableBuilderImpl.h" #include "networktables/NetworkTable.h" +#include "networktables/NetworkTableEntry.h" #include "networktables/NetworkTableInstance.h" using namespace frc; +using llvm::Twine; + +struct LiveWindow::Impl { + Impl(); + + struct Component { + std::shared_ptr sendable; + Sendable* parent = nullptr; + SendableBuilderImpl builder; + bool firstTime = true; + bool telemetryEnabled = true; + }; + + wpi::mutex mutex; + + llvm::DenseMap components; + + std::shared_ptr liveWindowTable; + std::shared_ptr statusTable; + nt::NetworkTableEntry enabledEntry; + + bool startLiveWindow = false; + bool liveWindowEnabled = false; + bool telemetryEnabled = true; +}; + +LiveWindow::Impl::Impl() + : liveWindowTable( + nt::NetworkTableInstance::GetDefault().GetTable("LiveWindow")) { + statusTable = liveWindowTable->GetSubTable(".status"); + enabledEntry = statusTable->GetEntry("LW Enabled"); +} + /** * Get an instance of the LiveWindow main class. * @@ -33,11 +71,11 @@ LiveWindow* LiveWindow::GetInstance() { * * Allocate the necessary tables. */ -LiveWindow::LiveWindow() : m_scheduler(Scheduler::GetInstance()) { - m_liveWindowTable = - nt::NetworkTableInstance::GetDefault().GetTable("LiveWindow"); - m_statusTable = m_liveWindowTable->GetSubTable(".status"); - m_enabledEntry = m_statusTable->GetEntry("LW Enabled"); +LiveWindow::LiveWindow() : m_impl(new Impl) {} + +bool LiveWindow::IsEnabled() const { + std::lock_guard lock(m_impl->mutex); + return m_impl->liveWindowEnabled; } /** @@ -46,25 +84,21 @@ LiveWindow::LiveWindow() : m_scheduler(Scheduler::GetInstance()) { * If it changes to enabled, start livewindow running otherwise stop it */ void LiveWindow::SetEnabled(bool enabled) { - if (m_enabled == enabled) return; + std::lock_guard lock(m_impl->mutex); + if (m_impl->liveWindowEnabled == enabled) return; + Scheduler* scheduler = Scheduler::GetInstance(); if (enabled) { - if (m_firstTime) { - InitializeLiveWindowComponents(); - m_firstTime = false; - } - m_scheduler->SetEnabled(false); - m_scheduler->RemoveAll(); - for (auto& elem : m_components) { - elem.first->StartLiveWindowMode(); - } + scheduler->SetEnabled(false); + scheduler->RemoveAll(); } else { - for (auto& elem : m_components) { - elem.first->StopLiveWindowMode(); + for (auto& i : m_impl->components) { + i.getSecond().builder.StopLiveWindowMode(); } - m_scheduler->SetEnabled(true); + scheduler->SetEnabled(true); } - m_enabled = enabled; - m_enabledEntry.SetBoolean(m_enabled); + m_impl->startLiveWindow = enabled; + m_impl->liveWindowEnabled = enabled; + m_impl->enabledEntry.SetBoolean(enabled); } /** @@ -74,41 +108,38 @@ void LiveWindow::SetEnabled(bool enabled) { * * @param subsystem The subsystem this component is part of. * @param name The name of this component. - * @param component A LiveWindowSendable component that represents a sensor. + * @param component A Sendable component that represents a sensor. */ //@{ /** * @brief Use a STL smart pointer to share ownership of component. + * @deprecated Use Sendable::SetName() instead. */ -void LiveWindow::AddSensor(const std::string& subsystem, - const std::string& name, - std::shared_ptr component) { - m_components[component].subsystem = subsystem; - m_components[component].name = name; - m_components[component].isSensor = true; +void LiveWindow::AddSensor(const llvm::Twine& subsystem, + const llvm::Twine& name, + std::shared_ptr component) { + Add(component); + component->SetName(subsystem, name); } /** * @brief Pass a reference to LiveWindow and retain ownership of the component. + * @deprecated Use Sendable::SetName() instead. */ -void LiveWindow::AddSensor(const std::string& subsystem, - const std::string& name, - LiveWindowSendable& component) { - AddSensor(subsystem, name, - std::shared_ptr(&component, - [](LiveWindowSendable*) {})); +void LiveWindow::AddSensor(const llvm::Twine& subsystem, + const llvm::Twine& name, Sendable& component) { + Add(&component); + component.SetName(subsystem, name); } /** * @brief Use a raw pointer to the LiveWindow. - * @deprecated Prefer smart pointers or references. + * @deprecated Use Sendable::SetName() instead. */ -void LiveWindow::AddSensor(const std::string& subsystem, - const std::string& name, - LiveWindowSendable* component) { - AddSensor(subsystem, name, - std::shared_ptr( - component, NullDeleter())); +void LiveWindow::AddSensor(const llvm::Twine& subsystem, + const llvm::Twine& name, Sendable* component) { + Add(component); + component->SetName(subsystem, name); } //@} @@ -116,87 +147,161 @@ void LiveWindow::AddSensor(const std::string& subsystem, * @name AddActuator(subsystem, name, component) * * Add an Actuator associated with the subsystem and call it by the given name. + * @deprecated Use Sendable::SetName() instead. * * @param subsystem The subsystem this component is part of. * @param name The name of this component. - * @param component A LiveWindowSendable component that represents a actuator. + * @param component A Sendable component that represents a actuator. */ //@{ /** * @brief Use a STL smart pointer to share ownership of component. */ -void LiveWindow::AddActuator(const std::string& subsystem, - const std::string& name, - std::shared_ptr component) { - m_components[component].subsystem = subsystem; - m_components[component].name = name; - m_components[component].isSensor = false; +void LiveWindow::AddActuator(const llvm::Twine& subsystem, + const llvm::Twine& name, + std::shared_ptr component) { + Add(component); + component->SetName(subsystem, name); } /** * @brief Pass a reference to LiveWindow and retain ownership of the component. + * @deprecated Use Sendable::SetName() instead. */ -void LiveWindow::AddActuator(const std::string& subsystem, - const std::string& name, - LiveWindowSendable& component) { - AddActuator(subsystem, name, - std::shared_ptr(&component, - [](LiveWindowSendable*) {})); +void LiveWindow::AddActuator(const llvm::Twine& subsystem, + const llvm::Twine& name, Sendable& component) { + Add(&component); + component.SetName(subsystem, name); } /** * @brief Use a raw pointer to the LiveWindow. - * @deprecated Prefer smart pointers or references. + * @deprecated Use Sendable::SetName() instead. */ -void LiveWindow::AddActuator(const std::string& subsystem, - const std::string& name, - LiveWindowSendable* component) { - AddActuator(subsystem, name, - std::shared_ptr( - component, NullDeleter())); +void LiveWindow::AddActuator(const llvm::Twine& subsystem, + const llvm::Twine& name, Sendable* component) { + Add(component); + component->SetName(subsystem, name); } //@} /** * Meant for internal use in other WPILib classes. + * @deprecated Use SensorBase::SetName() instead. */ -void LiveWindow::AddSensor(std::string type, int channel, - LiveWindowSendable* component) { - llvm::SmallString<128> buf; - llvm::raw_svector_ostream oss(buf); - oss << type << "[" << channel << "]"; - AddSensor("Ungrouped", oss.str(), component); - std::shared_ptr component_stl( - component, NullDeleter()); - if (std::find(m_sensors.begin(), m_sensors.end(), component_stl) == - m_sensors.end()) - m_sensors.push_back(component_stl); +void LiveWindow::AddSensor(const llvm::Twine& type, int channel, + Sendable* component) { + Add(component); + component->SetName("Ungrouped", + type + Twine('[') + Twine(channel) + Twine(']')); } /** * Meant for internal use in other WPILib classes. + * @deprecated Use SensorBase::SetName() instead. */ -void LiveWindow::AddActuator(std::string type, int channel, - LiveWindowSendable* component) { - llvm::SmallString<128> buf; - llvm::raw_svector_ostream oss(buf); - oss << type << "[" << channel << "]"; - AddActuator("Ungrouped", oss.str(), - std::shared_ptr(component, - [](LiveWindowSendable*) {})); +void LiveWindow::AddActuator(const llvm::Twine& type, int channel, + Sendable* component) { + Add(component); + component->SetName("Ungrouped", + type + Twine('[') + Twine(channel) + Twine(']')); } /** * Meant for internal use in other WPILib classes. + * @deprecated Use SensorBase::SetName() instead. */ -void LiveWindow::AddActuator(std::string type, int module, int channel, - LiveWindowSendable* component) { - llvm::SmallString<128> buf; - llvm::raw_svector_ostream oss(buf); - oss << type << "[" << module << "," << channel << "]"; - AddActuator("Ungrouped", oss.str(), - std::shared_ptr(component, - [](LiveWindowSendable*) {})); +void LiveWindow::AddActuator(const llvm::Twine& type, int module, int channel, + Sendable* component) { + Add(component); + component->SetName("Ungrouped", type + Twine('[') + Twine(module) + + Twine(',') + Twine(channel) + Twine(']')); +} + +/** + * Add a component to the LiveWindow. + * + * @param sendable component to add + */ +void LiveWindow::Add(std::shared_ptr sendable) { + std::lock_guard lock(m_impl->mutex); + auto& comp = m_impl->components[sendable.get()]; + comp.sendable = sendable; +} + +/** + * Add a component to the LiveWindow. + * + * @param sendable component to add + */ +void LiveWindow::Add(Sendable* sendable) { + Add(std::shared_ptr(sendable, NullDeleter())); +} + +/** + * Add a child component to a component. + * + * @param parent parent component + * @param child child component + */ +void LiveWindow::AddChild(Sendable* parent, std::shared_ptr child) { + AddChild(parent, child.get()); +} + +/** + * Add a child component to a component. + * + * @param parent parent component + * @param child child component + */ +void LiveWindow::AddChild(Sendable* parent, void* child) { + std::lock_guard lock(m_impl->mutex); + auto& comp = m_impl->components[child]; + comp.parent = parent; + comp.telemetryEnabled = false; +} + +/** + * Remove the component from the LiveWindow. + * + * @param sendable component to remove + */ +void LiveWindow::Remove(Sendable* sendable) { + std::lock_guard lock(m_impl->mutex); + m_impl->components.erase(sendable); +} + +/** + * Enable telemetry for a single component. + * + * @param sendable component + */ +void LiveWindow::EnableTelemetry(Sendable* sendable) { + std::lock_guard lock(m_impl->mutex); + // Re-enable global setting in case DisableAllTelemetry() was called. + m_impl->telemetryEnabled = true; + auto i = m_impl->components.find(sendable); + if (i != m_impl->components.end()) i->getSecond().telemetryEnabled = true; +} + +/** + * Disable telemetry for a single component. + * + * @param sendable component + */ +void LiveWindow::DisableTelemetry(Sendable* sendable) { + std::lock_guard lock(m_impl->mutex); + auto i = m_impl->components.find(sendable); + if (i != m_impl->components.end()) i->getSecond().telemetryEnabled = false; +} + +/** + * Disable ALL telemetry. + */ +void LiveWindow::DisableAllTelemetry() { + std::lock_guard lock(m_impl->mutex); + m_impl->telemetryEnabled = false; + for (auto& i : m_impl->components) i.getSecond().telemetryEnabled = false; } /** @@ -206,44 +311,41 @@ void LiveWindow::AddActuator(std::string type, int module, int channel, * SmartDashboard widgets. */ void LiveWindow::UpdateValues() { - for (auto& elem : m_sensors) { - elem->UpdateTable(); - } -} + std::lock_guard lock(m_impl->mutex); + // Only do this if either LiveWindow mode or telemetry is enabled. + if (!m_impl->liveWindowEnabled && !m_impl->telemetryEnabled) return; -/** - * This method is called periodically to cause the sensors to send new values - * to the SmartDashboard. - */ -void LiveWindow::Run() { - if (m_enabled) { - UpdateValues(); - } -} + for (auto& i : m_impl->components) { + auto& comp = i.getSecond(); + if (comp.sendable && !comp.parent && + (m_impl->liveWindowEnabled || comp.telemetryEnabled)) { + if (comp.firstTime) { + // By holding off creating the NetworkTable entries, it allows the + // components to be redefined. This allows default sensor and actuator + // values to be created that are replaced with the custom names from + // users calling setName. + auto name = comp.sendable->GetName(); + if (name.empty()) continue; + auto subsystem = comp.sendable->GetSubsystem(); + auto ssTable = m_impl->liveWindowTable->GetSubTable(subsystem); + std::shared_ptr table; + // Treat name==subsystem as top level of subsystem + if (name == subsystem) + table = ssTable; + else + table = ssTable->GetSubTable(name); + table->GetEntry(".name").SetString(name); + comp.builder.SetTable(table); + comp.sendable->InitSendable(comp.builder); + ssTable->GetEntry(".type").SetString("LW Subsystem"); -/** - * Initialize all the LiveWindow elements the first time we enter LiveWindow - * mode. By holding off creating the NetworkTable entries, it allows them to be - * redefined before the first time in LiveWindow mode. This allows default - * sensor and actuator values to be created that are replaced with the custom - * names from users calling addActuator and addSensor. - */ -void LiveWindow::InitializeLiveWindowComponents() { - for (auto& elem : m_components) { - std::shared_ptr component = elem.first; - LiveWindowComponent c = elem.second; - std::string subsystem = c.subsystem; - std::string name = c.name; - m_liveWindowTable->GetSubTable(subsystem)->GetEntry(".type").SetString( - "LW Subsystem"); - std::shared_ptr table( - m_liveWindowTable->GetSubTable(subsystem)->GetSubTable(name)); - table->GetEntry(".type").SetString(component->GetSmartDashboardType()); - table->GetEntry(".name").SetString(name); - table->GetEntry("Subsystem").SetString(subsystem); - component->InitTable(table); - if (c.isSensor) { - m_sensors.push_back(component); + comp.firstTime = false; + } + + if (m_impl->startLiveWindow) comp.builder.StartLiveWindowMode(); + comp.builder.UpdateTable(); } } + + m_impl->startLiveWindow = false; } diff --git a/wpilibc/src/main/native/cpp/NidecBrushless.cpp b/wpilibc/src/main/native/cpp/NidecBrushless.cpp index e81bfe4395..b47a9ed33c 100644 --- a/wpilibc/src/main/native/cpp/NidecBrushless.cpp +++ b/wpilibc/src/main/native/cpp/NidecBrushless.cpp @@ -9,7 +9,7 @@ #include -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" using namespace frc; @@ -23,6 +23,8 @@ using namespace frc; */ NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel) : m_safetyHelper(this), m_dio(dioChannel), m_pwm(pwmChannel) { + AddChild(&m_dio); + AddChild(&m_pwm); m_safetyHelper.SetExpiration(0.0); m_safetyHelper.SetSafetyEnabled(false); @@ -30,8 +32,8 @@ NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel) m_dio.SetPWMRate(15625); m_dio.EnablePWM(0.5); - LiveWindow::GetInstance()->AddActuator("Nidec Brushless", pwmChannel, this); HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel); + SetName("Nidec Brushless", pwmChannel); } /** @@ -145,44 +147,9 @@ void NidecBrushless::Enable() { m_disabled = false; } */ int NidecBrushless::GetChannel() const { return m_pwm.GetChannel(); } -/* - * Live Window code, only does anything if live window is activated. - */ -std::string NidecBrushless::GetSmartDashboardType() const { - return "Nidec Brushless"; -} - -void NidecBrushless::InitTable(std::shared_ptr subtable) { - if (subtable) { - m_valueEntry = subtable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } -} - -void NidecBrushless::UpdateTable() { - if (m_valueEntry) { - m_valueEntry.SetDouble(Get()); - } -} - -void NidecBrushless::StartLiveWindowMode() { - Set(0); // Stop for safety - if (m_valueEntry) { - m_valueListener = m_valueEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsDouble()) return; - Set(event.value->GetDouble()); - }, - NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - } -} - -void NidecBrushless::StopLiveWindowMode() { - Set(0); // Stop for safety - if (m_valueListener != 0) { - m_valueEntry.RemoveListener(m_valueListener); - m_valueListener = 0; - } +void NidecBrushless::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Nidec Brushless"); + builder.SetSafeState([=]() { StopMotor(); }); + builder.AddDoubleProperty("Value", [=]() { return Get(); }, + [=](double value) { Set(value); }); } diff --git a/wpilibc/src/main/native/cpp/PIDController.cpp b/wpilibc/src/main/native/cpp/PIDController.cpp index 0bbce79c63..995bf50abb 100644 --- a/wpilibc/src/main/native/cpp/PIDController.cpp +++ b/wpilibc/src/main/native/cpp/PIDController.cpp @@ -16,16 +16,10 @@ #include "Notifier.h" #include "PIDOutput.h" #include "PIDSource.h" +#include "SmartDashboard/SendableBuilder.h" using namespace frc; -static const std::string kP = "p"; -static const std::string kI = "i"; -static const std::string kD = "d"; -static const std::string kF = "f"; -static const std::string kSetpoint = "setpoint"; -static const std::string kEnabled = "enabled"; - template constexpr const T& clamp(const T& value, const T& low, const T& high) { return std::max(low, std::min(value, high)); @@ -61,7 +55,8 @@ PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource* source, */ PIDController::PIDController(double Kp, double Ki, double Kd, double Kf, PIDSource* source, PIDOutput* output, - double period) { + double period) + : SendableBase(false) { m_controlLoop = std::make_unique(&PIDController::Calculate, this); m_P = Kp; @@ -85,6 +80,7 @@ PIDController::PIDController(double Kp, double Ki, double Kd, double Kf, static int instances = 0; instances++; HAL_Report(HALUsageReporting::kResourceType_PIDController, instances); + SetName("PIDController", instances); } /** @@ -123,7 +119,6 @@ PIDController::PIDController(double Kp, double Ki, double Kd, double Kf, PIDController::~PIDController() { // forcefully stopping the notifier so the callback can successfully run. m_controlLoop->Stop(); - RemoveListeners(); } /** @@ -257,10 +252,6 @@ void PIDController::SetPID(double p, double i, double d) { m_I = i; m_D = d; } - - if (m_pEntry) m_pEntry.SetDouble(m_P); - if (m_iEntry) m_iEntry.SetDouble(m_I); - if (m_dEntry) m_dEntry.SetDouble(m_D); } /** @@ -274,18 +265,51 @@ void PIDController::SetPID(double p, double i, double d) { * @param f Feed forward coefficient */ void PIDController::SetPID(double p, double i, double d, double f) { - { - std::lock_guard lock(m_thisMutex); - m_P = p; - m_I = i; - m_D = d; - m_F = f; - } + std::lock_guard lock(m_thisMutex); + m_P = p; + m_I = i; + m_D = d; + m_F = f; +} - if (m_pEntry) m_pEntry.SetDouble(m_P); - if (m_iEntry) m_iEntry.SetDouble(m_I); - if (m_dEntry) m_dEntry.SetDouble(m_D); - if (m_fEntry) m_fEntry.SetDouble(m_F); +/** + * Set the Proportional coefficient of the PID controller gain. + * + * @param p proportional coefficient + */ +void PIDController::SetP(double p) { + std::lock_guard lock(m_thisMutex); + m_P = p; +} + +/** + * Set the Integral coefficient of the PID controller gain. + * + * @param i integral coefficient + */ +void PIDController::SetI(double i) { + std::lock_guard lock(m_thisMutex); + m_I = i; +} + +/** + * Set the Differential coefficient of the PID controller gain. + * + * @param d differential coefficient + */ +void PIDController::SetD(double d) { + std::lock_guard lock(m_thisMutex); + m_D = d; +} + +/** + * Get the Feed forward coefficient of the PID controller gain. + * + * @param f Feed forward coefficient + */ +void PIDController::SetF(double f) { + std::lock_guard lock(m_thisMutex); + m_F = f; } /** @@ -403,8 +427,6 @@ void PIDController::SetSetpoint(double setpoint) { m_setpoint = setpoint; } } - - if (m_setpointEntry) m_setpointEntry.SetDouble(m_setpoint); } /** @@ -556,8 +578,6 @@ void PIDController::Enable() { std::lock_guard lock(m_thisMutex); m_enabled = true; } - - if (m_enabledEntry) m_enabledEntry.SetBoolean(true); } /** @@ -574,8 +594,17 @@ void PIDController::Disable() { m_pidOutput->PIDWrite(0); } +} - if (m_enabledEntry) m_enabledEntry.SetBoolean(false); +/** + * Set the enabled state of the PIDController. + */ +void PIDController::SetEnabled(bool enable) { + if (enable) { + Enable(); + } else { + Disable(); + } } /** @@ -598,76 +627,21 @@ void PIDController::Reset() { m_result = 0; } -std::string PIDController::GetSmartDashboardType() const { - return "PIDController"; -} - -void PIDController::InitTable(std::shared_ptr subtable) { - RemoveListeners(); - if (subtable) { - m_pEntry = subtable->GetEntry(kP); - m_pEntry.SetDouble(GetP()); - m_iEntry = subtable->GetEntry(kI); - m_iEntry.SetDouble(GetI()); - m_dEntry = subtable->GetEntry(kD); - m_dEntry.SetDouble(GetD()); - m_fEntry = subtable->GetEntry(kF); - m_fEntry.SetDouble(GetF()); - m_setpointEntry = subtable->GetEntry(kSetpoint); - m_setpointEntry.SetDouble(GetSetpoint()); - m_enabledEntry = subtable->GetEntry(kEnabled); - m_enabledEntry.SetBoolean(IsEnabled()); - - m_pListener = m_pEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsDouble()) return; - std::lock_guard lock(m_thisMutex); - m_P = event.value->GetDouble(); - }, - NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - - m_iListener = m_iEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsDouble()) return; - std::lock_guard lock(m_thisMutex); - m_I = event.value->GetDouble(); - }, - NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - - m_dListener = m_dEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsDouble()) return; - std::lock_guard lock(m_thisMutex); - m_D = event.value->GetDouble(); - }, - NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - - m_fListener = m_fEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsDouble()) return; - std::lock_guard lock(m_thisMutex); - m_F = event.value->GetDouble(); - }, - NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - - m_setpointListener = m_setpointEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsDouble()) return; - SetSetpoint(event.value->GetDouble()); - }, - NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - - m_enabledListener = m_enabledEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsBoolean()) return; - if (event.value->GetBoolean()) { - Enable(); - } else { - Disable(); - } - }, - NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - } +void PIDController::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("PIDController"); + builder.SetSafeState([=]() { Reset(); }); + builder.AddDoubleProperty("p", [=]() { return GetP(); }, + [=](bool value) { SetP(value); }); + builder.AddDoubleProperty("i", [=]() { return GetI(); }, + [=](bool value) { SetI(value); }); + builder.AddDoubleProperty("d", [=]() { return GetD(); }, + [=](bool value) { SetD(value); }); + builder.AddDoubleProperty("f", [=]() { return GetF(); }, + [=](bool value) { SetF(value); }); + builder.AddDoubleProperty("setpoint", [=]() { return GetSetpoint(); }, + [=](bool value) { SetSetpoint(value); }); + builder.AddBooleanProperty("enabled", [=]() { return IsEnabled(); }, + [=](bool value) { SetEnabled(value); }); } /** @@ -691,36 +665,3 @@ double PIDController::GetContinuousError(double error) const { return error; } - -void PIDController::UpdateTable() {} - -void PIDController::StartLiveWindowMode() { Disable(); } - -void PIDController::StopLiveWindowMode() {} - -void PIDController::RemoveListeners() { - if (m_pListener != 0) { - m_pEntry.RemoveListener(m_pListener); - m_pListener = 0; - } - if (m_iListener != 0) { - m_iEntry.RemoveListener(m_iListener); - m_iListener = 0; - } - if (m_dListener != 0) { - m_dEntry.RemoveListener(m_dListener); - m_dListener = 0; - } - if (m_fListener != 0) { - m_fEntry.RemoveListener(m_fListener); - m_fListener = 0; - } - if (m_setpointListener != 0) { - m_setpointEntry.RemoveListener(m_setpointListener); - m_setpointListener = 0; - } - if (m_enabledListener != 0) { - m_enabledEntry.RemoveListener(m_enabledListener); - m_enabledListener = 0; - } -} diff --git a/wpilibc/src/main/native/cpp/PWM.cpp b/wpilibc/src/main/native/cpp/PWM.cpp index ee7ab4c0c2..c4abe363e3 100644 --- a/wpilibc/src/main/native/cpp/PWM.cpp +++ b/wpilibc/src/main/native/cpp/PWM.cpp @@ -13,6 +13,8 @@ #include #include +#include "SensorBase.h" +#include "SmartDashboard/SendableBuilder.h" #include "Utility.h" #include "WPIErrors.h" @@ -32,7 +34,7 @@ PWM::PWM(int channel) { llvm::SmallString<32> str; llvm::raw_svector_ostream buf(str); - if (!CheckPWMChannel(channel)) { + if (!SensorBase::CheckPWMChannel(channel)) { buf << "PWM Channel " << channel; wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str()); return; @@ -57,6 +59,7 @@ PWM::PWM(int channel) { wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); HAL_Report(HALUsageReporting::kResourceType_PWM, channel); + SetName("PWM", channel); } /** @@ -72,8 +75,6 @@ PWM::~PWM() { HAL_FreePWMPort(m_handle, &status); wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); - - if (m_valueListener != 0) m_valueEntry.RemoveListener(m_valueListener); } /** @@ -314,37 +315,9 @@ void PWM::SetZeroLatch() { wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); } -void PWM::UpdateTable() { - if (m_valueEntry) m_valueEntry.SetDouble(GetSpeed()); -} - -void PWM::StartLiveWindowMode() { - SetSpeed(0); - if (m_valueEntry) { - m_valueListener = m_valueEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsDouble()) return; - SetSpeed(event.value->GetDouble()); - }, - NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - } -} - -void PWM::StopLiveWindowMode() { - SetSpeed(0); - if (m_valueListener != 0) { - m_valueEntry.RemoveListener(m_valueListener); - m_valueListener = 0; - } -} - -std::string PWM::GetSmartDashboardType() const { return "Speed Controller"; } - -void PWM::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void PWM::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Speed Controller"); + builder.SetSafeState([=]() { SetDisabled(); }); + builder.AddDoubleProperty("Value", [=]() { return GetSpeed(); }, + [=](double value) { SetSpeed(value); }); } diff --git a/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp index 6422492423..de2e513d55 100644 --- a/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp +++ b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp @@ -9,8 +9,6 @@ #include -#include "LiveWindow/LiveWindow.h" - using namespace frc; /** @@ -38,5 +36,5 @@ PWMTalonSRX::PWMTalonSRX(int channel) : PWMSpeedController(channel) { SetZeroLatch(); HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel()); - LiveWindow::GetInstance()->AddActuator("PWMTalonSRX", GetChannel(), this); + SetName("PWMTalonSRX", GetChannel()); } diff --git a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp index 4e71caf8ac..82c37ca011 100644 --- a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp +++ b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp @@ -13,7 +13,7 @@ #include #include -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -32,6 +32,7 @@ PowerDistributionPanel::PowerDistributionPanel(int module) : m_module(module) { m_module = -1; return; } + SetName("PowerDistributionPanel", module); } /** @@ -171,39 +172,13 @@ void PowerDistributionPanel::ClearStickyFaults() { } } -void PowerDistributionPanel::UpdateTable() { - for (size_t i = 0; i < sizeof(m_chanEntry) / sizeof(m_chanEntry[0]); ++i) { - if (m_chanEntry[i]) m_chanEntry[i].SetDouble(GetCurrent(i)); - } - if (m_voltageEntry) m_voltageEntry.SetDouble(GetVoltage()); - if (m_totalCurrentEntry) m_totalCurrentEntry.SetDouble(GetTotalCurrent()); -} - -void PowerDistributionPanel::StartLiveWindowMode() {} - -void PowerDistributionPanel::StopLiveWindowMode() {} - -std::string PowerDistributionPanel::GetSmartDashboardType() const { - return "PowerDistributionPanel"; -} - -void PowerDistributionPanel::InitTable( - std::shared_ptr subTable) { - if (subTable != nullptr) { - for (size_t i = 0; i < sizeof(m_chanEntry) / sizeof(m_chanEntry[0]); ++i) { - llvm::SmallString<32> buf; - llvm::raw_svector_ostream oss(buf); - oss << "Chan" << i; - m_chanEntry[i] = subTable->GetEntry(oss.str()); - } - m_voltageEntry = subTable->GetEntry("Voltage"); - m_totalCurrentEntry = subTable->GetEntry("TotalCurrent"); - UpdateTable(); - } else { - for (size_t i = 0; i < sizeof(m_chanEntry) / sizeof(m_chanEntry[0]); ++i) { - m_chanEntry[i] = nt::NetworkTableEntry(); - } - m_voltageEntry = nt::NetworkTableEntry(); - m_totalCurrentEntry = nt::NetworkTableEntry(); +void PowerDistributionPanel::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("PowerDistributionPanel"); + for (int i = 0; i < kPDPChannels; ++i) { + builder.AddDoubleProperty("Chan" + llvm::Twine(i), + [=]() { return GetCurrent(i); }, nullptr); } + builder.AddDoubleProperty("Voltage", [=]() { return GetVoltage(); }, nullptr); + builder.AddDoubleProperty("TotalCurrent", [=]() { return GetTotalCurrent(); }, + nullptr); } diff --git a/wpilibc/src/main/native/cpp/Relay.cpp b/wpilibc/src/main/native/cpp/Relay.cpp index c5b5c090d3..5338144f91 100644 --- a/wpilibc/src/main/native/cpp/Relay.cpp +++ b/wpilibc/src/main/native/cpp/Relay.cpp @@ -12,8 +12,9 @@ #include #include -#include "LiveWindow/LiveWindow.h" #include "MotorSafetyHelper.h" +#include "SensorBase.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -88,7 +89,7 @@ Relay::Relay(int channel, Relay::Direction direction) m_safetyHelper = std::make_unique(this); m_safetyHelper->SetSafetyEnabled(false); - LiveWindow::GetInstance()->AddActuator("Relay", 1, m_channel, this); + SetName("Relay", m_channel); } /** @@ -103,8 +104,6 @@ Relay::~Relay() { // ignore errors, as we want to make sure a free happens. if (m_forwardHandle != HAL_kInvalidHandle) HAL_FreeRelayPort(m_forwardHandle); if (m_reverseHandle != HAL_kInvalidHandle) HAL_FreeRelayPort(m_reverseHandle); - - if (m_valueListener != 0) m_valueEntry.RemoveListener(m_valueListener); } /** @@ -275,52 +274,31 @@ void Relay::GetDescription(llvm::raw_ostream& desc) const { desc << "Relay " << GetChannel(); } -void Relay::UpdateTable() { - if (m_valueEntry) { - if (Get() == kOn) { - m_valueEntry.SetString("On"); - } else if (Get() == kForward) { - m_valueEntry.SetString("Forward"); - } else if (Get() == kReverse) { - m_valueEntry.SetString("Reverse"); - } else { - m_valueEntry.SetString("Off"); - } - } -} - -void Relay::StartLiveWindowMode() { - if (m_valueEntry) { - m_valueListener = m_valueEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsString()) return; - if (event.value->GetString() == "Off") - Set(kOff); - else if (event.value->GetString() == "Forward") - Set(kForward); - else if (event.value->GetString() == "Reverse") - Set(kReverse); - else if (event.value->GetString() == "On") - Set(kOn); - }, - NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - } -} - -void Relay::StopLiveWindowMode() { - if (m_valueListener != 0) { - m_valueEntry.RemoveListener(m_valueListener); - m_valueListener = 0; - } -} - -std::string Relay::GetSmartDashboardType() const { return "Relay"; } - -void Relay::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void Relay::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Relay"); + builder.SetSafeState([=]() { Set(kOff); }); + builder.AddSmallStringProperty( + "Value", + [=](llvm::SmallVectorImpl& buf) -> llvm::StringRef { + switch (Get()) { + case kOn: + return "On"; + case kForward: + return "Forward"; + case kReverse: + return "Reverse"; + default: + return "Off"; + } + }, + [=](llvm::StringRef value) { + if (value == "Off") + Set(kOff); + else if (value == "Forward") + Set(kForward); + else if (value == "Reverse") + Set(kReverse); + else if (value == "On") + Set(kOn); + }); } diff --git a/wpilibc/src/main/native/cpp/SD540.cpp b/wpilibc/src/main/native/cpp/SD540.cpp index a2db7adfe8..8e44a2c8f6 100644 --- a/wpilibc/src/main/native/cpp/SD540.cpp +++ b/wpilibc/src/main/native/cpp/SD540.cpp @@ -9,8 +9,6 @@ #include -#include "LiveWindow/LiveWindow.h" - using namespace frc; /** @@ -41,5 +39,5 @@ SD540::SD540(int channel) : PWMSpeedController(channel) { SetZeroLatch(); HAL_Report(HALUsageReporting::kResourceType_MindsensorsSD540, GetChannel()); - LiveWindow::GetInstance()->AddActuator("SD540", GetChannel(), this); + SetName("SD540", GetChannel()); } diff --git a/wpilibc/src/main/native/cpp/Servo.cpp b/wpilibc/src/main/native/cpp/Servo.cpp index b7b0d48da4..20659d7dc6 100644 --- a/wpilibc/src/main/native/cpp/Servo.cpp +++ b/wpilibc/src/main/native/cpp/Servo.cpp @@ -9,7 +9,7 @@ #include -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" using namespace frc; @@ -31,10 +31,7 @@ Servo::Servo(int channel) : SafePWM(channel) { SetPeriodMultiplier(kPeriodMultiplier_4X); HAL_Report(HALUsageReporting::kResourceType_Servo, channel); -} - -Servo::~Servo() { - if (m_valueListener != 0) m_valueEntry.RemoveListener(m_valueListener); + SetName("Servo", channel); } /** @@ -100,35 +97,8 @@ double Servo::GetAngle() const { return GetPosition() * GetServoAngleRange() + kMinServoAngle; } -void Servo::UpdateTable() { - if (m_valueEntry) m_valueEntry.SetDouble(Get()); -} - -void Servo::StartLiveWindowMode() { - if (m_valueEntry) { - m_valueListener = m_valueEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsDouble()) return; - Set(event.value->GetDouble()); - }, - NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - } -} - -void Servo::StopLiveWindowMode() { - if (m_valueListener != 0) { - m_valueEntry.RemoveListener(m_valueListener); - m_valueListener = 0; - } -} - -std::string Servo::GetSmartDashboardType() const { return "Servo"; } - -void Servo::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void Servo::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Servo"); + builder.AddDoubleProperty("Value", [=]() { return Get(); }, + [=](double value) { Set(value); }); } diff --git a/wpilibc/src/main/native/cpp/SmartDashboard/SendableBase.cpp b/wpilibc/src/main/native/cpp/SmartDashboard/SendableBase.cpp new file mode 100644 index 0000000000..b759731e12 --- /dev/null +++ b/wpilibc/src/main/native/cpp/SmartDashboard/SendableBase.cpp @@ -0,0 +1,91 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "SmartDashboard/SendableBase.h" + +#include "LiveWindow/LiveWindow.h" + +using namespace frc; + +/** + * Creates an instance of the sensor base. + * + * @param addLiveWindow if true, add this Sendable to LiveWindow + */ +SendableBase::SendableBase(bool addLiveWindow) { + if (addLiveWindow) LiveWindow::GetInstance()->Add(this); +} + +/** + * Free the resources used by this object. + */ +SendableBase::~SendableBase() { LiveWindow::GetInstance()->Remove(this); } + +std::string SendableBase::GetName() const { + std::lock_guard lock(m_mutex); + return m_name; +} + +void SendableBase::SetName(const llvm::Twine& name) { + std::lock_guard lock(m_mutex); + m_name = name.str(); +} + +std::string SendableBase::GetSubsystem() const { + std::lock_guard lock(m_mutex); + return m_subsystem; +} + +void SendableBase::SetSubsystem(const llvm::Twine& subsystem) { + std::lock_guard lock(m_mutex); + m_subsystem = subsystem.str(); +} + +/** + * Add a child component. + * + * @param child child component + */ +void SendableBase::AddChild(std::shared_ptr child) { + LiveWindow::GetInstance()->AddChild(this, child); +} + +/** + * Add a child component. + * + * @param child child component + */ +void SendableBase::AddChild(void* child) { + LiveWindow::GetInstance()->AddChild(this, child); +} + +/** + * Sets the name of the sensor with a channel number. + * + * @param moduleType A string that defines the module name in the label for the + * value + * @param channel The channel number the device is plugged into + */ +void SendableBase::SetName(const llvm::Twine& moduleType, int channel) { + SetName(moduleType + llvm::Twine('[') + llvm::Twine(channel) + + llvm::Twine(']')); +} + +/** + * Sets the name of the sensor with a module and channel number. + * + * @param moduleType A string that defines the module name in the label for + * the value + * @param moduleNumber The number of the particular module type + * @param channel The channel number the device is plugged into (usually + * PWM) + */ +void SendableBase::SetName(const llvm::Twine& moduleType, int moduleNumber, + int channel) { + SetName(moduleType + llvm::Twine('[') + llvm::Twine(moduleNumber) + + llvm::Twine(',') + llvm::Twine(channel) + llvm::Twine(']')); +} diff --git a/wpilibc/src/main/native/cpp/SmartDashboard/SendableBuilderImpl.cpp b/wpilibc/src/main/native/cpp/SmartDashboard/SendableBuilderImpl.cpp new file mode 100644 index 0000000000..72c42a43d2 --- /dev/null +++ b/wpilibc/src/main/native/cpp/SmartDashboard/SendableBuilderImpl.cpp @@ -0,0 +1,365 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "SmartDashboard/SendableBuilderImpl.h" + +#include + +#include "ntcore_cpp.h" + +using namespace frc; + +void SendableBuilderImpl::SetTable(std::shared_ptr table) { + m_table = table; +} + +std::shared_ptr SendableBuilderImpl::GetTable() { + return m_table; +} + +void SendableBuilderImpl::UpdateTable() { + uint64_t time = nt::Now(); + for (auto& property : m_properties) { + if (property.update) property.update(property.entry, time); + } + if (m_updateTable) m_updateTable(); +} + +void SendableBuilderImpl::StartLiveWindowMode() { + if (m_safeState) m_safeState(); + for (auto& property : m_properties) property.StartListener(); +} + +void SendableBuilderImpl::StopLiveWindowMode() { + if (m_safeState) m_safeState(); + for (auto& property : m_properties) property.StopListener(); +} + +void SendableBuilderImpl::SetSmartDashboardType(const llvm::Twine& type) { + m_table->GetEntry(".type").SetString(type); +} + +void SendableBuilderImpl::SetSafeState(std::function func) { + m_safeState = func; +} + +void SendableBuilderImpl::SetUpdateTable(std::function func) { + m_updateTable = func; +} + +nt::NetworkTableEntry SendableBuilderImpl::GetEntry(const llvm::Twine& key) { + return m_table->GetEntry(key); +} + +void SendableBuilderImpl::AddBooleanProperty(const llvm::Twine& key, + std::function getter, + std::function setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + entry.SetValue(nt::Value::MakeBoolean(getter(), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsBoolean()) return; + setter(event.value->GetBoolean()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddDoubleProperty( + const llvm::Twine& key, std::function getter, + std::function setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + entry.SetValue(nt::Value::MakeDouble(getter(), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsDouble()) return; + setter(event.value->GetDouble()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddStringProperty( + const llvm::Twine& key, std::function getter, + std::function setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + entry.SetValue(nt::Value::MakeString(getter(), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsString()) return; + setter(event.value->GetString()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddBooleanArrayProperty( + const llvm::Twine& key, std::function()> getter, + std::function)> setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + entry.SetValue(nt::Value::MakeBooleanArray(getter(), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsBooleanArray()) return; + setter(event.value->GetBooleanArray()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddDoubleArrayProperty( + const llvm::Twine& key, std::function()> getter, + std::function)> setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + entry.SetValue(nt::Value::MakeDoubleArray(getter(), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsDoubleArray()) return; + setter(event.value->GetDoubleArray()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddStringArrayProperty( + const llvm::Twine& key, std::function()> getter, + std::function)> setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + entry.SetValue(nt::Value::MakeStringArray(getter(), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsStringArray()) return; + setter(event.value->GetStringArray()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddRawProperty( + const llvm::Twine& key, std::function getter, + std::function setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + entry.SetValue(nt::Value::MakeRaw(getter(), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsRaw()) return; + setter(event.value->GetRaw()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddValueProperty( + const llvm::Twine& key, std::function()> getter, + std::function)> setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + entry.SetValue(getter()); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { setter(event.value); }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddSmallStringProperty( + const llvm::Twine& key, + std::function& buf)> getter, + std::function setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + llvm::SmallString<128> buf; + entry.SetValue(nt::Value::MakeString(getter(buf), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsString()) return; + setter(event.value->GetString()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddSmallBooleanArrayProperty( + const llvm::Twine& key, + std::function(llvm::SmallVectorImpl& buf)> getter, + std::function)> setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + llvm::SmallVector buf; + entry.SetValue(nt::Value::MakeBooleanArray(getter(buf), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsBooleanArray()) return; + setter(event.value->GetBooleanArray()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddSmallDoubleArrayProperty( + const llvm::Twine& key, + std::function(llvm::SmallVectorImpl& buf)> + getter, + std::function)> setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + llvm::SmallVector buf; + entry.SetValue(nt::Value::MakeDoubleArray(getter(buf), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsDoubleArray()) return; + setter(event.value->GetDoubleArray()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddSmallStringArrayProperty( + const llvm::Twine& key, + std::function< + llvm::ArrayRef(llvm::SmallVectorImpl& buf)> + getter, + std::function)> setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + llvm::SmallVector buf; + entry.SetValue(nt::Value::MakeStringArray(getter(buf), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsStringArray()) return; + setter(event.value->GetStringArray()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} + +void SendableBuilderImpl::AddSmallRawProperty( + const llvm::Twine& key, + std::function& buf)> getter, + std::function setter) { + m_properties.emplace_back(*m_table, key); + if (getter) { + m_properties.back().update = [=](nt::NetworkTableEntry entry, + uint64_t time) { + llvm::SmallVector buf; + entry.SetValue(nt::Value::MakeRaw(getter(buf), time)); + }; + } + if (setter) { + m_properties.back().createListener = + [=](nt::NetworkTableEntry entry) -> NT_EntryListener { + return entry.AddListener( + [=](const nt::EntryNotification& event) { + if (!event.value->IsRaw()) return; + setter(event.value->GetRaw()); + }, + NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); + }; + } +} diff --git a/wpilibc/src/main/native/cpp/SmartDashboard/SendableChooserBase.cpp b/wpilibc/src/main/native/cpp/SmartDashboard/SendableChooserBase.cpp index ee7be0ad3c..795006b7cb 100644 --- a/wpilibc/src/main/native/cpp/SmartDashboard/SendableChooserBase.cpp +++ b/wpilibc/src/main/native/cpp/SmartDashboard/SendableChooserBase.cpp @@ -12,7 +12,3 @@ using namespace frc; const char* SendableChooserBase::kDefault = "default"; const char* SendableChooserBase::kOptions = "options"; const char* SendableChooserBase::kSelected = "selected"; - -std::string SendableChooserBase::GetSmartDashboardType() const { - return "String Chooser"; -} diff --git a/wpilibc/src/main/native/cpp/SmartDashboard/SmartDashboard.cpp b/wpilibc/src/main/native/cpp/SmartDashboard/SmartDashboard.cpp index 92051f187c..2938c46b38 100644 --- a/wpilibc/src/main/native/cpp/SmartDashboard/SmartDashboard.cpp +++ b/wpilibc/src/main/native/cpp/SmartDashboard/SmartDashboard.cpp @@ -7,18 +7,28 @@ #include "SmartDashboard/SmartDashboard.h" -#include +#include +#include #include "HLUsageReporting.h" -#include "SmartDashboard/NamedSendable.h" +#include "SmartDashboard/Sendable.h" +#include "SmartDashboard/SendableBuilderImpl.h" #include "WPIErrors.h" #include "networktables/NetworkTable.h" #include "networktables/NetworkTableInstance.h" using namespace frc; +namespace { +struct SmartDashboardData { + Sendable* sendable = nullptr; + SendableBuilderImpl builder; +}; +} // namespace + static std::shared_ptr s_table; -static std::map, Sendable*> s_tablesToData; +static llvm::StringMap s_tablesToData; +static wpi::mutex s_tablesToDataMutex; void SmartDashboard::init() { s_table = nt::NetworkTableInstance::GetDefault().GetTable("SmartDashboard"); @@ -126,22 +136,26 @@ void SmartDashboard::PutData(llvm::StringRef key, Sendable* data) { wpi_setGlobalWPIErrorWithContext(NullParameter, "value"); return; } - std::shared_ptr dataTable(s_table->GetSubTable(key)); - dataTable->GetEntry(".type").SetString(data->GetSmartDashboardType()); - data->InitTable(dataTable); - s_tablesToData[dataTable] = data; + std::lock_guard lock(s_tablesToDataMutex); + auto& sddata = s_tablesToData[key]; + if (!sddata.sendable || sddata.sendable != data) { + sddata.sendable = data; + sddata.builder.SetTable(s_table->GetSubTable(key)); + data->InitSendable(sddata.builder); + } + sddata.builder.UpdateTable(); } /** - * Maps the specified key (where the key is the name of the - * {@link SmartDashboardNamedData} to the specified value in this table. + * Maps the specified key (where the key is the name of the Sendable) + * to the specified value in this table. * * The value can be retrieved by calling the get method with a key that is equal * to the original key. * * @param value the value */ -void SmartDashboard::PutData(NamedSendable* value) { +void SmartDashboard::PutData(Sendable* value) { if (value == nullptr) { wpi_setGlobalWPIErrorWithContext(NullParameter, "value"); return; @@ -156,13 +170,13 @@ void SmartDashboard::PutData(NamedSendable* value) { * @return the value */ Sendable* SmartDashboard::GetData(llvm::StringRef key) { - std::shared_ptr subtable(s_table->GetSubTable(key)); - Sendable* data = s_tablesToData[subtable]; - if (data == nullptr) { + std::lock_guard lock(s_tablesToDataMutex); + auto data = s_tablesToData.find(key); + if (data == s_tablesToData.end()) { wpi_setGlobalWPIErrorWithContext(SmartDashboardMissingKey, key); return nullptr; } - return data; + return data->getValue().sendable; } /** diff --git a/wpilibc/src/main/native/cpp/Solenoid.cpp b/wpilibc/src/main/native/cpp/Solenoid.cpp index ef5c9d0b59..18b09ebccf 100644 --- a/wpilibc/src/main/native/cpp/Solenoid.cpp +++ b/wpilibc/src/main/native/cpp/Solenoid.cpp @@ -13,7 +13,8 @@ #include #include -#include "LiveWindow/LiveWindow.h" +#include "SensorBase.h" +#include "SmartDashboard/SendableBuilder.h" #include "WPIErrors.h" using namespace frc; @@ -24,7 +25,7 @@ using namespace frc; * @param channel The channel on the PCM to control (0..7). */ Solenoid::Solenoid(int channel) - : Solenoid(GetDefaultSolenoidModule(), channel) {} + : Solenoid(SensorBase::GetDefaultSolenoidModule(), channel) {} /** * Constructor. @@ -36,12 +37,12 @@ Solenoid::Solenoid(int moduleNumber, int channel) : SolenoidBase(moduleNumber), m_channel(channel) { llvm::SmallString<32> str; llvm::raw_svector_ostream buf(str); - if (!CheckSolenoidModule(m_moduleNumber)) { + if (!SensorBase::CheckSolenoidModule(m_moduleNumber)) { buf << "Solenoid Module " << m_moduleNumber; wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf.str()); return; } - if (!CheckSolenoidChannel(m_channel)) { + if (!SensorBase::CheckSolenoidChannel(m_channel)) { buf << "Solenoid Module " << m_channel; wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str()); return; @@ -57,19 +58,15 @@ Solenoid::Solenoid(int moduleNumber, int channel) return; } - LiveWindow::GetInstance()->AddActuator("Solenoid", m_moduleNumber, m_channel, - this); HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel, m_moduleNumber); + SetName("Solenoid", m_moduleNumber, m_channel); } /** * Destructor. */ -Solenoid::~Solenoid() { - HAL_FreeSolenoidPort(m_solenoidHandle); - if (m_valueListener != 0) m_valueEntry.RemoveListener(m_valueListener); -} +Solenoid::~Solenoid() { HAL_FreeSolenoidPort(m_solenoidHandle); } /** * Set the value of a solenoid. @@ -141,34 +138,9 @@ void Solenoid::StartPulse() { wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); } -void Solenoid::UpdateTable() { - if (m_valueEntry) m_valueEntry.SetBoolean(Get()); -} - -void Solenoid::StartLiveWindowMode() { - Set(false); - if (m_valueEntry) { - m_valueEntry.AddListener( - [=](const nt::EntryNotification& event) { - if (!event.value->IsBoolean()) return; - Set(event.value->GetBoolean()); - }, - NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE); - } -} - -void Solenoid::StopLiveWindowMode() { - Set(false); - if (m_valueListener != 0) m_valueEntry.RemoveListener(m_valueListener); -} - -std::string Solenoid::GetSmartDashboardType() const { return "Solenoid"; } - -void Solenoid::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void Solenoid::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Solenoid"); + builder.SetSafeState([=]() { Set(false); }); + builder.AddBooleanProperty("Value", [=]() { return Get(); }, + [=](bool value) { Set(value); }); } diff --git a/wpilibc/src/main/native/cpp/Spark.cpp b/wpilibc/src/main/native/cpp/Spark.cpp index 653d5838dc..c0a231e989 100644 --- a/wpilibc/src/main/native/cpp/Spark.cpp +++ b/wpilibc/src/main/native/cpp/Spark.cpp @@ -9,8 +9,6 @@ #include -#include "LiveWindow/LiveWindow.h" - using namespace frc; /** @@ -41,5 +39,5 @@ Spark::Spark(int channel) : PWMSpeedController(channel) { SetZeroLatch(); HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel()); - LiveWindow::GetInstance()->AddActuator("Spark", GetChannel(), this); + SetName("Spark", GetChannel()); } diff --git a/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp b/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp index b77f9552ab..722b2490b6 100644 --- a/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp +++ b/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp @@ -7,6 +7,8 @@ #include "SpeedControllerGroup.h" +#include "SmartDashboard/SendableBuilder.h" + using namespace frc; void SpeedControllerGroup::Set(double speed) { @@ -45,3 +47,10 @@ void SpeedControllerGroup::PIDWrite(double output) { speedController.get().PIDWrite(output); } } + +void SpeedControllerGroup::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Speed Controller"); + builder.SetSafeState([=]() { StopMotor(); }); + builder.AddDoubleProperty("Value", [=]() { return Get(); }, + [=](double value) { Set(value); }); +} diff --git a/wpilibc/src/main/native/cpp/Talon.cpp b/wpilibc/src/main/native/cpp/Talon.cpp index bbd3b27a8b..db0e1e954c 100644 --- a/wpilibc/src/main/native/cpp/Talon.cpp +++ b/wpilibc/src/main/native/cpp/Talon.cpp @@ -9,8 +9,6 @@ #include -#include "LiveWindow/LiveWindow.h" - using namespace frc; /** @@ -39,5 +37,5 @@ Talon::Talon(int channel) : PWMSpeedController(channel) { SetZeroLatch(); HAL_Report(HALUsageReporting::kResourceType_Talon, GetChannel()); - LiveWindow::GetInstance()->AddActuator("Talon", GetChannel(), this); + SetName("Talon", GetChannel()); } diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp index 9ef0dbec98..b33a0508d7 100644 --- a/wpilibc/src/main/native/cpp/Ultrasonic.cpp +++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp @@ -12,7 +12,7 @@ #include "Counter.h" #include "DigitalInput.h" #include "DigitalOutput.h" -#include "LiveWindow/LiveWindow.h" +#include "SmartDashboard/SendableBuilder.h" #include "Timer.h" #include "Utility.h" #include "WPIErrors.h" @@ -72,8 +72,7 @@ void Ultrasonic::Initialize() { static int instances = 0; instances++; HAL_Report(HALUsageReporting::kResourceType_Ultrasonic, instances); - LiveWindow::GetInstance()->AddSensor("Ultrasonic", - m_echoChannel->GetChannel(), this); + SetName("Ultrasonic", m_echoChannel->GetChannel()); } /** @@ -94,6 +93,8 @@ Ultrasonic::Ultrasonic(int pingChannel, int echoChannel, DistanceUnit units) m_counter(m_echoChannel) { m_units = units; Initialize(); + AddChild(m_pingChannel); + AddChild(m_echoChannel); } /** @@ -314,23 +315,8 @@ Ultrasonic::DistanceUnit Ultrasonic::GetDistanceUnits() const { return m_units; } -void Ultrasonic::UpdateTable() { - if (m_valueEntry) { - m_valueEntry.SetDouble(GetRangeInches()); - } -} - -void Ultrasonic::StartLiveWindowMode() {} - -void Ultrasonic::StopLiveWindowMode() {} - -std::string Ultrasonic::GetSmartDashboardType() const { return "Ultrasonic"; } - -void Ultrasonic::InitTable(std::shared_ptr subTable) { - if (subTable) { - m_valueEntry = subTable->GetEntry("Value"); - UpdateTable(); - } else { - m_valueEntry = nt::NetworkTableEntry(); - } +void Ultrasonic::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("Ultrasonic"); + builder.AddDoubleProperty("Value", [=]() { return GetRangeInches(); }, + nullptr); } diff --git a/wpilibc/src/main/native/cpp/Victor.cpp b/wpilibc/src/main/native/cpp/Victor.cpp index a08e15c983..4fa9942781 100644 --- a/wpilibc/src/main/native/cpp/Victor.cpp +++ b/wpilibc/src/main/native/cpp/Victor.cpp @@ -9,8 +9,6 @@ #include -#include "LiveWindow/LiveWindow.h" - using namespace frc; /** @@ -39,6 +37,6 @@ Victor::Victor(int channel) : PWMSpeedController(channel) { SetSpeed(0.0); SetZeroLatch(); - LiveWindow::GetInstance()->AddActuator("Victor", GetChannel(), this); HAL_Report(HALUsageReporting::kResourceType_Victor, GetChannel()); + SetName("Victor", GetChannel()); } diff --git a/wpilibc/src/main/native/cpp/VictorSP.cpp b/wpilibc/src/main/native/cpp/VictorSP.cpp index 859d2b7a1d..f2382d3d42 100644 --- a/wpilibc/src/main/native/cpp/VictorSP.cpp +++ b/wpilibc/src/main/native/cpp/VictorSP.cpp @@ -9,8 +9,6 @@ #include -#include "LiveWindow/LiveWindow.h" - using namespace frc; /** @@ -40,5 +38,5 @@ VictorSP::VictorSP(int channel) : PWMSpeedController(channel) { SetZeroLatch(); HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel()); - LiveWindow::GetInstance()->AddActuator("VictorSP", GetChannel(), this); + SetName("VictorSP", GetChannel()); } diff --git a/wpilibc/src/main/native/include/ADXL345_I2C.h b/wpilibc/src/main/native/include/ADXL345_I2C.h index 456ada7f9c..654cee89a8 100644 --- a/wpilibc/src/main/native/include/ADXL345_I2C.h +++ b/wpilibc/src/main/native/include/ADXL345_I2C.h @@ -7,13 +7,9 @@ #pragma once -#include -#include - #include "I2C.h" -#include "LiveWindow/LiveWindowSendable.h" +#include "SensorBase.h" #include "interfaces/Accelerometer.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -24,7 +20,7 @@ namespace frc { * an I2C bus. This class assumes the default (not alternate) sensor address of * 0x1D (7-bit address). */ -class ADXL345_I2C : public Accelerometer, public LiveWindowSendable { +class ADXL345_I2C : public SensorBase, public Accelerometer { public: enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 }; @@ -36,7 +32,7 @@ class ADXL345_I2C : public Accelerometer, public LiveWindowSendable { explicit ADXL345_I2C(I2C::Port port, Range range = kRange_2G, int deviceAddress = kAddress); - virtual ~ADXL345_I2C() = default; + ~ADXL345_I2C() override = default; ADXL345_I2C(const ADXL345_I2C&) = delete; ADXL345_I2C& operator=(const ADXL345_I2C&) = delete; @@ -50,11 +46,7 @@ class ADXL345_I2C : public Accelerometer, public LiveWindowSendable { virtual double GetAcceleration(Axes axis); virtual AllAxes GetAccelerations(); - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subtable) override; - void UpdateTable() override; - void StartLiveWindowMode() override {} - void StopLiveWindowMode() override {} + void InitSendable(SendableBuilder& builder) override; protected: I2C m_i2c; @@ -79,11 +71,6 @@ class ADXL345_I2C : public Accelerometer, public LiveWindowSendable { kDataFormat_FullRes = 0x08, kDataFormat_Justify = 0x04 }; - - private: - nt::NetworkTableEntry m_xEntry; - nt::NetworkTableEntry m_yEntry; - nt::NetworkTableEntry m_zEntry; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/ADXL345_SPI.h b/wpilibc/src/main/native/include/ADXL345_SPI.h index 4531b2ea5d..db71598e7d 100644 --- a/wpilibc/src/main/native/include/ADXL345_SPI.h +++ b/wpilibc/src/main/native/include/ADXL345_SPI.h @@ -7,27 +7,19 @@ #pragma once -#include -#include - -#include "LiveWindow/LiveWindowSendable.h" #include "SPI.h" #include "SensorBase.h" #include "interfaces/Accelerometer.h" -#include "networktables/NetworkTableEntry.h" namespace frc { -class DigitalInput; -class DigitalOutput; - /** * ADXL345 Accelerometer on SPI. * * This class allows access to an Analog Devices ADXL345 3-axis accelerometer * via SPI. This class assumes the sensor is wired in 4-wire SPI mode. */ -class ADXL345_SPI : public Accelerometer, public LiveWindowSendable { +class ADXL345_SPI : public SensorBase, public Accelerometer { public: enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 }; @@ -38,7 +30,7 @@ class ADXL345_SPI : public Accelerometer, public LiveWindowSendable { }; explicit ADXL345_SPI(SPI::Port port, Range range = kRange_2G); - virtual ~ADXL345_SPI() = default; + ~ADXL345_SPI() override = default; ADXL345_SPI(const ADXL345_SPI&) = delete; ADXL345_SPI& operator=(const ADXL345_SPI&) = delete; @@ -52,11 +44,7 @@ class ADXL345_SPI : public Accelerometer, public LiveWindowSendable { virtual double GetAcceleration(Axes axis); virtual AllAxes GetAccelerations(); - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subtable) override; - void UpdateTable() override; - void StartLiveWindowMode() override {} - void StopLiveWindowMode() override {} + void InitSendable(SendableBuilder& builder) override; protected: SPI m_spi; @@ -82,11 +70,6 @@ class ADXL345_SPI : public Accelerometer, public LiveWindowSendable { kDataFormat_FullRes = 0x08, kDataFormat_Justify = 0x04 }; - - private: - nt::NetworkTableEntry m_xEntry; - nt::NetworkTableEntry m_yEntry; - nt::NetworkTableEntry m_zEntry; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/ADXL362.h b/wpilibc/src/main/native/include/ADXL362.h index 191a4553b6..f9eeca49a2 100644 --- a/wpilibc/src/main/native/include/ADXL362.h +++ b/wpilibc/src/main/native/include/ADXL362.h @@ -7,26 +7,18 @@ #pragma once -#include -#include - -#include "LiveWindow/LiveWindowSendable.h" #include "SPI.h" #include "SensorBase.h" #include "interfaces/Accelerometer.h" -#include "networktables/NetworkTableEntry.h" namespace frc { -class DigitalInput; -class DigitalOutput; - /** * ADXL362 SPI Accelerometer. * * This class allows access to an Analog Devices ADXL362 3-axis accelerometer. */ -class ADXL362 : public Accelerometer, public LiveWindowSendable { +class ADXL362 : public SensorBase, public Accelerometer { public: enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 }; struct AllAxes { @@ -52,19 +44,11 @@ class ADXL362 : public Accelerometer, public LiveWindowSendable { virtual double GetAcceleration(Axes axis); virtual AllAxes GetAccelerations(); - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subtable) override; - void UpdateTable() override; - void StartLiveWindowMode() override {} - void StopLiveWindowMode() override {} + void InitSendable(SendableBuilder& builder) override; private: SPI m_spi; double m_gsPerLSB = 0.001; - - nt::NetworkTableEntry m_xEntry; - nt::NetworkTableEntry m_yEntry; - nt::NetworkTableEntry m_zEntry; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/AnalogAccelerometer.h b/wpilibc/src/main/native/include/AnalogAccelerometer.h index ed3c9d96d1..29eab8e435 100644 --- a/wpilibc/src/main/native/include/AnalogAccelerometer.h +++ b/wpilibc/src/main/native/include/AnalogAccelerometer.h @@ -8,13 +8,10 @@ #pragma once #include -#include #include "AnalogInput.h" -#include "LiveWindow/LiveWindowSendable.h" #include "PIDSource.h" #include "SensorBase.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -25,25 +22,19 @@ namespace frc { * sensors have multiple axis and can be treated as multiple devices. Each is * calibrated by finding the center value over a period of time. */ -class AnalogAccelerometer : public SensorBase, - public PIDSource, - public LiveWindowSendable { +class AnalogAccelerometer : public SensorBase, public PIDSource { public: explicit AnalogAccelerometer(int channel); explicit AnalogAccelerometer(AnalogInput* channel); explicit AnalogAccelerometer(std::shared_ptr channel); - virtual ~AnalogAccelerometer() = default; + ~AnalogAccelerometer() override = default; double GetAcceleration() const; void SetSensitivity(double sensitivity); void SetZero(double zero); double PIDGet() override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; + void InitSendable(SendableBuilder& builder) override; private: void InitAccelerometer(); @@ -51,8 +42,6 @@ class AnalogAccelerometer : public SensorBase, std::shared_ptr m_analogInput; double m_voltsPerG = 1.0; double m_zeroGVoltage = 2.5; - - nt::NetworkTableEntry m_valueEntry; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/AnalogInput.h b/wpilibc/src/main/native/include/AnalogInput.h index 8b7e50ce6e..533256e57e 100644 --- a/wpilibc/src/main/native/include/AnalogInput.h +++ b/wpilibc/src/main/native/include/AnalogInput.h @@ -9,15 +9,10 @@ #include -#include -#include - #include -#include "LiveWindow/LiveWindowSendable.h" #include "PIDSource.h" #include "SensorBase.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -33,9 +28,7 @@ namespace frc { * are divided by the number of samples to retain the resolution, but get more * stable values. */ -class AnalogInput : public SensorBase, - public PIDSource, - public LiveWindowSendable { +class AnalogInput : public SensorBase, public PIDSource { friend class AnalogTrigger; friend class AnalogGyro; @@ -45,7 +38,7 @@ class AnalogInput : public SensorBase, static constexpr int kAccumulatorChannels[kAccumulatorNumChannels] = {0, 1}; explicit AnalogInput(int channel); - virtual ~AnalogInput(); + ~AnalogInput() override; int GetValue() const; int GetAverageValue() const; @@ -78,19 +71,13 @@ class AnalogInput : public SensorBase, double PIDGet() override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; + void InitSendable(SendableBuilder& builder) override; private: int m_channel; // TODO: Adjust HAL to avoid use of raw pointers. HAL_AnalogInputHandle m_port; int64_t m_accumulatorOffset; - - nt::NetworkTableEntry m_valueEntry; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/AnalogOutput.h b/wpilibc/src/main/native/include/AnalogOutput.h index 55401e7146..c28b8d5fd1 100644 --- a/wpilibc/src/main/native/include/AnalogOutput.h +++ b/wpilibc/src/main/native/include/AnalogOutput.h @@ -7,40 +7,30 @@ #pragma once -#include -#include - #include -#include "LiveWindow/LiveWindowSendable.h" -#include "SensorBase.h" -#include "networktables/NetworkTableEntry.h" +#include "ErrorBase.h" +#include "SmartDashboard/SendableBase.h" namespace frc { /** * MXP analog output class. */ -class AnalogOutput : public SensorBase, public LiveWindowSendable { +class AnalogOutput : public ErrorBase, public SendableBase { public: explicit AnalogOutput(int channel); - virtual ~AnalogOutput(); + ~AnalogOutput() override; void SetVoltage(double voltage); double GetVoltage() const; int GetChannel(); - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; + void InitSendable(SendableBuilder& builder) override; protected: int m_channel; HAL_AnalogOutputHandle m_port; - - nt::NetworkTableEntry m_valueEntry; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/AnalogPotentiometer.h b/wpilibc/src/main/native/include/AnalogPotentiometer.h index 02b4da5077..16036a431d 100644 --- a/wpilibc/src/main/native/include/AnalogPotentiometer.h +++ b/wpilibc/src/main/native/include/AnalogPotentiometer.h @@ -8,12 +8,10 @@ #pragma once #include -#include #include "AnalogInput.h" -#include "LiveWindow/LiveWindowSendable.h" +#include "SensorBase.h" #include "interfaces/Potentiometer.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -23,7 +21,7 @@ namespace frc { * units you choose, by way of the scaling and offset constants passed to the * constructor. */ -class AnalogPotentiometer : public Potentiometer, public LiveWindowSendable { +class AnalogPotentiometer : public SensorBase, public Potentiometer { public: /** * AnalogPotentiometer constructor. @@ -52,7 +50,7 @@ class AnalogPotentiometer : public Potentiometer, public LiveWindowSendable { explicit AnalogPotentiometer(std::shared_ptr input, double fullRange = 1.0, double offset = 0.0); - virtual ~AnalogPotentiometer() = default; + ~AnalogPotentiometer() override = default; /** * Get the current reading of the potentiomer. @@ -68,29 +66,11 @@ class AnalogPotentiometer : public Potentiometer, public LiveWindowSendable { */ double PIDGet() override; - /* - * Live Window code, only does anything if live window is activated. - */ - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subtable) override; - void UpdateTable() override; - - /** - * AnalogPotentiometers don't have to do anything special when entering the - * LiveWindow. - */ - void StartLiveWindowMode() override {} - - /** - * AnalogPotentiometers don't have to do anything special when exiting the - * LiveWindow. - */ - void StopLiveWindowMode() override {} + void InitSendable(SendableBuilder& builder) override; private: std::shared_ptr m_analog_input; double m_fullRange, m_offset; - nt::NetworkTableEntry m_valueEntry; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/AnalogTrigger.h b/wpilibc/src/main/native/include/AnalogTrigger.h index e0c9ead4b9..d7fae0acfa 100644 --- a/wpilibc/src/main/native/include/AnalogTrigger.h +++ b/wpilibc/src/main/native/include/AnalogTrigger.h @@ -24,7 +24,7 @@ class AnalogTrigger : public SensorBase { public: explicit AnalogTrigger(int channel); explicit AnalogTrigger(AnalogInput* channel); - virtual ~AnalogTrigger(); + ~AnalogTrigger() override; void SetLimitsVoltage(double lower, double upper); void SetLimitsRaw(int lower, int upper); @@ -36,6 +36,8 @@ class AnalogTrigger : public SensorBase { std::shared_ptr CreateOutput( AnalogTriggerType type) const; + void InitSendable(SendableBuilder& builder) override; + private: int m_index; HAL_AnalogTriggerHandle m_trigger; diff --git a/wpilibc/src/main/native/include/AnalogTriggerOutput.h b/wpilibc/src/main/native/include/AnalogTriggerOutput.h index 041aa44c25..c34dcc3692 100644 --- a/wpilibc/src/main/native/include/AnalogTriggerOutput.h +++ b/wpilibc/src/main/native/include/AnalogTriggerOutput.h @@ -50,7 +50,7 @@ class AnalogTriggerOutput : public DigitalSource { friend class AnalogTrigger; public: - virtual ~AnalogTriggerOutput(); + ~AnalogTriggerOutput() override; bool Get() const; // DigitalSource interface @@ -59,6 +59,8 @@ class AnalogTriggerOutput : public DigitalSource { bool IsAnalogTrigger() const override; int GetChannel() const override; + void InitSendable(SendableBuilder& builder) override; + protected: AnalogTriggerOutput(const AnalogTrigger& trigger, AnalogTriggerType outputType); diff --git a/wpilibc/src/main/native/include/BuiltInAccelerometer.h b/wpilibc/src/main/native/include/BuiltInAccelerometer.h index 4b16625c18..6f9771ae94 100644 --- a/wpilibc/src/main/native/include/BuiltInAccelerometer.h +++ b/wpilibc/src/main/native/include/BuiltInAccelerometer.h @@ -7,13 +7,8 @@ #pragma once -#include -#include - -#include "LiveWindow/LiveWindowSendable.h" #include "SensorBase.h" #include "interfaces/Accelerometer.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -22,12 +17,9 @@ namespace frc { * * This class allows access to the roboRIO's internal accelerometer. */ -class BuiltInAccelerometer : public Accelerometer, - public SensorBase, - public LiveWindowSendable { +class BuiltInAccelerometer : public SensorBase, public Accelerometer { public: explicit BuiltInAccelerometer(Range range = kRange_8G); - virtual ~BuiltInAccelerometer() = default; // Accelerometer interface void SetRange(Range range) override; @@ -35,16 +27,7 @@ class BuiltInAccelerometer : public Accelerometer, double GetY() override; double GetZ() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subtable) override; - void UpdateTable() override; - void StartLiveWindowMode() override {} - void StopLiveWindowMode() override {} - - private: - nt::NetworkTableEntry m_xEntry; - nt::NetworkTableEntry m_yEntry; - nt::NetworkTableEntry m_zEntry; + void InitSendable(SendableBuilder& builder) override; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/Buttons/Trigger.h b/wpilibc/src/main/native/include/Buttons/Trigger.h index a5de72b6c3..c50ad24a18 100644 --- a/wpilibc/src/main/native/include/Buttons/Trigger.h +++ b/wpilibc/src/main/native/include/Buttons/Trigger.h @@ -7,11 +7,9 @@ #pragma once -#include -#include +#include -#include "SmartDashboard/Sendable.h" -#include "networktables/NetworkTableEntry.h" +#include "SmartDashboard/SendableBase.h" namespace frc { @@ -30,10 +28,10 @@ class Command; * only have to write the {@link Trigger#Get()} method to get the full * functionality of the Trigger class. */ -class Trigger : public Sendable { +class Trigger : public SendableBase { public: Trigger() = default; - virtual ~Trigger() = default; + ~Trigger() override = default; bool Grab(); virtual bool Get() = 0; void WhenActive(Command* command); @@ -42,11 +40,10 @@ class Trigger : public Sendable { void CancelWhenActive(Command* command); void ToggleWhenActive(Command* command); - void InitTable(std::shared_ptr subtable) override; - std::string GetSmartDashboardType() const override; + void InitSendable(SendableBuilder& builder) override; - protected: - nt::NetworkTableEntry m_pressedEntry; + private: + std::atomic_bool m_sendablePressed{false}; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/Commands/Command.h b/wpilibc/src/main/native/include/Commands/Command.h index 898f372b8a..9eb97c1214 100644 --- a/wpilibc/src/main/native/include/Commands/Command.h +++ b/wpilibc/src/main/native/include/Commands/Command.h @@ -11,9 +11,10 @@ #include #include +#include + #include "ErrorBase.h" -#include "SmartDashboard/NamedSendable.h" -#include "networktables/NetworkTableEntry.h" +#include "SmartDashboard/SendableBase.h" namespace frc { @@ -44,16 +45,16 @@ class Subsystem; * @see CommandGroup * @see Subsystem */ -class Command : public ErrorBase, public NamedSendable { +class Command : public ErrorBase, public SendableBase { friend class CommandGroup; friend class Scheduler; public: Command(); - explicit Command(const std::string& name); + explicit Command(const llvm::Twine& name); explicit Command(double timeout); - Command(const std::string& name, double timeout); - virtual ~Command(); + Command(const llvm::Twine& name, double timeout); + ~Command() override = default; double TimeSinceInitialized() const; void Requires(Subsystem* s); bool IsCanceled() const; @@ -76,6 +77,7 @@ class Command : public ErrorBase, public NamedSendable { bool IsTimedOut() const; bool AssertUnlocked(const std::string& message); void SetParent(CommandGroup* parent); + bool IsParented() const; void ClearRequirements(); virtual void Initialize(); @@ -116,9 +118,6 @@ class Command : public ErrorBase, public NamedSendable { void StartRunning(); void StartTiming(); - // The name of this command - std::string m_name; - // The time since this command was initialized double m_startTime = -1; @@ -153,14 +152,7 @@ class Command : public ErrorBase, public NamedSendable { static int m_commandCounter; public: - std::string GetName() const override; - void InitTable(std::shared_ptr subtable) override; - std::string GetSmartDashboardType() const override; - - private: - nt::NetworkTableEntry m_runningEntry; - nt::NetworkTableEntry m_isParentedEntry; - NT_EntryListener m_runningListener = 0; + void InitSendable(SendableBuilder& builder) override; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/Commands/PIDCommand.h b/wpilibc/src/main/native/include/Commands/PIDCommand.h index 62c8e9528c..26f23b902c 100644 --- a/wpilibc/src/main/native/include/Commands/PIDCommand.h +++ b/wpilibc/src/main/native/include/Commands/PIDCommand.h @@ -54,8 +54,7 @@ class PIDCommand : public Command, public PIDOutput, public PIDSource { std::shared_ptr m_controller; public: - void InitTable(std::shared_ptr subtable) override; - std::string GetSmartDashboardType() const override; + void InitSendable(SendableBuilder& builder) override; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/Commands/PIDSubsystem.h b/wpilibc/src/main/native/include/Commands/PIDSubsystem.h index a15a5d53ad..0c5a27469c 100644 --- a/wpilibc/src/main/native/include/Commands/PIDSubsystem.h +++ b/wpilibc/src/main/native/include/Commands/PIDSubsystem.h @@ -35,7 +35,7 @@ class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource { PIDSubsystem(double p, double i, double d); PIDSubsystem(double p, double i, double d, double f); PIDSubsystem(double p, double i, double d, double f, double period); - virtual ~PIDSubsystem() = default; + ~PIDSubsystem() override = default; void Enable(); void Disable(); @@ -66,10 +66,6 @@ class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource { private: // The internal PIDController std::shared_ptr m_controller; - - public: - void InitTable(std::shared_ptr subtable) override; - std::string GetSmartDashboardType() const override; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/Commands/Scheduler.h b/wpilibc/src/main/native/include/Commands/Scheduler.h index 68f3354cbd..17c4c5b101 100644 --- a/wpilibc/src/main/native/include/Commands/Scheduler.h +++ b/wpilibc/src/main/native/include/Commands/Scheduler.h @@ -16,9 +16,7 @@ #include "Commands/Command.h" #include "ErrorBase.h" -#include "SmartDashboard/NamedSendable.h" -#include "SmartDashboard/SmartDashboard.h" -#include "networktables/NetworkTable.h" +#include "SmartDashboard/SendableBase.h" #include "networktables/NetworkTableEntry.h" namespace frc { @@ -26,7 +24,7 @@ namespace frc { class ButtonScheduler; class Subsystem; -class Scheduler : public ErrorBase, public NamedSendable { +class Scheduler : public ErrorBase, public SendableBase { public: static Scheduler* GetInstance(); @@ -39,15 +37,11 @@ class Scheduler : public ErrorBase, public NamedSendable { void ResetAll(); void SetEnabled(bool enabled); - void UpdateTable(); - std::string GetSmartDashboardType() const; - void InitTable(std::shared_ptr subTable); - std::string GetName() const; - std::string GetType() const; + void InitSendable(SendableBuilder& builder) override; private: Scheduler(); - virtual ~Scheduler() = default; + ~Scheduler() override = default; void ProcessCommandAddition(Command* command); diff --git a/wpilibc/src/main/native/include/Commands/Subsystem.h b/wpilibc/src/main/native/include/Commands/Subsystem.h index 78bd7bc215..02886e29fb 100644 --- a/wpilibc/src/main/native/include/Commands/Subsystem.h +++ b/wpilibc/src/main/native/include/Commands/Subsystem.h @@ -8,49 +8,50 @@ #pragma once #include -#include + +#include +#include #include "ErrorBase.h" -#include "SmartDashboard/NamedSendable.h" -#include "networktables/NetworkTableEntry.h" +#include "SmartDashboard/Sendable.h" +#include "SmartDashboard/SendableBase.h" namespace frc { class Command; -class Subsystem : public ErrorBase, public NamedSendable { +class Subsystem : public ErrorBase, public SendableBase { friend class Scheduler; public: - explicit Subsystem(const std::string& name); - virtual ~Subsystem() = default; + explicit Subsystem(const llvm::Twine& name); void SetDefaultCommand(Command* command); Command* GetDefaultCommand(); + llvm::StringRef GetDefaultCommandName(); void SetCurrentCommand(Command* command); Command* GetCurrentCommand() const; + llvm::StringRef GetCurrentCommandName() const; virtual void Periodic(); virtual void InitDefaultCommand(); + void AddChild(const llvm::Twine& name, std::shared_ptr child); + void AddChild(const llvm::Twine& name, Sendable* child); + void AddChild(const llvm::Twine& name, Sendable& child); + void AddChild(std::shared_ptr child); + void AddChild(Sendable* child); + void AddChild(Sendable& child); + private: void ConfirmCommand(); Command* m_currentCommand = nullptr; bool m_currentCommandChanged = true; Command* m_defaultCommand = nullptr; - std::string m_name; bool m_initializedDefaultCommand = false; public: - std::string GetName() const override; - void InitTable(std::shared_ptr subtable) override; - std::string GetSmartDashboardType() const override; - - protected: - nt::NetworkTableEntry m_hasDefaultEntry; - nt::NetworkTableEntry m_defaultEntry; - nt::NetworkTableEntry m_hasCommandEntry; - nt::NetworkTableEntry m_commandEntry; + void InitSendable(SendableBuilder& builder) override; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/Compressor.h b/wpilibc/src/main/native/include/Compressor.h index 2e84412a11..a9a7b90043 100644 --- a/wpilibc/src/main/native/include/Compressor.h +++ b/wpilibc/src/main/native/include/Compressor.h @@ -7,14 +7,11 @@ #pragma once -#include -#include - #include -#include "LiveWindow/LiveWindowSendable.h" +#include "ErrorBase.h" #include "SensorBase.h" -#include "networktables/NetworkTableEntry.h" +#include "SmartDashboard/SendableBase.h" namespace frc { @@ -33,11 +30,11 @@ namespace frc { * loop control. You can only turn off closed loop control, thereby stopping * the compressor from operating. */ -class Compressor : public SensorBase, public LiveWindowSendable { +class Compressor : public ErrorBase, public SendableBase { public: // Default PCM ID is 0 - explicit Compressor(int pcmID = GetDefaultSolenoidModule()); - virtual ~Compressor(); + explicit Compressor(int pcmID = SensorBase::GetDefaultSolenoidModule()); + ~Compressor() override = default; void Start(); void Stop(); @@ -58,11 +55,7 @@ class Compressor : public SensorBase, public LiveWindowSendable { bool GetCompressorNotConnectedFault() const; void ClearAllPCMStickyFaults(); - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; + void InitSendable(SendableBuilder& builder) override; protected: HAL_CompressorHandle m_compressorHandle; @@ -70,10 +63,6 @@ class Compressor : public SensorBase, public LiveWindowSendable { private: void SetCompressor(bool on); int m_module; - - nt::NetworkTableEntry m_enabledEntry; - nt::NetworkTableEntry m_pressureSwitchEntry; - NT_EntryListener m_enabledListener = 0; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/Counter.h b/wpilibc/src/main/native/include/Counter.h index 98bea46836..ee55ca6fce 100644 --- a/wpilibc/src/main/native/include/Counter.h +++ b/wpilibc/src/main/native/include/Counter.h @@ -8,16 +8,13 @@ #pragma once #include -#include #include #include #include "AnalogTrigger.h" #include "CounterBase.h" -#include "LiveWindow/LiveWindowSendable.h" #include "SensorBase.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -33,9 +30,7 @@ class DigitalGlitchFilter; * All counters will immediately start counting - Reset() them if you need them * to be zeroed before use. */ -class Counter : public SensorBase, - public CounterBase, - public LiveWindowSendable { +class Counter : public SensorBase, public CounterBase { public: enum Mode { kTwoPulse = 0, @@ -53,7 +48,7 @@ class Counter : public SensorBase, DigitalSource* downSource, bool inverted); Counter(EncodingType encodingType, std::shared_ptr upSource, std::shared_ptr downSource, bool inverted); - virtual ~Counter(); + ~Counter() override; void SetUpSource(int channel); void SetUpSource(AnalogTrigger* analogTrigger, AnalogTriggerType triggerType); @@ -96,11 +91,7 @@ class Counter : public SensorBase, int GetSamplesToAverage() const; int GetFPGAIndex() const { return m_index; } - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; + void InitSendable(SendableBuilder& builder) override; protected: // Makes the counter count up. @@ -115,7 +106,6 @@ class Counter : public SensorBase, private: int m_index = 0; // The index of this counter. - nt::NetworkTableEntry m_valueEntry; friend class DigitalGlitchFilter; }; diff --git a/wpilibc/src/main/native/include/DigitalGlitchFilter.h b/wpilibc/src/main/native/include/DigitalGlitchFilter.h index 4a3c2236c1..3f087a7829 100644 --- a/wpilibc/src/main/native/include/DigitalGlitchFilter.h +++ b/wpilibc/src/main/native/include/DigitalGlitchFilter.h @@ -14,6 +14,7 @@ #include #include "DigitalSource.h" +#include "SensorBase.h" namespace frc { @@ -30,7 +31,7 @@ class Counter; class DigitalGlitchFilter : public SensorBase { public: DigitalGlitchFilter(); - ~DigitalGlitchFilter(); + ~DigitalGlitchFilter() override; void Add(DigitalSource* input); void Add(Encoder* input); @@ -46,6 +47,8 @@ class DigitalGlitchFilter : public SensorBase { int GetPeriodCycles(); uint64_t GetPeriodNanoSeconds(); + void InitSendable(SendableBuilder& builder) override; + private: // Sets the filter for the input to be the requested index. A value of 0 // disables the filter, and the filter value must be between 1 and 3, diff --git a/wpilibc/src/main/native/include/DigitalInput.h b/wpilibc/src/main/native/include/DigitalInput.h index 91cc1097e8..177afe1ffa 100644 --- a/wpilibc/src/main/native/include/DigitalInput.h +++ b/wpilibc/src/main/native/include/DigitalInput.h @@ -7,12 +7,7 @@ #pragma once -#include -#include - #include "DigitalSource.h" -#include "LiveWindow/LiveWindowSendable.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -27,10 +22,10 @@ class DigitalGlitchFilter; * as required. This class is only for devices like switches etc. that aren't * implemented anywhere else. */ -class DigitalInput : public DigitalSource, public LiveWindowSendable { +class DigitalInput : public DigitalSource { public: explicit DigitalInput(int channel); - virtual ~DigitalInput(); + ~DigitalInput() override; bool Get() const; int GetChannel() const override; @@ -39,17 +34,12 @@ class DigitalInput : public DigitalSource, public LiveWindowSendable { AnalogTriggerType GetAnalogTriggerTypeForRouting() const override; bool IsAnalogTrigger() const override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; + void InitSendable(SendableBuilder& builder) override; private: int m_channel; HAL_DigitalHandle m_handle; - nt::NetworkTableEntry m_valueEntry; friend class DigitalGlitchFilter; }; diff --git a/wpilibc/src/main/native/include/DigitalOutput.h b/wpilibc/src/main/native/include/DigitalOutput.h index 27f7434ecf..cee7919401 100644 --- a/wpilibc/src/main/native/include/DigitalOutput.h +++ b/wpilibc/src/main/native/include/DigitalOutput.h @@ -7,14 +7,10 @@ #pragma once -#include -#include - #include -#include "DigitalSource.h" -#include "LiveWindow/LiveWindowSendable.h" -#include "networktables/NetworkTableEntry.h" +#include "ErrorBase.h" +#include "SmartDashboard/SendableBase.h" namespace frc { @@ -25,13 +21,13 @@ namespace frc { * elsewhere will allocate channels automatically so for those devices it * shouldn't be done here. */ -class DigitalOutput : public DigitalSource, public LiveWindowSendable { +class DigitalOutput : public ErrorBase, public SendableBase { public: explicit DigitalOutput(int channel); - virtual ~DigitalOutput(); + ~DigitalOutput() override; void Set(bool value); bool Get() const; - int GetChannel() const override; + int GetChannel() const; void Pulse(double length); bool IsPulsing() const; void SetPWMRate(double rate); @@ -39,24 +35,12 @@ class DigitalOutput : public DigitalSource, public LiveWindowSendable { void DisablePWM(); void UpdateDutyCycle(double dutyCycle); - // Digital Source Interface - HAL_Handle GetPortHandleForRouting() const override; - AnalogTriggerType GetAnalogTriggerTypeForRouting() const override; - bool IsAnalogTrigger() const override; - - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; + void InitSendable(SendableBuilder& builder) override; private: int m_channel; HAL_DigitalHandle m_handle; HAL_DigitalPWMHandle m_pwmGenerator; - - nt::NetworkTableEntry m_valueEntry; - NT_EntryListener m_valueListener = 0; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/DigitalSource.h b/wpilibc/src/main/native/include/DigitalSource.h index 0481d75c24..e08a73147d 100644 --- a/wpilibc/src/main/native/include/DigitalSource.h +++ b/wpilibc/src/main/native/include/DigitalSource.h @@ -24,7 +24,6 @@ namespace frc { */ class DigitalSource : public InterruptableSensorBase { public: - virtual ~DigitalSource() = default; virtual HAL_Handle GetPortHandleForRouting() const = 0; virtual AnalogTriggerType GetAnalogTriggerTypeForRouting() const = 0; virtual bool IsAnalogTrigger() const = 0; diff --git a/wpilibc/src/main/native/include/DoubleSolenoid.h b/wpilibc/src/main/native/include/DoubleSolenoid.h index 572cf88bb5..2d1e85d922 100644 --- a/wpilibc/src/main/native/include/DoubleSolenoid.h +++ b/wpilibc/src/main/native/include/DoubleSolenoid.h @@ -7,14 +7,9 @@ #pragma once -#include -#include - #include -#include "LiveWindow/LiveWindowSendable.h" #include "SolenoidBase.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -25,23 +20,19 @@ namespace frc { * The DoubleSolenoid class is typically used for pneumatics solenoids that * have two positions controlled by two separate channels. */ -class DoubleSolenoid : public SolenoidBase, public LiveWindowSendable { +class DoubleSolenoid : public SolenoidBase { public: enum Value { kOff, kForward, kReverse }; explicit DoubleSolenoid(int forwardChannel, int reverseChannel); DoubleSolenoid(int moduleNumber, int forwardChannel, int reverseChannel); - virtual ~DoubleSolenoid(); + ~DoubleSolenoid() override; virtual void Set(Value value); virtual Value Get() const; bool IsFwdSolenoidBlackListed() const; bool IsRevSolenoidBlackListed() const; - void UpdateTable(); - void StartLiveWindowMode(); - void StopLiveWindowMode(); - std::string GetSmartDashboardType() const; - void InitTable(std::shared_ptr subTable); + void InitSendable(SendableBuilder& builder) override; private: int m_forwardChannel; // The forward channel on the module to control. @@ -50,9 +41,6 @@ class DoubleSolenoid : public SolenoidBase, public LiveWindowSendable { int m_reverseMask; // The mask for the reverse channel. HAL_SolenoidHandle m_forwardHandle = HAL_kInvalidHandle; HAL_SolenoidHandle m_reverseHandle = HAL_kInvalidHandle; - - nt::NetworkTableEntry m_valueEntry; - NT_EntryListener m_valueListener = 0; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/Drive/DifferentialDrive.h b/wpilibc/src/main/native/include/Drive/DifferentialDrive.h index ca1afc4169..a1f1f9ad77 100644 --- a/wpilibc/src/main/native/include/Drive/DifferentialDrive.h +++ b/wpilibc/src/main/native/include/Drive/DifferentialDrive.h @@ -102,7 +102,7 @@ class DifferentialDrive : public RobotDriveBase { static constexpr double kDefaultQuickStopAlpha = 0.1; DifferentialDrive(SpeedController& leftMotor, SpeedController& rightMotor); - virtual ~DifferentialDrive() = default; + ~DifferentialDrive() override = default; DifferentialDrive(const DifferentialDrive&) = delete; DifferentialDrive& operator=(const DifferentialDrive&) = delete; @@ -118,6 +118,8 @@ class DifferentialDrive : public RobotDriveBase { void StopMotor() override; void GetDescription(llvm::raw_ostream& desc) const override; + void InitSendable(SendableBuilder& builder) override; + private: SpeedController& m_leftMotor; SpeedController& m_rightMotor; diff --git a/wpilibc/src/main/native/include/Drive/KilloughDrive.h b/wpilibc/src/main/native/include/Drive/KilloughDrive.h index 52ece6d3cb..5631152c65 100644 --- a/wpilibc/src/main/native/include/Drive/KilloughDrive.h +++ b/wpilibc/src/main/native/include/Drive/KilloughDrive.h @@ -55,7 +55,7 @@ class KilloughDrive : public RobotDriveBase { KilloughDrive(SpeedController& leftMotor, SpeedController& rightMotor, SpeedController& backMotor, double leftMotorAngle, double rightMotorAngle, double backMotorAngle); - virtual ~KilloughDrive() = default; + ~KilloughDrive() override = default; KilloughDrive(const KilloughDrive&) = delete; KilloughDrive& operator=(const KilloughDrive&) = delete; @@ -67,6 +67,8 @@ class KilloughDrive : public RobotDriveBase { void StopMotor() override; void GetDescription(llvm::raw_ostream& desc) const override; + void InitSendable(SendableBuilder& builder) override; + private: SpeedController& m_leftMotor; SpeedController& m_rightMotor; diff --git a/wpilibc/src/main/native/include/Drive/MecanumDrive.h b/wpilibc/src/main/native/include/Drive/MecanumDrive.h index 33be8dff23..ba9e2eeda5 100644 --- a/wpilibc/src/main/native/include/Drive/MecanumDrive.h +++ b/wpilibc/src/main/native/include/Drive/MecanumDrive.h @@ -67,7 +67,7 @@ class MecanumDrive : public RobotDriveBase { MecanumDrive(SpeedController& frontLeftMotor, SpeedController& rearLeftMotor, SpeedController& frontRightMotor, SpeedController& rearRightMotor); - virtual ~MecanumDrive() = default; + ~MecanumDrive() override = default; MecanumDrive(const MecanumDrive&) = delete; MecanumDrive& operator=(const MecanumDrive&) = delete; @@ -79,6 +79,8 @@ class MecanumDrive : public RobotDriveBase { void StopMotor() override; void GetDescription(llvm::raw_ostream& desc) const override; + void InitSendable(SendableBuilder& builder) override; + private: SpeedController& m_frontLeftMotor; SpeedController& m_rearLeftMotor; diff --git a/wpilibc/src/main/native/include/Drive/RobotDriveBase.h b/wpilibc/src/main/native/include/Drive/RobotDriveBase.h index dc940d8a0e..06f2b1d3f9 100644 --- a/wpilibc/src/main/native/include/Drive/RobotDriveBase.h +++ b/wpilibc/src/main/native/include/Drive/RobotDriveBase.h @@ -12,9 +12,9 @@ #include #include -#include "ErrorBase.h" #include "MotorSafety.h" #include "MotorSafetyHelper.h" +#include "SmartDashboard/SendableBase.h" namespace frc { @@ -23,7 +23,7 @@ class SpeedController; /** * Common base class for drive platforms. */ -class RobotDriveBase : public MotorSafety, public ErrorBase { +class RobotDriveBase : public MotorSafety, public SendableBase { public: /** * The location of a motor on the robot for the purpose of driving. @@ -39,7 +39,7 @@ class RobotDriveBase : public MotorSafety, public ErrorBase { }; RobotDriveBase(); - virtual ~RobotDriveBase() = default; + ~RobotDriveBase() override = default; RobotDriveBase(const RobotDriveBase&) = delete; RobotDriveBase& operator=(const RobotDriveBase&) = delete; diff --git a/wpilibc/src/main/native/include/DriverStation.h b/wpilibc/src/main/native/include/DriverStation.h index 3954a45443..28678e466c 100644 --- a/wpilibc/src/main/native/include/DriverStation.h +++ b/wpilibc/src/main/native/include/DriverStation.h @@ -17,8 +17,8 @@ #include #include +#include "ErrorBase.h" #include "RobotState.h" -#include "SensorBase.h" namespace frc { @@ -28,12 +28,12 @@ struct MatchInfoData; * Provide access to the network communication data to / from the Driver * Station. */ -class DriverStation : public SensorBase, public RobotStateInterface { +class DriverStation : public ErrorBase, public RobotStateInterface { public: enum Alliance { kRed, kBlue, kInvalid }; enum MatchType { kNone, kPractice, kQualification, kElimination }; - virtual ~DriverStation(); + ~DriverStation() override; static DriverStation& GetInstance(); static void ReportError(llvm::StringRef error); static void ReportWarning(llvm::StringRef error); diff --git a/wpilibc/src/main/native/include/Encoder.h b/wpilibc/src/main/native/include/Encoder.h index fb3270cb46..b34f73ba29 100644 --- a/wpilibc/src/main/native/include/Encoder.h +++ b/wpilibc/src/main/native/include/Encoder.h @@ -8,16 +8,13 @@ #pragma once #include -#include #include #include "Counter.h" #include "CounterBase.h" -#include "LiveWindow/LiveWindowSendable.h" #include "PIDSource.h" #include "SensorBase.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -39,10 +36,7 @@ class DigitalGlitchFilter; * All encoders will immediately start counting - Reset() them if you need them * to be zeroed before use. */ -class Encoder : public SensorBase, - public CounterBase, - public PIDSource, - public LiveWindowSendable { +class Encoder : public SensorBase, public CounterBase, public PIDSource { public: enum IndexingType { kResetWhileHigh, @@ -60,7 +54,7 @@ class Encoder : public SensorBase, bool reverseDirection = false, EncodingType encodingType = k4X); Encoder(DigitalSource& aSource, DigitalSource& bSource, bool reverseDirection = false, EncodingType encodingType = k4X); - virtual ~Encoder(); + ~Encoder() override; // CounterBase interface int Get() const override; @@ -76,6 +70,7 @@ class Encoder : public SensorBase, double GetRate() const; void SetMinRate(double minRate); void SetDistancePerPulse(double distancePerPulse); + double GetDistancePerPulse() const; void SetReverseDirection(bool reverseDirection); void SetSamplesToAverage(int samplesToAverage); int GetSamplesToAverage() const; @@ -85,11 +80,7 @@ class Encoder : public SensorBase, void SetIndexSource(const DigitalSource& source, IndexingType type = kResetOnRisingEdge); - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; + void InitSendable(SendableBuilder& builder) override; int GetFPGAIndex() const; @@ -103,9 +94,6 @@ class Encoder : public SensorBase, std::unique_ptr m_indexSource = nullptr; HAL_EncoderHandle m_encoder = HAL_kInvalidHandle; - nt::NetworkTableEntry m_speedEntry; - nt::NetworkTableEntry m_distanceEntry; - nt::NetworkTableEntry m_distancePerTickEntry; friend class DigitalGlitchFilter; }; diff --git a/wpilibc/src/main/native/include/GearTooth.h b/wpilibc/src/main/native/include/GearTooth.h index 75fbf542ce..23dff4b8dd 100644 --- a/wpilibc/src/main/native/include/GearTooth.h +++ b/wpilibc/src/main/native/include/GearTooth.h @@ -30,11 +30,10 @@ class GearTooth : public Counter { explicit GearTooth(DigitalSource* source, bool directionSensitive = false); explicit GearTooth(std::shared_ptr source, bool directionSensitive = false); - virtual ~GearTooth() = default; void EnableDirectionSensing(bool directionSensitive); - std::string GetSmartDashboardType() const override; + void InitSendable(SendableBuilder& builder) override; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/GyroBase.h b/wpilibc/src/main/native/include/GyroBase.h index e8437f2a01..9c99698d96 100644 --- a/wpilibc/src/main/native/include/GyroBase.h +++ b/wpilibc/src/main/native/include/GyroBase.h @@ -7,14 +7,9 @@ #pragma once -#include -#include - -#include "LiveWindow/LiveWindowSendable.h" #include "PIDSource.h" #include "SensorBase.h" #include "interfaces/Gyro.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -22,24 +17,12 @@ namespace frc { * GyroBase is the common base class for Gyro implementations such as * AnalogGyro. */ -class GyroBase : public Gyro, - public SensorBase, - public PIDSource, - public LiveWindowSendable { +class GyroBase : public Gyro, public SensorBase, public PIDSource { public: - virtual ~GyroBase() = default; - // PIDSource interface double PIDGet() override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - - private: - nt::NetworkTableEntry m_valueEntry; + void InitSendable(SendableBuilder& builder) override; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/I2C.h b/wpilibc/src/main/native/include/I2C.h index a7d669b975..9631993db7 100644 --- a/wpilibc/src/main/native/include/I2C.h +++ b/wpilibc/src/main/native/include/I2C.h @@ -9,7 +9,7 @@ #include -#include "SensorBase.h" +#include "ErrorBase.h" enum HAL_I2CPort : int32_t; @@ -21,12 +21,12 @@ namespace frc { * This class is intended to be used by sensor (and other I2C device) drivers. * It probably should not be used directly. */ -class I2C : SensorBase { +class I2C : public ErrorBase { public: enum Port { kOnboard = 0, kMXP }; I2C(Port port, int deviceAddress); - virtual ~I2C(); + ~I2C() override; I2C(const I2C&) = delete; I2C& operator=(const I2C&) = delete; diff --git a/wpilibc/src/main/native/include/InterruptableSensorBase.h b/wpilibc/src/main/native/include/InterruptableSensorBase.h index 2c3aaf40a9..7830507a96 100644 --- a/wpilibc/src/main/native/include/InterruptableSensorBase.h +++ b/wpilibc/src/main/native/include/InterruptableSensorBase.h @@ -24,7 +24,6 @@ class InterruptableSensorBase : public SensorBase { }; InterruptableSensorBase() = default; - virtual ~InterruptableSensorBase() = default; virtual HAL_Handle GetPortHandleForRouting() const = 0; virtual AnalogTriggerType GetAnalogTriggerTypeForRouting() const = 0; diff --git a/wpilibc/src/main/native/include/Jaguar.h b/wpilibc/src/main/native/include/Jaguar.h index 85cb404a5b..bcb9c20bb4 100644 --- a/wpilibc/src/main/native/include/Jaguar.h +++ b/wpilibc/src/main/native/include/Jaguar.h @@ -17,7 +17,6 @@ namespace frc { class Jaguar : public PWMSpeedController { public: explicit Jaguar(int channel); - virtual ~Jaguar() = default; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/LiveWindow/LiveWindow.h b/wpilibc/src/main/native/include/LiveWindow/LiveWindow.h index 1c755c8266..c8dc66e7a7 100644 --- a/wpilibc/src/main/native/include/LiveWindow/LiveWindow.h +++ b/wpilibc/src/main/native/include/LiveWindow/LiveWindow.h @@ -7,82 +7,76 @@ #pragma once -#include #include -#include -#include -#include "Commands/Scheduler.h" -#include "LiveWindow/LiveWindowSendable.h" -#include "networktables/NetworkTable.h" -#include "networktables/NetworkTableEntry.h" +#include +#include + +#include "SmartDashboard/Sendable.h" namespace frc { -struct LiveWindowComponent { - std::string subsystem; - std::string name; - bool isSensor = false; - - LiveWindowComponent() = default; - LiveWindowComponent(std::string subsystem, std::string name, bool isSensor) { - this->subsystem = subsystem; - this->name = name; - this->isSensor = isSensor; - } -}; - /** * The LiveWindow class is the public interface for putting sensors and * actuators on the LiveWindow. */ class LiveWindow { public: + LiveWindow(const LiveWindow&) = delete; + LiveWindow& operator=(const LiveWindow&) = delete; + static LiveWindow* GetInstance(); - void Run(); - void AddSensor(const std::string& subsystem, const std::string& name, - LiveWindowSendable* component); - void AddSensor(const std::string& subsystem, const std::string& name, - LiveWindowSendable& component); - void AddSensor(const std::string& subsystem, const std::string& name, - std::shared_ptr component); - void AddActuator(const std::string& subsystem, const std::string& name, - LiveWindowSendable* component); - void AddActuator(const std::string& subsystem, const std::string& name, - LiveWindowSendable& component); - void AddActuator(const std::string& subsystem, const std::string& name, - std::shared_ptr component); - void AddSensor(std::string type, int channel, LiveWindowSendable* component); - void AddActuator(std::string type, int channel, - LiveWindowSendable* component); - void AddActuator(std::string type, int module, int channel, - LiveWindowSendable* component); + WPI_DEPRECATED("no longer required") + void Run() { UpdateValues(); } - bool IsEnabled() const { return m_enabled; } + WPI_DEPRECATED("use Sendable::SetName() instead") + void AddSensor(const llvm::Twine& subsystem, const llvm::Twine& name, + Sendable* component); + WPI_DEPRECATED("use Sendable::SetName() instead") + void AddSensor(const llvm::Twine& subsystem, const llvm::Twine& name, + Sendable& component); + WPI_DEPRECATED("use Sendable::SetName() instead") + void AddSensor(const llvm::Twine& subsystem, const llvm::Twine& name, + std::shared_ptr component); + WPI_DEPRECATED("use Sendable::SetName() instead") + void AddActuator(const llvm::Twine& subsystem, const llvm::Twine& name, + Sendable* component); + WPI_DEPRECATED("use Sendable::SetName() instead") + void AddActuator(const llvm::Twine& subsystem, const llvm::Twine& name, + Sendable& component); + WPI_DEPRECATED("use Sendable::SetName() instead") + void AddActuator(const llvm::Twine& subsystem, const llvm::Twine& name, + std::shared_ptr component); + + WPI_DEPRECATED("use SensorBase::SetName() instead") + void AddSensor(const llvm::Twine& type, int channel, Sendable* component); + WPI_DEPRECATED("use SensorBase::SetName() instead") + void AddActuator(const llvm::Twine& type, int channel, Sendable* component); + WPI_DEPRECATED("use SensorBase::SetName() instead") + void AddActuator(const llvm::Twine& type, int module, int channel, + Sendable* component); + + void Add(std::shared_ptr component); + void Add(Sendable* component); + void AddChild(Sendable* parent, std::shared_ptr component); + void AddChild(Sendable* parent, void* component); + void Remove(Sendable* component); + + void EnableTelemetry(Sendable* component); + void DisableTelemetry(Sendable* component); + void DisableAllTelemetry(); + + bool IsEnabled() const; void SetEnabled(bool enabled); - protected: - LiveWindow(); - virtual ~LiveWindow() = default; + void UpdateValues(); private: - void UpdateValues(); - void Initialize(); - void InitializeLiveWindowComponents(); + LiveWindow(); - std::vector> m_sensors; - std::map, LiveWindowComponent> - m_components; - - std::shared_ptr m_liveWindowTable; - std::shared_ptr m_statusTable; - nt::NetworkTableEntry m_enabledEntry; - - Scheduler* m_scheduler; - - bool m_enabled = false; - bool m_firstTime = true; + struct Impl; + std::unique_ptr m_impl; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/LiveWindow/LiveWindowSendable.h b/wpilibc/src/main/native/include/LiveWindow/LiveWindowSendable.h deleted file mode 100644 index 18311f2fe2..0000000000 --- a/wpilibc/src/main/native/include/LiveWindow/LiveWindowSendable.h +++ /dev/null @@ -1,36 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2012-2017 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#pragma once - -#include "SmartDashboard/Sendable.h" - -namespace frc { - -/** - * Live Window Sendable is a special type of object sendable to the live window. - */ -class LiveWindowSendable : public Sendable { - public: - /** - * Update the table for this sendable object with the latest values. - */ - virtual void UpdateTable() = 0; - - /** - * Start having this sendable object automatically respond to value changes - * reflect the value on the table. - */ - virtual void StartLiveWindowMode() = 0; - - /** - * Stop having this sendable object automatically respond to value changes. - */ - virtual void StopLiveWindowMode() = 0; -}; - -} // namespace frc diff --git a/wpilibc/src/main/native/include/NidecBrushless.h b/wpilibc/src/main/native/include/NidecBrushless.h index 9a80da5a70..6cee6f0b98 100644 --- a/wpilibc/src/main/native/include/NidecBrushless.h +++ b/wpilibc/src/main/native/include/NidecBrushless.h @@ -8,28 +8,27 @@ #pragma once #include -#include -#include #include "DigitalOutput.h" -#include "LiveWindow/LiveWindowSendable.h" +#include "ErrorBase.h" #include "MotorSafety.h" #include "MotorSafetyHelper.h" #include "PWM.h" +#include "SmartDashboard/SendableBase.h" #include "SpeedController.h" -#include "networktables/NetworkTableEntry.h" namespace frc { /** * Nidec Brushless Motor. */ -class NidecBrushless : public SpeedController, - public MotorSafety, - public LiveWindowSendable { +class NidecBrushless : public ErrorBase, + public SendableBase, + public SpeedController, + public MotorSafety { public: NidecBrushless(int pwmChannel, int dioChannel); - ~NidecBrushless() = default; + ~NidecBrushless() override = default; // SpeedController interface void Set(double speed) override; @@ -55,13 +54,7 @@ class NidecBrushless : public SpeedController, int GetChannel() const; // Sendable interface - void InitTable(std::shared_ptr subtable) override; - std::string GetSmartDashboardType() const override; - - // LiveWindowSendable interface - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; + void InitSendable(SendableBuilder& builder) override; private: MotorSafetyHelper m_safetyHelper; @@ -70,8 +63,6 @@ class NidecBrushless : public SpeedController, DigitalOutput m_dio; PWM m_pwm; double m_speed = 0.0; - nt::NetworkTableEntry m_valueEntry; - int m_valueListener; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/PIDController.h b/wpilibc/src/main/native/include/PIDController.h index 6ed4a878fa..990fff1481 100644 --- a/wpilibc/src/main/native/include/PIDController.h +++ b/wpilibc/src/main/native/include/PIDController.h @@ -7,21 +7,20 @@ #pragma once -#include #include #include +#include #include #include "Base.h" #include "Controller.h" #include "Filters/LinearDigitalFilter.h" -#include "LiveWindow/LiveWindow.h" #include "Notifier.h" #include "PIDInterface.h" #include "PIDSource.h" +#include "SmartDashboard/SendableBase.h" #include "Timer.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -37,7 +36,7 @@ class PIDOutput; * in the integral and derivative calculations. Therefore, the sample rate * affects the controller's behavior for a given set of PID constants. */ -class PIDController : public LiveWindowSendable, public PIDInterface { +class PIDController : public SendableBase, public PIDInterface { public: PIDController(double p, double i, double d, PIDSource* source, PIDOutput* output, double period = 0.05); @@ -47,7 +46,7 @@ class PIDController : public LiveWindowSendable, public PIDInterface { PIDOutput& output, double period = 0.05); PIDController(double p, double i, double d, double f, PIDSource& source, PIDOutput& output, double period = 0.05); - virtual ~PIDController(); + ~PIDController() override; PIDController(const PIDController&) = delete; PIDController& operator=(const PIDController) = delete; @@ -58,6 +57,10 @@ class PIDController : public LiveWindowSendable, public PIDInterface { virtual void SetOutputRange(double minimumOutput, double maximumOutput); void SetPID(double p, double i, double d) override; virtual void SetPID(double p, double i, double d, double f); + void SetP(double p); + void SetI(double i); + void SetD(double d); + void SetF(double f); double GetP() const override; double GetI() const override; double GetD() const override; @@ -87,29 +90,17 @@ class PIDController : public LiveWindowSendable, public PIDInterface { void Enable() override; void Disable() override; + void SetEnabled(bool enable); bool IsEnabled() const override; void Reset() override; - void InitTable(std::shared_ptr subtable) override; + void InitSendable(SendableBuilder& builder) override; protected: PIDSource* m_pidInput; PIDOutput* m_pidOutput; - nt::NetworkTableEntry m_pEntry; - nt::NetworkTableEntry m_iEntry; - nt::NetworkTableEntry m_dEntry; - nt::NetworkTableEntry m_fEntry; - nt::NetworkTableEntry m_setpointEntry; - nt::NetworkTableEntry m_enabledEntry; - NT_EntryListener m_pListener = 0; - NT_EntryListener m_iListener = 0; - NT_EntryListener m_dListener = 0; - NT_EntryListener m_fListener = 0; - NT_EntryListener m_setpointListener = 0; - NT_EntryListener m_enabledListener = 0; - virtual void Calculate(); virtual double CalculateFeedForward(); double GetContinuousError(double error) const; @@ -180,12 +171,6 @@ class PIDController : public LiveWindowSendable, public PIDInterface { std::unique_ptr m_controlLoop; Timer m_setpointTimer; - - std::string GetSmartDashboardType() const override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - void RemoveListeners(); }; } // namespace frc diff --git a/wpilibc/src/main/native/include/PIDInterface.h b/wpilibc/src/main/native/include/PIDInterface.h index b54e2e7002..86528f2b9a 100644 --- a/wpilibc/src/main/native/include/PIDInterface.h +++ b/wpilibc/src/main/native/include/PIDInterface.h @@ -9,7 +9,6 @@ #include "Base.h" #include "Controller.h" -#include "LiveWindow/LiveWindow.h" namespace frc { diff --git a/wpilibc/src/main/native/include/PWM.h b/wpilibc/src/main/native/include/PWM.h index 87e3a85b5a..1b3b8edc13 100644 --- a/wpilibc/src/main/native/include/PWM.h +++ b/wpilibc/src/main/native/include/PWM.h @@ -9,14 +9,10 @@ #include -#include -#include - #include -#include "LiveWindow/LiveWindowSendable.h" -#include "SensorBase.h" -#include "networktables/NetworkTableEntry.h" +#include "ErrorBase.h" +#include "SmartDashboard/SendableBase.h" namespace frc { @@ -37,7 +33,7 @@ namespace frc { * - 1 = minimum pulse width (currently .5ms) * - 0 = disabled (i.e. PWM output is held low) */ -class PWM : public SensorBase, public LiveWindowSendable { +class PWM : public ErrorBase, public SendableBase { public: /** * Represents the amount to multiply the minimum servo-pulse pwm period by. @@ -58,7 +54,7 @@ class PWM : public SensorBase, public LiveWindowSendable { }; explicit PWM(int channel); - virtual ~PWM(); + ~PWM() override; virtual void SetRaw(uint16_t value); virtual uint16_t GetRaw() const; virtual void SetPosition(double pos); @@ -78,14 +74,7 @@ class PWM : public SensorBase, public LiveWindowSendable { int GetChannel() const { return m_channel; } protected: - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - - nt::NetworkTableEntry m_valueEntry; - NT_EntryListener m_valueListener = 0; + void InitSendable(SendableBuilder& builder) override; private: int m_channel; diff --git a/wpilibc/src/main/native/include/PWMSpeedController.h b/wpilibc/src/main/native/include/PWMSpeedController.h index 521a13189f..af9b24da11 100644 --- a/wpilibc/src/main/native/include/PWMSpeedController.h +++ b/wpilibc/src/main/native/include/PWMSpeedController.h @@ -17,7 +17,6 @@ namespace frc { */ class PWMSpeedController : public SafePWM, public SpeedController { public: - virtual ~PWMSpeedController() = default; void Set(double value) override; double Get() const override; void SetInverted(bool isInverted) override; diff --git a/wpilibc/src/main/native/include/PWMTalonSRX.h b/wpilibc/src/main/native/include/PWMTalonSRX.h index 6d00de0f31..c9df091791 100644 --- a/wpilibc/src/main/native/include/PWMTalonSRX.h +++ b/wpilibc/src/main/native/include/PWMTalonSRX.h @@ -18,7 +18,6 @@ namespace frc { class PWMTalonSRX : public PWMSpeedController { public: explicit PWMTalonSRX(int channel); - virtual ~PWMTalonSRX() = default; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/PowerDistributionPanel.h b/wpilibc/src/main/native/include/PowerDistributionPanel.h index 78e44682a9..63b2d70061 100644 --- a/wpilibc/src/main/native/include/PowerDistributionPanel.h +++ b/wpilibc/src/main/native/include/PowerDistributionPanel.h @@ -7,12 +7,7 @@ #pragma once -#include -#include - -#include "LiveWindow/LiveWindowSendable.h" #include "SensorBase.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -20,7 +15,7 @@ namespace frc { * Class for getting voltage, current, temperature, power and energy from the * CAN PDP. */ -class PowerDistributionPanel : public SensorBase, public LiveWindowSendable { +class PowerDistributionPanel : public SensorBase { public: PowerDistributionPanel(); explicit PowerDistributionPanel(int module); @@ -34,16 +29,9 @@ class PowerDistributionPanel : public SensorBase, public LiveWindowSendable { void ResetTotalEnergy(); void ClearStickyFaults(); - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; + void InitSendable(SendableBuilder& builder) override; private: - nt::NetworkTableEntry m_chanEntry[16]; - nt::NetworkTableEntry m_voltageEntry; - nt::NetworkTableEntry m_totalCurrentEntry; int m_module; }; diff --git a/wpilibc/src/main/native/include/Relay.h b/wpilibc/src/main/native/include/Relay.h index 20904a08a4..cbda8ccfa6 100644 --- a/wpilibc/src/main/native/include/Relay.h +++ b/wpilibc/src/main/native/include/Relay.h @@ -8,15 +8,13 @@ #pragma once #include -#include #include #include -#include "LiveWindow/LiveWindowSendable.h" +#include "ErrorBase.h" #include "MotorSafety.h" -#include "SensorBase.h" -#include "networktables/NetworkTableEntry.h" +#include "SmartDashboard/SendableBase.h" namespace frc { @@ -34,13 +32,13 @@ class MotorSafetyHelper; * independently for something that does not care about voltage polarity (like * a solenoid). */ -class Relay : public MotorSafety, public SensorBase, public LiveWindowSendable { +class Relay : public MotorSafety, public ErrorBase, public SendableBase { public: enum Value { kOff, kOn, kForward, kReverse }; enum Direction { kBothDirections, kForwardOnly, kReverseOnly }; explicit Relay(int channel, Direction direction = kBothDirections); - virtual ~Relay(); + ~Relay() override; void Set(Value value); Value Get() const; @@ -54,15 +52,7 @@ class Relay : public MotorSafety, public SensorBase, public LiveWindowSendable { void SetSafetyEnabled(bool enabled) override; void GetDescription(llvm::raw_ostream& desc) const override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - - protected: - nt::NetworkTableEntry m_valueEntry; - NT_EntryListener m_valueListener = 0; + void InitSendable(SendableBuilder& builder) override; private: int m_channel; diff --git a/wpilibc/src/main/native/include/SD540.h b/wpilibc/src/main/native/include/SD540.h index 1a07cf1af1..e7311a909b 100644 --- a/wpilibc/src/main/native/include/SD540.h +++ b/wpilibc/src/main/native/include/SD540.h @@ -17,7 +17,6 @@ namespace frc { class SD540 : public PWMSpeedController { public: explicit SD540(int channel); - virtual ~SD540() = default; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/SPI.h b/wpilibc/src/main/native/include/SPI.h index 4c2a9b130d..315361addf 100644 --- a/wpilibc/src/main/native/include/SPI.h +++ b/wpilibc/src/main/native/include/SPI.h @@ -9,15 +9,12 @@ #include -#include "SensorBase.h" +#include "ErrorBase.h" enum HAL_SPIPort : int32_t; namespace frc { -class DigitalOutput; -class DigitalInput; - /** * SPI bus interface class. * @@ -25,12 +22,12 @@ class DigitalInput; * It probably should not be used directly. * */ -class SPI : public SensorBase { +class SPI : public ErrorBase { public: enum Port { kOnboardCS0 = 0, kOnboardCS1, kOnboardCS2, kOnboardCS3, kMXP }; explicit SPI(Port port); - virtual ~SPI(); + ~SPI() override; SPI(const SPI&) = delete; SPI& operator=(const SPI&) = delete; diff --git a/wpilibc/src/main/native/include/SensorBase.h b/wpilibc/src/main/native/include/SensorBase.h index 40818d8cc3..9917210029 100644 --- a/wpilibc/src/main/native/include/SensorBase.h +++ b/wpilibc/src/main/native/include/SensorBase.h @@ -7,8 +7,12 @@ #pragma once +#include + #include "Base.h" #include "ErrorBase.h" +#include "SmartDashboard/Sendable.h" +#include "SmartDashboard/SendableBase.h" namespace frc { @@ -18,10 +22,9 @@ namespace frc { * Stores most recent status information as well as containing utility functions * for checking channels and error processing. */ -class SensorBase : public ErrorBase { +class SensorBase : public ErrorBase, public SendableBase { public: SensorBase() = default; - virtual ~SensorBase() = default; SensorBase(const SensorBase&) = delete; SensorBase& operator=(const SensorBase&) = delete; diff --git a/wpilibc/src/main/native/include/Servo.h b/wpilibc/src/main/native/include/Servo.h index 4c0b45cad4..31a116d4a6 100644 --- a/wpilibc/src/main/native/include/Servo.h +++ b/wpilibc/src/main/native/include/Servo.h @@ -7,12 +7,8 @@ #pragma once -#include -#include - #include "SafePWM.h" #include "SpeedController.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -25,7 +21,6 @@ namespace frc { class Servo : public SafePWM { public: explicit Servo(int channel); - virtual ~Servo(); void Set(double value); void SetOffline(); double Get() const; @@ -34,15 +29,7 @@ class Servo : public SafePWM { static double GetMaxAngle() { return kMaxServoAngle; } static double GetMinAngle() { return kMinServoAngle; } - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - - protected: - nt::NetworkTableEntry m_valueEntry; - NT_EntryListener m_valueListener = 0; + void InitSendable(SendableBuilder& builder) override; private: double GetServoAngleRange() const { return kMaxServoAngle - kMinServoAngle; } diff --git a/wpilibc/src/main/native/include/SmartDashboard/NamedSendable.h b/wpilibc/src/main/native/include/SmartDashboard/NamedSendable.h deleted file mode 100644 index 1fb2a5a325..0000000000 --- a/wpilibc/src/main/native/include/SmartDashboard/NamedSendable.h +++ /dev/null @@ -1,29 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2012-2017 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#pragma once - -#include - -#include "SmartDashboard/Sendable.h" - -namespace frc { - -/** - * The interface for sendable objects that gives the sendable a default name in - * the Smart Dashboard. - */ -class NamedSendable : public Sendable { - public: - /** - * @return The name of the subtable of SmartDashboard that the Sendable object - * will use - */ - virtual std::string GetName() const = 0; -}; - -} // namespace frc diff --git a/wpilibc/src/main/native/include/SmartDashboard/Sendable.h b/wpilibc/src/main/native/include/SmartDashboard/Sendable.h index 8ebcf54262..077e923934 100644 --- a/wpilibc/src/main/native/include/SmartDashboard/Sendable.h +++ b/wpilibc/src/main/native/include/SmartDashboard/Sendable.h @@ -7,27 +7,63 @@ #pragma once -#include #include -#include "networktables/NetworkTable.h" +#include namespace frc { +class SendableBuilder; + class Sendable { public: - /** - * Initializes a table for this sendable object. - * - * @param subtable The table to put the values in. - */ - virtual void InitTable(std::shared_ptr subtable) = 0; + virtual ~Sendable() = default; /** - * @return The string representation of the named data type that will be used - * by the smart dashboard for this sendable + * Gets the name of this Sendable object. + * + * @return Name */ - virtual std::string GetSmartDashboardType() const = 0; + virtual std::string GetName() const = 0; + + /** + * Sets the name of this Sendable object. + * + * @param name name + */ + virtual void SetName(const llvm::Twine& name) = 0; + + /** + * Sets both the subsystem name and device name of this Sendable object. + * + * @param subsystem subsystem name + * @param name device name + */ + void SetName(const llvm::Twine& subsystem, const llvm::Twine& name) { + SetSubsystem(subsystem); + SetName(name); + } + + /** + * Gets the subsystem name of this Sendable object. + * + * @return Subsystem name + */ + virtual std::string GetSubsystem() const = 0; + + /** + * Sets the subsystem name of this Sendable object. + * + * @param subsystem subsystem name + */ + virtual void SetSubsystem(const llvm::Twine& subsystem) = 0; + + /** + * Initializes this Sendable object. + * + * @param builder sendable builder + */ + virtual void InitSendable(SendableBuilder& builder) = 0; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableBase.h b/wpilibc/src/main/native/include/SmartDashboard/SendableBase.h new file mode 100644 index 0000000000..4722bbdbd5 --- /dev/null +++ b/wpilibc/src/main/native/include/SmartDashboard/SendableBase.h @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include + +#include "Sendable.h" + +namespace frc { + +class SendableBase : public Sendable { + public: + explicit SendableBase(bool addLiveWindow = true); + ~SendableBase() override; + + using Sendable::SetName; + + std::string GetName() const final; + void SetName(const llvm::Twine& name) final; + std::string GetSubsystem() const final; + void SetSubsystem(const llvm::Twine& subsystem) final; + + protected: + void AddChild(std::shared_ptr child); + void AddChild(void* child); + + void SetName(const llvm::Twine& moduleType, int channel); + void SetName(const llvm::Twine& moduleType, int moduleNumber, int channel); + + private: + mutable wpi::mutex m_mutex; + std::string m_name; + std::string m_subsystem = "Ungrouped"; +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableBuilder.h b/wpilibc/src/main/native/include/SmartDashboard/SendableBuilder.h new file mode 100644 index 0000000000..8e76296d3b --- /dev/null +++ b/wpilibc/src/main/native/include/SmartDashboard/SendableBuilder.h @@ -0,0 +1,217 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include "networktables/NetworkTableEntry.h" +#include "networktables/NetworkTableValue.h" + +namespace frc { + +class SendableBuilder { + public: + virtual ~SendableBuilder() = default; + + /** + * Set the string representation of the named data type that will be used + * by the smart dashboard for this sendable. + * + * @param type data type + */ + virtual void SetSmartDashboardType(const llvm::Twine& type) = 0; + + /** + * Set the function that should be called to set the Sendable into a safe + * state. This is called when entering and exiting Live Window mode. + * + * @param func function + */ + virtual void SetSafeState(std::function func) = 0; + + /** + * Set the function that should be called to update the network table + * for things other than properties. Note this function is not passed + * the network table object; instead it should use the entry handles + * returned by GetEntry(). + * + * @param func function + */ + virtual void SetUpdateTable(std::function func) = 0; + + /** + * Add a property without getters or setters. This can be used to get + * entry handles for the function called by SetUpdateTable(). + * + * @param key property name + * @return Network table entry + */ + virtual nt::NetworkTableEntry GetEntry(const llvm::Twine& key) = 0; + + /** + * Add a boolean property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddBooleanProperty(const llvm::Twine& key, + std::function getter, + std::function setter) = 0; + + /** + * Add a double property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddDoubleProperty(const llvm::Twine& key, + std::function getter, + std::function setter) = 0; + + /** + * Add a string property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddStringProperty( + const llvm::Twine& key, std::function getter, + std::function setter) = 0; + + /** + * Add a boolean array property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddBooleanArrayProperty( + const llvm::Twine& key, std::function()> getter, + std::function)> setter) = 0; + + /** + * Add a double array property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddDoubleArrayProperty( + const llvm::Twine& key, std::function()> getter, + std::function)> setter) = 0; + + /** + * Add a string array property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddStringArrayProperty( + const llvm::Twine& key, std::function()> getter, + std::function)> setter) = 0; + + /** + * Add a raw property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddRawProperty(const llvm::Twine& key, + std::function getter, + std::function setter) = 0; + + /** + * Add a NetworkTableValue property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddValueProperty( + const llvm::Twine& key, + std::function()> getter, + std::function)> setter) = 0; + + /** + * Add a string property (SmallString form). + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddSmallStringProperty( + const llvm::Twine& key, + std::function& buf)> getter, + std::function setter) = 0; + + /** + * Add a boolean array property (SmallVector form). + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddSmallBooleanArrayProperty( + const llvm::Twine& key, + std::function(llvm::SmallVectorImpl& buf)> + getter, + std::function)> setter) = 0; + + /** + * Add a double array property (SmallVector form). + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddSmallDoubleArrayProperty( + const llvm::Twine& key, + std::function(llvm::SmallVectorImpl& buf)> + getter, + std::function)> setter) = 0; + + /** + * Add a string array property (SmallVector form). + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddSmallStringArrayProperty( + const llvm::Twine& key, + std::function< + llvm::ArrayRef(llvm::SmallVectorImpl& buf)> + getter, + std::function)> setter) = 0; + + /** + * Add a raw property (SmallVector form). + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + virtual void AddSmallRawProperty( + const llvm::Twine& key, + std::function& buf)> getter, + std::function setter) = 0; +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableBuilderImpl.h b/wpilibc/src/main/native/include/SmartDashboard/SendableBuilderImpl.h new file mode 100644 index 0000000000..5929262225 --- /dev/null +++ b/wpilibc/src/main/native/include/SmartDashboard/SendableBuilderImpl.h @@ -0,0 +1,182 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "SendableBuilder.h" +#include "networktables/NetworkTable.h" +#include "networktables/NetworkTableEntry.h" +#include "networktables/NetworkTableValue.h" + +namespace frc { + +class SendableBuilderImpl : public SendableBuilder { + public: + SendableBuilderImpl() = default; + SendableBuilderImpl(const SendableBuilderImpl&) = delete; + SendableBuilderImpl(SendableBuilderImpl&& other) = default; + SendableBuilderImpl& operator=(const SendableBuilderImpl&) = delete; + SendableBuilderImpl& operator=(SendableBuilderImpl&& other) = default; + ~SendableBuilderImpl() override = default; + + /** + * Set the network table. Must be called prior to any Add* functions being + * called. + * @param table Network table + */ + void SetTable(std::shared_ptr table); + + /** + * Get the network table. + * @return The network table + */ + std::shared_ptr GetTable(); + + /** + * Update the network table values by calling the getters for all properties. + */ + void UpdateTable(); + + /** + * Start LiveWindow mode by hooking the setters for all properties. + */ + void StartLiveWindowMode(); + + /** + * Stop LiveWindow mode by unhooking the setters for all properties. + */ + void StopLiveWindowMode(); + + void SetSmartDashboardType(const llvm::Twine& type) override; + void SetSafeState(std::function func) override; + void SetUpdateTable(std::function func) override; + nt::NetworkTableEntry GetEntry(const llvm::Twine& key) override; + + void AddBooleanProperty(const llvm::Twine& key, std::function getter, + std::function setter) override; + + void AddDoubleProperty(const llvm::Twine& key, std::function getter, + std::function setter) override; + + void AddStringProperty(const llvm::Twine& key, + std::function getter, + std::function setter) override; + + void AddBooleanArrayProperty( + const llvm::Twine& key, std::function()> getter, + std::function)> setter) override; + + void AddDoubleArrayProperty( + const llvm::Twine& key, std::function()> getter, + std::function)> setter) override; + + void AddStringArrayProperty( + const llvm::Twine& key, std::function()> getter, + std::function)> setter) override; + + void AddRawProperty(const llvm::Twine& key, + std::function getter, + std::function setter) override; + + void AddValueProperty( + const llvm::Twine& key, + std::function()> getter, + std::function)> setter) override; + + void AddSmallStringProperty( + const llvm::Twine& key, + std::function& buf)> getter, + std::function setter) override; + + void AddSmallBooleanArrayProperty( + const llvm::Twine& key, + std::function(llvm::SmallVectorImpl& buf)> + getter, + std::function)> setter) override; + + void AddSmallDoubleArrayProperty( + const llvm::Twine& key, + std::function(llvm::SmallVectorImpl& buf)> + getter, + std::function)> setter) override; + + void AddSmallStringArrayProperty( + const llvm::Twine& key, + std::function< + llvm::ArrayRef(llvm::SmallVectorImpl& buf)> + getter, + std::function)> setter) override; + + void AddSmallRawProperty( + const llvm::Twine& key, + std::function& buf)> getter, + std::function setter) override; + + private: + struct Property { + Property(nt::NetworkTable& table, const llvm::Twine& key) + : entry(table.GetEntry(key)) {} + + Property(const Property&) = delete; + Property& operator=(const Property&) = delete; + + Property(Property&& other) noexcept + : entry(other.entry), + listener(other.listener), + update(std::move(other.update)), + createListener(std::move(other.createListener)) { + other.entry = nt::NetworkTableEntry(); + other.listener = 0; + } + + Property& operator=(Property&& other) noexcept { + entry = other.entry; + listener = other.listener; + other.entry = nt::NetworkTableEntry(); + other.listener = 0; + update = std::move(other.update); + createListener = std::move(other.createListener); + return *this; + } + + ~Property() { StopListener(); } + + void StartListener() { + if (entry && listener == 0 && createListener) + listener = createListener(entry); + } + + void StopListener() { + if (entry && listener != 0) { + entry.RemoveListener(listener); + listener = 0; + } + } + + nt::NetworkTableEntry entry; + NT_EntryListener listener = 0; + std::function update; + std::function createListener; + }; + + std::vector m_properties; + std::function m_safeState; + std::function m_updateTable; + std::shared_ptr m_table; +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.h b/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.h index 1e3b774a0c..e42a8d4f71 100644 --- a/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.h +++ b/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.h @@ -8,14 +8,11 @@ #pragma once #include -#include #include #include #include "SmartDashboard/SendableChooserBase.h" -#include "networktables/NetworkTable.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -35,7 +32,6 @@ namespace frc { template class SendableChooser : public SendableChooserBase { llvm::StringMap m_choices; - nt::NetworkTableEntry m_selectedEntry; template static U _unwrap_smart_ptr(const U& value); @@ -47,14 +43,14 @@ class SendableChooser : public SendableChooserBase { static std::weak_ptr _unwrap_smart_ptr(const std::shared_ptr& value); public: - virtual ~SendableChooser() = default; + ~SendableChooser() override = default; void AddObject(llvm::StringRef name, T object); void AddDefault(llvm::StringRef name, T object); auto GetSelected() -> decltype(_unwrap_smart_ptr(m_choices[""])); - void InitTable(std::shared_ptr subtable) override; + void InitSendable(SendableBuilder& builder) override; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.inc b/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.inc index 1d0dbdad8b..8efb8a5a86 100644 --- a/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.inc +++ b/wpilibc/src/main/native/include/SmartDashboard/SendableChooser.inc @@ -13,6 +13,10 @@ #include #include +#include + +#include "SendableBuilder.h" + namespace frc { /** @@ -58,8 +62,11 @@ void SendableChooser::AddDefault(llvm::StringRef name, T object) { template auto SendableChooser::GetSelected() -> decltype(_unwrap_smart_ptr(m_choices[""])) { - std::string selected = m_selectedEntry.GetString(m_defaultChoice); - if (selected == "") { + llvm::StringRef selected = m_defaultChoice; + if (m_selectedEntry) { + selected = m_selectedEntry.GetString(m_defaultChoice); + } + if (selected.empty()) { return decltype(_unwrap_smart_ptr(m_choices[""])){}; } else { return _unwrap_smart_ptr(m_choices[selected]); @@ -67,21 +74,29 @@ auto SendableChooser::GetSelected() } template -void SendableChooser::InitTable(std::shared_ptr subtable) { - std::vector keys; - if (subtable) { - m_selectedEntry = subtable->GetEntry(kSelected); - for (const auto& choice : m_choices) { - keys.push_back(choice.first()); - } +void SendableChooser::InitSendable(SendableBuilder& builder) { + builder.SetSmartDashboardType("String Chooser"); + builder.AddStringArrayProperty(kOptions, + [=]() { + std::vector keys; + for (const auto& choice : m_choices) { + keys.push_back(choice.first()); + } - // Unlike std::map, llvm::StringMap elements are not sorted - std::sort(keys.begin(), keys.end()); + // Unlike std::map, llvm::StringMap elements + // are not sorted + std::sort(keys.begin(), keys.end()); - subtable->GetEntry(kOptions).SetValue( - nt::Value::MakeStringArray(std::move(keys))); - subtable->GetEntry(kDefault).SetString(m_defaultChoice); - } + return keys; + }, + nullptr); + builder.AddSmallStringProperty( + kDefault, + [=](const llvm::SmallVectorImpl&) -> llvm::StringRef { + return m_defaultChoice; + }, + nullptr); + m_selectedEntry = builder.GetEntry(kSelected); } template diff --git a/wpilibc/src/main/native/include/SmartDashboard/SendableChooserBase.h b/wpilibc/src/main/native/include/SmartDashboard/SendableChooserBase.h index 704c7b65ac..e62f410212 100644 --- a/wpilibc/src/main/native/include/SmartDashboard/SendableChooserBase.h +++ b/wpilibc/src/main/native/include/SmartDashboard/SendableChooserBase.h @@ -7,11 +7,10 @@ #pragma once -#include #include -#include "SmartDashboard/Sendable.h" -#include "networktables/NetworkTable.h" +#include "SmartDashboard/SendableBase.h" +#include "networktables/NetworkTableEntry.h" namespace frc { @@ -21,11 +20,9 @@ namespace frc { * It contains static, non-templated variables to avoid their duplication in the * template class. */ -class SendableChooserBase : public Sendable { +class SendableChooserBase : public SendableBase { public: - virtual ~SendableChooserBase() = default; - - std::string GetSmartDashboardType() const override; + ~SendableChooserBase() override = default; protected: static const char* kDefault; @@ -33,6 +30,7 @@ class SendableChooserBase : public Sendable { static const char* kSelected; std::string m_defaultChoice; + nt::NetworkTableEntry m_selectedEntry; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/SmartDashboard/SmartDashboard.h b/wpilibc/src/main/native/include/SmartDashboard/SmartDashboard.h index 2dec18bd70..bde423206d 100644 --- a/wpilibc/src/main/native/include/SmartDashboard/SmartDashboard.h +++ b/wpilibc/src/main/native/include/SmartDashboard/SmartDashboard.h @@ -16,7 +16,6 @@ namespace frc { -class NamedSendable; class Sendable; class SmartDashboard : public SensorBase { @@ -38,7 +37,7 @@ class SmartDashboard : public SensorBase { static void Delete(llvm::StringRef key); static void PutData(llvm::StringRef key, Sendable* data); - static void PutData(NamedSendable* value); + static void PutData(Sendable* value); static Sendable* GetData(llvm::StringRef keyName); static bool PutBoolean(llvm::StringRef keyName, bool value); diff --git a/wpilibc/src/main/native/include/Solenoid.h b/wpilibc/src/main/native/include/Solenoid.h index 3f43ced2db..59c134d3dc 100644 --- a/wpilibc/src/main/native/include/Solenoid.h +++ b/wpilibc/src/main/native/include/Solenoid.h @@ -7,14 +7,9 @@ #pragma once -#include -#include - #include -#include "LiveWindow/LiveWindowSendable.h" #include "SolenoidBase.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -24,28 +19,22 @@ namespace frc { * The Solenoid class is typically used for pneumatics solenoids, but could be * used for any device within the current spec of the PCM. */ -class Solenoid : public SolenoidBase, public LiveWindowSendable { +class Solenoid : public SolenoidBase { public: explicit Solenoid(int channel); Solenoid(int moduleNumber, int channel); - virtual ~Solenoid(); + ~Solenoid() override; virtual void Set(bool on); virtual bool Get() const; bool IsBlackListed() const; void SetPulseDuration(double durationSeconds); void StartPulse(); - void UpdateTable(); - void StartLiveWindowMode(); - void StopLiveWindowMode(); - std::string GetSmartDashboardType() const; - void InitTable(std::shared_ptr subTable); + void InitSendable(SendableBuilder& builder) override; private: HAL_SolenoidHandle m_solenoidHandle = HAL_kInvalidHandle; int m_channel; // The channel on the module to control - nt::NetworkTableEntry m_valueEntry; - NT_EntryListener m_valueListener = 0; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/SolenoidBase.h b/wpilibc/src/main/native/include/SolenoidBase.h index 7edc933eba..22101b89ef 100644 --- a/wpilibc/src/main/native/include/SolenoidBase.h +++ b/wpilibc/src/main/native/include/SolenoidBase.h @@ -7,7 +7,8 @@ #pragma once -#include "SensorBase.h" +#include "ErrorBase.h" +#include "SmartDashboard/SendableBase.h" namespace frc { @@ -15,9 +16,8 @@ namespace frc { * SolenoidBase class is the common base class for the Solenoid and * DoubleSolenoid classes. */ -class SolenoidBase : public SensorBase { +class SolenoidBase : public ErrorBase, public SendableBase { public: - virtual ~SolenoidBase() = default; static int GetAll(int module); int GetAll() const; diff --git a/wpilibc/src/main/native/include/Spark.h b/wpilibc/src/main/native/include/Spark.h index 3337aa1c0e..65e18210e0 100644 --- a/wpilibc/src/main/native/include/Spark.h +++ b/wpilibc/src/main/native/include/Spark.h @@ -17,7 +17,6 @@ namespace frc { class Spark : public PWMSpeedController { public: explicit Spark(int channel); - virtual ~Spark() = default; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/SpeedControllerGroup.h b/wpilibc/src/main/native/include/SpeedControllerGroup.h index ca862d6da4..ba7b75d349 100644 --- a/wpilibc/src/main/native/include/SpeedControllerGroup.h +++ b/wpilibc/src/main/native/include/SpeedControllerGroup.h @@ -10,15 +10,17 @@ #include #include +#include "SmartDashboard/SendableBase.h" #include "SpeedController.h" namespace frc { -class SpeedControllerGroup : public SpeedController { +class SpeedControllerGroup : public SendableBase, public SpeedController { public: template explicit SpeedControllerGroup(SpeedController& speedController, SpeedControllers&... speedControllers); + ~SpeedControllerGroup() override = default; void Set(double speed) override; double Get() const override; @@ -28,6 +30,8 @@ class SpeedControllerGroup : public SpeedController { void StopMotor() override; void PIDWrite(double output) override; + void InitSendable(SendableBuilder& builder) override; + private: bool m_isInverted = false; std::vector> m_speedControllers; diff --git a/wpilibc/src/main/native/include/SpeedControllerGroup.inc b/wpilibc/src/main/native/include/SpeedControllerGroup.inc index 44f06c2813..bef02f0899 100644 --- a/wpilibc/src/main/native/include/SpeedControllerGroup.inc +++ b/wpilibc/src/main/native/include/SpeedControllerGroup.inc @@ -12,6 +12,12 @@ namespace frc { template SpeedControllerGroup::SpeedControllerGroup( SpeedController& speedController, SpeedControllers&... speedControllers) - : m_speedControllers{speedController, speedControllers...} {} + : m_speedControllers{speedController, speedControllers...} { + for (auto& speedController : m_speedControllers) + AddChild(&speedController.get()); + static int instances = 0; + ++instances; + SetName("SpeedControllerGroup", instances); +} } // namespace frc diff --git a/wpilibc/src/main/native/include/Talon.h b/wpilibc/src/main/native/include/Talon.h index 926fa608d7..154229a9cd 100644 --- a/wpilibc/src/main/native/include/Talon.h +++ b/wpilibc/src/main/native/include/Talon.h @@ -17,7 +17,6 @@ namespace frc { class Talon : public PWMSpeedController { public: explicit Talon(int channel); - virtual ~Talon() = default; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/Ultrasonic.h b/wpilibc/src/main/native/include/Ultrasonic.h index 7772ae3bd5..c62a4ab789 100644 --- a/wpilibc/src/main/native/include/Ultrasonic.h +++ b/wpilibc/src/main/native/include/Ultrasonic.h @@ -9,15 +9,12 @@ #include #include -#include #include #include #include "Counter.h" -#include "LiveWindow/LiveWindowSendable.h" #include "PIDSource.h" #include "SensorBase.h" -#include "networktables/NetworkTableEntry.h" namespace frc { @@ -36,9 +33,7 @@ class DigitalOutput; * received. The time that the line is high determines the round trip distance * (time of flight). */ -class Ultrasonic : public SensorBase, - public PIDSource, - public LiveWindowSendable { +class Ultrasonic : public SensorBase, public PIDSource { public: enum DistanceUnit { kInches = 0, kMilliMeters = 1 }; @@ -50,7 +45,7 @@ class Ultrasonic : public SensorBase, std::shared_ptr echoChannel, DistanceUnit units = kInches); Ultrasonic(int pingChannel, int echoChannel, DistanceUnit units = kInches); - virtual ~Ultrasonic(); + ~Ultrasonic() override; void Ping(); bool IsRangeValid() const; @@ -65,11 +60,7 @@ class Ultrasonic : public SensorBase, void SetDistanceUnits(DistanceUnit units); DistanceUnit GetDistanceUnits() const; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; + void InitSendable(SendableBuilder& builder) override; private: void Initialize(); @@ -100,8 +91,6 @@ class Ultrasonic : public SensorBase, bool m_enabled = false; Counter m_counter; DistanceUnit m_units; - - nt::NetworkTableEntry m_valueEntry; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/Victor.h b/wpilibc/src/main/native/include/Victor.h index b7630b568a..8e3ecc108c 100644 --- a/wpilibc/src/main/native/include/Victor.h +++ b/wpilibc/src/main/native/include/Victor.h @@ -20,7 +20,6 @@ namespace frc { class Victor : public PWMSpeedController { public: explicit Victor(int channel); - virtual ~Victor() = default; }; } // namespace frc diff --git a/wpilibc/src/main/native/include/VictorSP.h b/wpilibc/src/main/native/include/VictorSP.h index 61940825b0..8f305c86d5 100644 --- a/wpilibc/src/main/native/include/VictorSP.h +++ b/wpilibc/src/main/native/include/VictorSP.h @@ -17,7 +17,6 @@ namespace frc { class VictorSP : public PWMSpeedController { public: explicit VictorSP(int channel); - virtual ~VictorSP() = default; }; } // namespace frc diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Robot.cpp index 1db76e154b..1b27b43975 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Robot.cpp @@ -45,8 +45,6 @@ void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); } -void Robot::TestPeriodic() { - m_lw.Run(); -} +void Robot::TestPeriodic() {} START_ROBOT_CLASS(Robot) diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Robot.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Robot.h index c201c8ab3c..4efc81b087 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Robot.h @@ -8,8 +8,10 @@ #pragma once #include +#include #include #include +#include #include "Commands/Autonomous.h" #include "OI.h" diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Claw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Claw.cpp index 99cb350256..4451e59ef3 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Claw.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Claw.cpp @@ -7,12 +7,10 @@ #include "Claw.h" -#include - Claw::Claw() : frc::Subsystem("Claw") { // Let's show everything on the LiveWindow - // frc::LiveWindow::GetInstance()->AddActuator("Claw", "Motor", &motor); + AddChild("Motor", m_motor); } void Claw::InitDefaultCommand() {} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/DriveTrain.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/DriveTrain.cpp index abcbeae94c..e1d1599e56 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/DriveTrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/DriveTrain.cpp @@ -8,7 +8,7 @@ #include "DriveTrain.h" #include -#include +#include #include "../Commands/TankDriveWithJoystick.h" @@ -31,25 +31,14 @@ DriveTrain::DriveTrain() #endif // Let's show everything on the LiveWindow - // frc::LiveWindow::GetInstance()->AddActuator("Drive Train", - // "Front_Left Motor", &m_frontLeft); - // frc::LiveWindow::GetInstance()->AddActuator("Drive Train", - // "Rear Left Motor", &m_rearLeft); - // frc::LiveWindow::GetInstance()->AddActuator("Drive Train", - // "Front Right Motor", &m_frontRight); - // frc::LiveWindow::GetInstance()->AddActuator("Drive Train", - // "Rear Right Motor", &m_rearRight); - // frc::LiveWindow::GetInstance()->AddSensor("Drive Train", "Left - // Encoder", - // &m_leftEncoder); - // frc::LiveWindow::GetInstance()->AddSensor("Drive Train", "Right - // Encoder", - // &m_rightEncoder); - // frc::LiveWindow::GetInstance()->AddSensor("Drive Train", - // "Rangefinder", - // &m_rangefinder); - // frc::LiveWindow::GetInstance()->AddSensor("Drive Train", "Gyro", - // &m_gyro); + // AddChild("Front_Left Motor", m_frontLeft); + // AddChild("Rear Left Motor", m_rearLeft); + // AddChild("Front Right Motor", m_frontRight); + // AddChild("Rear Right Motor", m_rearRight); + AddChild("Left Encoder", m_leftEncoder); + AddChild("Right Encoder", m_rightEncoder); + AddChild("Rangefinder", m_rangefinder); + AddChild("Gyro", m_gyro); } /** diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Elevator.cpp index d07d0828ff..51d7d24eca 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Elevator.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Elevator.cpp @@ -18,11 +18,8 @@ Elevator::Elevator() SetAbsoluteTolerance(0.005); // Let's show everything on the LiveWindow - // frc::LiveWindow::GetInstance()->AddActuator("Elevator", "Motor", - // &m_motor); - // frc::LiveWindow::GetInstance()->AddSensor("Elevator", "Pot", &m_pot); - // frc::LiveWindow::GetInstance()->AddActuator("Elevator", "PID", - // GetPIDController()); + AddChild("Motor", m_motor); + AddChild("Pot", &m_pot); } void Elevator::InitDefaultCommand() {} diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Wrist.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Wrist.cpp index 447a4313cf..dd65df5b7d 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Wrist.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/src/Subsystems/Wrist.cpp @@ -7,7 +7,6 @@ #include "Wrist.h" -#include #include Wrist::Wrist() @@ -18,11 +17,8 @@ Wrist::Wrist() SetAbsoluteTolerance(2.5); // Let's show everything on the LiveWindow - // frc::LiveWindow::GetInstance()->AddActuator("Wrist", "Motor", - // &motor); - // frc::LiveWindow::GetInstance()->AddSensor("Wrist", "Pot", &pot); - frc::LiveWindow::GetInstance()->AddActuator( - "Wrist", "PID", GetPIDController()); + AddChild("Motor", m_motor); + AddChild("Pot", m_pot); } void Wrist::InitDefaultCommand() {} diff --git a/wpilibcExamples/src/main/cpp/examples/GettingStarted/src/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GettingStarted/src/Robot.cpp index c1894a4fc7..4650807583 100644 --- a/wpilibcExamples/src/main/cpp/examples/GettingStarted/src/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GettingStarted/src/Robot.cpp @@ -42,7 +42,7 @@ public: m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX()); } - void TestPeriodic() override { m_lw.Run(); } + void TestPeriodic() override {} private: // Robot drive system diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/OI.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/OI.cpp index 2963ad4812..4bfa2f914b 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/OI.cpp +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/OI.cpp @@ -13,6 +13,7 @@ #include "Commands/SetCollectionSpeed.h" #include "Commands/SetPivotSetpoint.h" #include "Commands/Shoot.h" +#include "SmartDashboard/SmartDashboard.h" #include "Subsystems/Collector.h" #include "Subsystems/Pivot.h" diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Robot.cpp index f2f7c8034b..1886077dc3 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Robot.cpp @@ -9,6 +9,8 @@ #include +#include +#include #include DriveTrain Robot::drivetrain; @@ -60,9 +62,7 @@ void Robot::TeleopPeriodic() { Log(); } -void Robot::TestPeriodic() { - frc::LiveWindow::GetInstance()->Run(); -} +void Robot::TestPeriodic() {} void Robot::DisabledInit() { shooter.Unlatch(); diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Collector.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Collector.cpp index d5e5233fe9..457952c667 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Collector.cpp +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Collector.cpp @@ -12,14 +12,10 @@ Collector::Collector() : frc::Subsystem("Collector") { // Put everything to the LiveWindow for testing. - // XXX: LiveWindow::GetInstance()->AddActuator("Collector", "Roller - // Motor", &m_rollerMotor); - LiveWindow::GetInstance()->AddSensor( - "Collector", "Ball Detector", &m_ballDetector); - LiveWindow::GetInstance()->AddSensor( - "Collector", "Claw Open Detector", &m_openDetector); - LiveWindow::GetInstance()->AddActuator( - "Collector", "Piston", &m_piston); + AddChild("Roller Motor", m_rollerMotor); + AddChild("Ball Detector", m_ballDetector); + AddChild("Claw Open Detector", m_openDetector); + AddChild("Piston", m_piston); } bool Collector::HasBall() { diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/DriveTrain.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/DriveTrain.cpp index 5afdaaa92b..8a9118c737 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/DriveTrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/DriveTrain.cpp @@ -10,20 +10,15 @@ #include #include -#include #include "../Commands/DriveWithJoystick.h" DriveTrain::DriveTrain() : frc::Subsystem("DriveTrain") { - // frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Front Left - // CIM", &m_frontLeftCIM); - // frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Front - // Right CIM", &m_frontRightCIM); - // frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Back Left - // CIM", &m_backLeftCIM); - // frc::LiveWindow::GetInstance()->AddActuator("DriveTrain", "Back Right - // CIM", &m_backRightCIM); + // AddChild("Front Left CIM", m_frontLeftCIM); + // AddChild("Front Right CIM", m_frontRightCIM); + // AddChild("Back Left CIM", m_backLeftCIM); + // AddChild("Back Right CIM", m_backRightCIM); // Configure the DifferentialDrive to reflect the fact that all our // motors are wired backwards and our drivers sensitivity preferences. @@ -49,16 +44,14 @@ DriveTrain::DriveTrain() (4.0 /*in*/ * M_PI) / (360.0 * 12.0 /*in/ft*/)); #endif - LiveWindow::GetInstance()->AddSensor( - "DriveTrain", "Right Encoder", m_rightEncoder); - LiveWindow::GetInstance()->AddSensor( - "DriveTrain", "Left Encoder", m_leftEncoder); + AddChild("Right Encoder", m_rightEncoder); + AddChild("Left Encoder", m_leftEncoder); // Configure gyro #ifndef SIMULATION m_gyro.SetSensitivity(0.007); // TODO: Handle more gracefully? #endif - LiveWindow::GetInstance()->AddSensor("DriveTrain", "Gyro", &m_gyro); + AddChild("Gyro", m_gyro); } void DriveTrain::InitDefaultCommand() { diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pivot.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pivot.cpp index 59d4e153c4..9b7562ad4c 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pivot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pivot.cpp @@ -7,8 +7,6 @@ #include "Pivot.h" -#include - Pivot::Pivot() : frc::PIDSubsystem("Pivot", 7.0, 0.0, 8.0) { SetAbsoluteTolerance(0.005); @@ -20,16 +18,10 @@ Pivot::Pivot() #endif // Put everything to the LiveWindow for testing. - frc::LiveWindow::GetInstance()->AddSensor( - "Pivot", "Upper Limit Switch", &m_upperLimitSwitch); - frc::LiveWindow::GetInstance()->AddSensor( - "Pivot", "Lower Limit Switch", &m_lowerLimitSwitch); - // XXX: frc::LiveWindow::GetInstance()->AddSensor("Pivot", "Pot", - // &m_pot); - // XXX: frc::LiveWindow::GetInstance()->AddActuator("Pivot", "Motor", - // &m_motor); - frc::LiveWindow::GetInstance()->AddActuator( - "Pivot", "PIDSubsystem Controller", GetPIDController()); + AddChild("Upper Limit Switch", m_upperLimitSwitch); + AddChild("Lower Limit Switch", m_lowerLimitSwitch); + AddChild("Pot", m_pot); + AddChild("Motor", m_motor); } void InitDefaultCommand() {} diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pneumatics.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pneumatics.cpp index c84f636365..030db9df37 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pneumatics.cpp +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Pneumatics.cpp @@ -7,12 +7,11 @@ #include "Pneumatics.h" -#include +#include Pneumatics::Pneumatics() : frc::Subsystem("Pneumatics") { - frc::LiveWindow::GetInstance()->AddSensor( - "Pneumatics", "Pressure Sensor", m_pressureSensor); + AddChild("Pressure Sensor", m_pressureSensor); } /** diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Shooter.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Shooter.cpp index 432b2ed76b..f0dc0fcc5e 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Shooter.cpp +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/src/Subsystems/Shooter.cpp @@ -7,20 +7,13 @@ #include "Shooter.h" -#include - Shooter::Shooter() : Subsystem("Shooter") { // Put everything to the LiveWindow for testing. - frc::LiveWindow::GetInstance()->AddSensor( - "Shooter", "Hot Goal Sensor", &m_hotGoalSensor); - frc::LiveWindow::GetInstance()->AddSensor("Shooter", - "Piston1 Reed Switch Front ", - &m_piston1ReedSwitchFront); - frc::LiveWindow::GetInstance()->AddSensor("Shooter", - "Piston1 Reed Switch Back ", &m_piston1ReedSwitchBack); - frc::LiveWindow::GetInstance()->AddActuator( - "Shooter", "Latch Piston", &m_latchPiston); + AddChild("Hot Goal Sensor", m_hotGoalSensor); + AddChild("Piston1 Reed Switch Front ", m_piston1ReedSwitchFront); + AddChild("Piston1 Reed Switch Back ", m_piston1ReedSwitchBack); + AddChild("Latch Piston", m_latchPiston); } void Shooter::InitDefaultCommand() { diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/Robot.cpp index 172110fd71..a84209ec6e 100644 --- a/wpilibcExamples/src/main/cpp/templates/commandbased/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/templates/commandbased/Robot.cpp @@ -83,7 +83,7 @@ public: void TeleopPeriodic() override { frc::Scheduler::GetInstance()->Run(); } - void TestPeriodic() override { frc::LiveWindow::GetInstance()->Run(); } + void TestPeriodic() override {} private: // Have it null by default so that if testing teleop it diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/iterative/Robot.cpp index 688de7845a..1dba12af9a 100644 --- a/wpilibcExamples/src/main/cpp/templates/iterative/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/templates/iterative/Robot.cpp @@ -60,7 +60,7 @@ public: void TeleopPeriodic() {} - void TestPeriodic() { m_lw.Run(); } + void TestPeriodic() {} private: frc::LiveWindow& m_lw = *LiveWindow::GetInstance(); diff --git a/wpilibcExamples/src/main/cpp/templates/timed/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/timed/Robot.cpp index dec86deed8..de5c920475 100644 --- a/wpilibcExamples/src/main/cpp/templates/timed/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/templates/timed/Robot.cpp @@ -59,7 +59,7 @@ public: void TeleopPeriodic() {} - void TestPeriodic() { m_lw.Run(); } + void TestPeriodic() {} private: frc::LiveWindow& m_lw = *LiveWindow::GetInstance(); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java index 05b3003263..aaad4aca64 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java @@ -10,20 +10,18 @@ package edu.wpi.first.wpilibj; import java.nio.ByteBuffer; import java.nio.ByteOrder; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; import edu.wpi.first.wpilibj.interfaces.Accelerometer; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * ADXL345 I2C Accelerometer. */ @SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"}) -public class ADXL345_I2C extends SensorBase implements Accelerometer, LiveWindowSendable { +public class ADXL345_I2C extends SensorBase implements Accelerometer, Sendable { private static final byte kAddress = 0x1D; private static final byte kPowerCtlRegister = 0x2D; @@ -93,9 +91,14 @@ public class ADXL345_I2C extends SensorBase implements Accelerometer, LiveWindow setRange(range); HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_I2C); - LiveWindow.addSensor("ADXL345_I2C", port.value, this); + setName("ADXL345_I2C", port.value); } + @Override + public void free() { + super.free(); + m_i2c.free(); + } @Override public void setRange(Range range) { @@ -171,49 +174,16 @@ public class ADXL345_I2C extends SensorBase implements Accelerometer, LiveWindow } @Override - public String getSmartDashboardType() { - return "3AxisAccelerometer"; - } - - @SuppressWarnings("MemberName") - private NetworkTableEntry m_xEntry; - @SuppressWarnings("MemberName") - private NetworkTableEntry m_yEntry; - @SuppressWarnings("MemberName") - private NetworkTableEntry m_zEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_xEntry = subtable.getEntry("X"); - m_yEntry = subtable.getEntry("Y"); - m_zEntry = subtable.getEntry("Z"); - updateTable(); - } else { - m_xEntry = null; - m_yEntry = null; - m_zEntry = null; - } - } - - @Override - public void updateTable() { - if (m_xEntry != null) { - m_xEntry.setDouble(getX()); - } - if (m_yEntry != null) { - m_yEntry.setDouble(getY()); - } - if (m_zEntry != null) { - m_zEntry.setDouble(getZ()); - } - } - - @Override - public void startLiveWindowMode() { - } - - @Override - public void stopLiveWindowMode() { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("3AxisAccelerometer"); + NetworkTableEntry entryX = builder.getEntry("X"); + NetworkTableEntry entryY = builder.getEntry("Y"); + NetworkTableEntry entryZ = builder.getEntry("Z"); + builder.setUpdateTable(() -> { + AllAxes data = getAccelerations(); + entryX.setDouble(data.XAxis); + entryY.setDouble(data.YAxis); + entryZ.setDouble(data.ZAxis); + }); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java index 938db54350..fcc2136226 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java @@ -10,20 +10,18 @@ package edu.wpi.first.wpilibj; import java.nio.ByteBuffer; import java.nio.ByteOrder; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; import edu.wpi.first.wpilibj.interfaces.Accelerometer; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * ADXL345 SPI Accelerometer. */ @SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"}) -public class ADXL345_SPI extends SensorBase implements Accelerometer, LiveWindowSendable { +public class ADXL345_SPI extends SensorBase implements Accelerometer, Sendable { private static final int kPowerCtlRegister = 0x2D; private static final int kDataFormatRegister = 0x31; private static final int kDataRegister = 0x32; @@ -78,10 +76,12 @@ public class ADXL345_SPI extends SensorBase implements Accelerometer, LiveWindow public ADXL345_SPI(SPI.Port port, Range range) { m_spi = new SPI(port); init(range); - LiveWindow.addSensor("ADXL345_SPI", port.value, this); + setName("ADXL345_SPI", port.value); } + @Override public void free() { + super.free(); m_spi.free(); } @@ -189,49 +189,16 @@ public class ADXL345_SPI extends SensorBase implements Accelerometer, LiveWindow } @Override - public String getSmartDashboardType() { - return "3AxisAccelerometer"; - } - - @SuppressWarnings("MemberName") - private NetworkTableEntry m_xEntry; - @SuppressWarnings("MemberName") - private NetworkTableEntry m_yEntry; - @SuppressWarnings("MemberName") - private NetworkTableEntry m_zEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_xEntry = subtable.getEntry("X"); - m_yEntry = subtable.getEntry("Y"); - m_zEntry = subtable.getEntry("Z"); - updateTable(); - } else { - m_xEntry = null; - m_yEntry = null; - m_zEntry = null; - } - } - - @Override - public void updateTable() { - if (m_xEntry != null) { - m_xEntry.setDouble(getX()); - } - if (m_yEntry != null) { - m_yEntry.setDouble(getY()); - } - if (m_zEntry != null) { - m_zEntry.setDouble(getZ()); - } - } - - @Override - public void startLiveWindowMode() { - } - - @Override - public void stopLiveWindowMode() { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("3AxisAccelerometer"); + NetworkTableEntry entryX = builder.getEntry("X"); + NetworkTableEntry entryY = builder.getEntry("Y"); + NetworkTableEntry entryZ = builder.getEntry("Z"); + builder.setUpdateTable(() -> { + AllAxes data = getAccelerations(); + entryX.setDouble(data.XAxis); + entryY.setDouble(data.YAxis); + entryZ.setDouble(data.ZAxis); + }); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java index 8ed8c2e9df..875474b79a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java @@ -10,13 +10,11 @@ package edu.wpi.first.wpilibj; import java.nio.ByteBuffer; import java.nio.ByteOrder; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; import edu.wpi.first.wpilibj.interfaces.Accelerometer; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * ADXL362 SPI Accelerometer. @@ -24,7 +22,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; *

This class allows access to an Analog Devices ADXL362 3-axis accelerometer. */ @SuppressWarnings("PMD.UnusedPrivateField") -public class ADXL362 extends SensorBase implements Accelerometer, LiveWindowSendable { +public class ADXL362 extends SensorBase implements Accelerometer, Sendable { private static final byte kRegWrite = 0x0A; private static final byte kRegRead = 0x0B; @@ -82,6 +80,7 @@ public class ADXL362 extends SensorBase implements Accelerometer, LiveWindowSend */ public ADXL362(SPI.Port port, Range range) { m_spi = new SPI(port); + m_spi.setClockRate(3000000); m_spi.setMSBFirst(); m_spi.setSampleDataOnFalling(); @@ -109,17 +108,25 @@ public class ADXL362 extends SensorBase implements Accelerometer, LiveWindowSend m_spi.write(transferBuffer, 3); HAL.report(tResourceType.kResourceType_ADXL362, port.value); - LiveWindow.addSensor("ADXL362", port.value, this); + setName("ADXL362", port.value); } + @Override public void free() { - m_spi.free(); + super.free(); + if (m_spi != null) { + m_spi.free(); + m_spi = null; + } } @Override public void setRange(Range range) { - final byte value; + if (m_spi == null) { + return; + } + final byte value; switch (range) { case k2G: value = kFilterCtl_Range2G; @@ -205,49 +212,16 @@ public class ADXL362 extends SensorBase implements Accelerometer, LiveWindowSend } @Override - public String getSmartDashboardType() { - return "3AxisAccelerometer"; - } - - @SuppressWarnings("MemberName") - private NetworkTableEntry m_xEntry; - @SuppressWarnings("MemberName") - private NetworkTableEntry m_yEntry; - @SuppressWarnings("MemberName") - private NetworkTableEntry m_zEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_xEntry = subtable.getEntry("X"); - m_yEntry = subtable.getEntry("Y"); - m_zEntry = subtable.getEntry("Z"); - updateTable(); - } else { - m_xEntry = null; - m_yEntry = null; - m_zEntry = null; - } - } - - @Override - public void updateTable() { - if (m_xEntry != null) { - m_xEntry.setDouble(getX()); - } - if (m_yEntry != null) { - m_yEntry.setDouble(getY()); - } - if (m_zEntry != null) { - m_zEntry.setDouble(getZ()); - } - } - - @Override - public void startLiveWindowMode() { - } - - @Override - public void stopLiveWindowMode() { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("3AxisAccelerometer"); + NetworkTableEntry entryX = builder.getEntry("X"); + NetworkTableEntry entryY = builder.getEntry("Y"); + NetworkTableEntry entryZ = builder.getEntry("Z"); + builder.setUpdateTable(() -> { + AllAxes data = getAccelerations(); + entryX.setDouble(data.XAxis); + entryY.setDouble(data.YAxis); + entryZ.setDouble(data.ZAxis); + }); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java index f0255add1d..37e451555d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java @@ -13,8 +13,6 @@ import java.nio.ByteOrder; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; /** * Use a rate gyro to return the robots heading relative to a starting position. The Gyro class @@ -26,7 +24,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; *

This class is for the digital ADXRS450 gyro sensor that connects via SPI. */ @SuppressWarnings({"TypeName", "AbbreviationAsWordInName", "PMD.UnusedPrivateField"}) -public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, LiveWindowSendable { +public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable { private static final double kSamplePeriod = 0.001; private static final double kCalibrationSampleTime = 5.0; private static final double kDegreePerSecondPerLSB = 0.0125; @@ -57,6 +55,7 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, LiveWind */ public ADXRS450_Gyro(SPI.Port port) { m_spi = new SPI(port); + m_spi.setClockRate(3000000); m_spi.setMSBFirst(); m_spi.setSampleDataOnRising(); @@ -78,7 +77,7 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, LiveWind calibrate(); HAL.report(tResourceType.kResourceType_ADXRS450, port.value); - LiveWindow.addSensor("ADXRS450_Gyro", port.value, this); + setName("ADXRS450_Gyro", port.value); } @Override @@ -137,6 +136,7 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, LiveWind */ @Override public void free() { + super.free(); if (m_spi != null) { m_spi.free(); m_spi = null; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java index 6746172f2a..07a6f5d9cf 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java @@ -7,12 +7,9 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import static java.util.Objects.requireNonNull; @@ -21,7 +18,7 @@ import static java.util.Objects.requireNonNull; * through the sensor. Many sensors have multiple axis and can be treated as multiple devices. Each * is calibrated by finding the center value over a period of time. */ -public class AnalogAccelerometer extends SensorBase implements PIDSource, LiveWindowSendable { +public class AnalogAccelerometer extends SensorBase implements PIDSource, Sendable { private AnalogInput m_analogChannel; private double m_voltsPerG = 1.0; @@ -35,7 +32,7 @@ public class AnalogAccelerometer extends SensorBase implements PIDSource, LiveWi private void initAccelerometer() { HAL.report(tResourceType.kResourceType_Accelerometer, m_analogChannel.getChannel()); - LiveWindow.addSensor("Accelerometer", m_analogChannel.getChannel(), this); + setName("Accelerometer", m_analogChannel.getChannel()); } /** @@ -46,9 +43,9 @@ public class AnalogAccelerometer extends SensorBase implements PIDSource, LiveWi * @param channel The channel number for the analog input the accelerometer is connected to */ public AnalogAccelerometer(final int channel) { - m_analogChannel = new AnalogInput(channel); + this(new AnalogInput(channel)); m_allocatedChannel = true; - initAccelerometer(); + addChild(m_analogChannel); } /** @@ -72,6 +69,7 @@ public class AnalogAccelerometer extends SensorBase implements PIDSource, LiveWi */ @Override public void free() { + super.free(); if (m_analogChannel != null && m_allocatedChannel) { m_analogChannel.free(); } @@ -86,6 +84,9 @@ public class AnalogAccelerometer extends SensorBase implements PIDSource, LiveWi * @return The current acceleration of the sensor in Gs. */ public double getAcceleration() { + if (m_analogChannel == null) { + return 0.0; + } return (m_analogChannel.getAverageVoltage() - m_zeroGVoltage) / m_voltsPerG; } @@ -134,43 +135,8 @@ public class AnalogAccelerometer extends SensorBase implements PIDSource, LiveWi } @Override - public String getSmartDashboardType() { - return "Accelerometer"; - } - - /* - * Live Window code, only does anything if live window is activated. - */ - private NetworkTableEntry m_valueEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setDouble(getAcceleration()); - } - } - - /** - * Analog Channels don't have to do anything special when entering the LiveWindow. {@inheritDoc} - */ - @Override - public void startLiveWindowMode() { - } - - /** - * Analog Channels don't have to do anything special when exiting the LiveWindow. {@inheritDoc} - */ - @Override - public void stopLiveWindowMode() { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Accelerometer"); + builder.addDoubleProperty("Value", this::getAcceleration, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java index dc053c36fc..11b769778f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java @@ -11,8 +11,6 @@ import edu.wpi.first.wpilibj.hal.AnalogGyroJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; import static java.util.Objects.requireNonNull; @@ -25,7 +23,7 @@ import static java.util.Objects.requireNonNull; * *

This class is for gyro sensors that connect to an analog input. */ -public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowSendable { +public class AnalogGyro extends GyroBase implements Gyro, PIDSource, Sendable { private static final double kDefaultVoltsPerDegreePerSecond = 0.007; protected AnalogInput m_analog; @@ -44,7 +42,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS AnalogGyroJNI.setupAnalogGyro(m_gyroHandle); HAL.report(tResourceType.kResourceType_Gyro, m_analog.getChannel()); - LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this); + setName("AnalogGyro", m_analog.getChannel()); } @Override @@ -61,6 +59,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS public AnalogGyro(int channel) { this(new AnalogInput(channel)); m_channelAllocated = true; + addChild(m_analog); } /** @@ -90,6 +89,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS public AnalogGyro(int channel, int center, double offset) { this(new AnalogInput(channel), center, offset); m_channelAllocated = true; + addChild(m_analog); } /** @@ -121,6 +121,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS */ @Override public void free() { + super.free(); if (m_analog != null && m_channelAllocated) { m_analog.free(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java index b18c5167f7..668a278645 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java @@ -7,13 +7,10 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.AnalogJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import edu.wpi.first.wpilibj.util.AllocationException; /** @@ -28,7 +25,7 @@ import edu.wpi.first.wpilibj.util.AllocationException; * accumulated effectively increasing the resolution, while the averaged samples are divided by the * number of samples to retain the resolution, but get more stable values. */ -public class AnalogInput extends SensorBase implements PIDSource, LiveWindowSendable { +public class AnalogInput extends SensorBase implements PIDSource, Sendable { private static final int kAccumulatorSlot = 1; int m_port; // explicit no modifier, private and package accessible. @@ -49,14 +46,16 @@ public class AnalogInput extends SensorBase implements PIDSource, LiveWindowSend final int portHandle = AnalogJNI.getPort((byte) channel); m_port = AnalogJNI.initializeAnalogInputPort(portHandle); - LiveWindow.addSensor("AnalogInput", channel, this); HAL.report(tResourceType.kResourceType_AnalogChannel, channel); + setName("AnalogInput", channel); } /** * Channel destructor. */ + @Override public void free() { + super.free(); AnalogJNI.freeAnalogInputPort(m_port); m_port = 0; m_channel = 0; @@ -347,44 +346,9 @@ public class AnalogInput extends SensorBase implements PIDSource, LiveWindowSend return getAverageVoltage(); } - /** - * Live Window code, only does anything if live window is activated. - */ @Override - public String getSmartDashboardType() { - return "Analog Input"; - } - - private NetworkTableEntry m_valueEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setDouble(getAverageVoltage()); - } - } - - /** - * Analog Channels don't have to do anything special when entering the LiveWindow. {@inheritDoc} - */ - @Override - public void startLiveWindowMode() { - } - - /** - * Analog Channels don't have to do anything special when exiting the LiveWindow. {@inheritDoc} - */ - @Override - public void stopLiveWindowMode() { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Analog Input"); + builder.addDoubleProperty("Value", this::getAverageVoltage, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java index 990b9f9d10..0d0bf354cc 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java @@ -7,18 +7,15 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.AnalogJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Analog output class. */ -public class AnalogOutput extends SensorBase implements LiveWindowSendable { +public class AnalogOutput extends SendableBase implements Sendable { private int m_port; private int m_channel; @@ -28,20 +25,22 @@ public class AnalogOutput extends SensorBase implements LiveWindowSendable { * @param channel The channel number to represent. */ public AnalogOutput(final int channel) { - checkAnalogOutputChannel(channel); + SensorBase.checkAnalogOutputChannel(channel); m_channel = channel; final int portHandle = AnalogJNI.getPort((byte) channel); m_port = AnalogJNI.initializeAnalogOutputPort(portHandle); - LiveWindow.addSensor("AnalogOutput", channel, this); HAL.report(tResourceType.kResourceType_AnalogOutput, channel); + setName("AnalogOutput", channel); } /** * Channel destructor. */ + @Override public void free() { + super.free(); AnalogJNI.freeAnalogOutputPort(m_port); m_port = 0; m_channel = 0; @@ -62,44 +61,9 @@ public class AnalogOutput extends SensorBase implements LiveWindowSendable { return AnalogJNI.getAnalogOutput(m_port); } - /* - * Live Window code, only does anything if live window is activated. - */ @Override - public String getSmartDashboardType() { - return "Analog Output"; - } - - private NetworkTableEntry m_valueEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setDouble(getVoltage()); - } - } - - /** - * Analog Channels don't have to do anything special when entering the LiveWindow. {@inheritDoc} - */ - @Override - public void startLiveWindowMode() { - } - - /** - * Analog Channels don't have to do anything special when exiting the LiveWindow. {@inheritDoc} - */ - @Override - public void stopLiveWindowMode() { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Analog Output"); + builder.addDoubleProperty("Value", this::getVoltage, this::setVoltage); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java index 5ed5a6955b..161225d6d9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java @@ -7,17 +7,15 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.interfaces.Potentiometer; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Class for reading analog potentiometers. Analog potentiometers read in an analog voltage that * corresponds to a position. The position is in whichever units you choose, by way of the scaling * and offset constants passed to the constructor. */ -public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable { +public class AnalogPotentiometer extends SensorBase implements Potentiometer, Sendable { private AnalogInput m_analogInput; private boolean m_initAnalogInput; private double m_fullRange; @@ -40,6 +38,7 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable { public AnalogPotentiometer(final int channel, double fullRange, double offset) { this(new AnalogInput(channel), fullRange, offset); m_initAnalogInput = true; + addChild(m_analogInput); } /** @@ -56,6 +55,7 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable { * @param offset The offset to add to the scaled value for controlling the zero value */ public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) { + setName("AnalogPotentiometer", input.getChannel()); m_analogInput = input; m_initAnalogInput = false; @@ -118,6 +118,9 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable { */ @Override public double get() { + if (m_analogInput == null) { + return m_offset; + } return (m_analogInput.getVoltage() / ControllerPower.getVoltage5V()) * m_fullRange + m_offset; } @@ -144,56 +147,23 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable { return get(); } - - /** - * Live Window code, only does anything if live window is activated. - */ @Override - public String getSmartDashboardType() { - return "Analog Input"; - } - - private NetworkTableEntry m_valueEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setDouble(get()); + public void initSendable(SendableBuilder builder) { + if (m_analogInput != null) { + m_analogInput.initSendable(builder); } } /** * Frees this resource. */ + @Override public void free() { + super.free(); if (m_initAnalogInput) { m_analogInput.free(); m_analogInput = null; m_initAnalogInput = false; } } - - /** - * Analog Channels don't have to do anything special when entering the LiveWindow. {@inheritDoc} - */ - @Override - public void startLiveWindowMode() { - } - - /** - * Analog Channels don't have to do anything special when exiting the LiveWindow. {@inheritDoc} - */ - @Override - public void stopLiveWindowMode() { - } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java index 1e6d197dd6..0a7197ac3b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java @@ -14,12 +14,13 @@ import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType; import edu.wpi.first.wpilibj.hal.AnalogJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import edu.wpi.first.wpilibj.util.BoundaryException; /** * Class for creating and configuring Analog Triggers. */ -public class AnalogTrigger { +public class AnalogTrigger extends SensorBase implements Sendable { /** * Exceptions dealing with improper operation of the Analog trigger. @@ -53,6 +54,7 @@ public class AnalogTrigger { public AnalogTrigger(final int channel) { this(new AnalogInput(channel)); m_ownsAnalog = true; + addChild(m_analogInput); } /** @@ -71,12 +73,15 @@ public class AnalogTrigger { m_index = index.asIntBuffer().get(0); HAL.report(tResourceType.kResourceType_AnalogTrigger, channel.getChannel()); + setName("AnalogTrigger", channel.getChannel()); } /** * Release the resources used by this object. */ + @Override public void free() { + super.free(); AnalogJNI.cleanAnalogTrigger(m_port); m_port = 0; if (m_ownsAnalog && m_analogInput != null) { @@ -173,4 +178,11 @@ public class AnalogTrigger { public AnalogTriggerOutput createOutput(AnalogTriggerType type) { return new AnalogTriggerOutput(this, type); } + + @Override + public void initSendable(SendableBuilder builder) { + if (m_ownsAnalog) { + m_analogInput.initSendable(builder); + } + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java index 26b163372a..793c81aae8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java @@ -10,6 +10,7 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.AnalogJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import static java.util.Objects.requireNonNull; @@ -77,15 +78,6 @@ public class AnalogTriggerOutput extends DigitalSource { trigger.getIndex(), outputType.value); } - /** - * Frees the resources for this output. - */ - public void free() { - if (m_interrupt != 0) { - cancelInterrupts(); - } - } - /** * Get the state of the analog trigger output. * @@ -130,4 +122,8 @@ public class AnalogTriggerOutput extends DigitalSource { this.value = value; } } + + @Override + public void initSendable(SendableBuilder builder) { + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java index c3e71abd9f..7013e77cc8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java @@ -7,21 +7,18 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.AccelerometerJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; import edu.wpi.first.wpilibj.interfaces.Accelerometer; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Built-in accelerometer. * *

This class allows access to the roboRIO's internal accelerometer. */ -public class BuiltInAccelerometer implements Accelerometer, LiveWindowSendable { +public class BuiltInAccelerometer extends SensorBase implements Accelerometer, Sendable { /** * Constructor. * @@ -30,7 +27,7 @@ public class BuiltInAccelerometer implements Accelerometer, LiveWindowSendable { public BuiltInAccelerometer(Range range) { setRange(range); HAL.report(tResourceType.kResourceType_Accelerometer, 0, 0, "Built-in accelerometer"); - LiveWindow.addSensor("BuiltInAccel", 0, this); + setName("BuiltInAccel", 0); } /** @@ -94,49 +91,10 @@ public class BuiltInAccelerometer implements Accelerometer, LiveWindowSendable { } @Override - public String getSmartDashboardType() { - return "3AxisAccelerometer"; - } - - @SuppressWarnings("MemberName") - private NetworkTableEntry m_xEntry; - @SuppressWarnings("MemberName") - private NetworkTableEntry m_yEntry; - @SuppressWarnings("MemberName") - private NetworkTableEntry m_zEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_xEntry = subtable.getEntry("X"); - m_yEntry = subtable.getEntry("Y"); - m_zEntry = subtable.getEntry("Z"); - updateTable(); - } else { - m_xEntry = null; - m_yEntry = null; - m_zEntry = null; - } - } - - @Override - public void updateTable() { - if (m_xEntry != null) { - m_xEntry.setDouble(getX()); - } - if (m_yEntry != null) { - m_yEntry.setDouble(getY()); - } - if (m_zEntry != null) { - m_zEntry.setDouble(getZ()); - } - } - - @Override - public void startLiveWindowMode() { - } - - @Override - public void stopLiveWindowMode() { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("3AxisAccelerometer"); + builder.addDoubleProperty("X", this::getX, null); + builder.addDoubleProperty("Y", this::getY, null); + builder.addDoubleProperty("Z", this::getZ, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java index 79cf0ad8cd..2a259ab169 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java @@ -7,13 +7,10 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.EntryListenerFlags; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.CompressorJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Class for operating a compressor connected to a PCM (Pneumatic Control Module). The PCM will @@ -26,7 +23,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; * the safety provided by using the pressure switch and closed loop control. You can only turn off * closed loop control, thereby stopping the compressor from operating. */ -public class Compressor extends SensorBase implements LiveWindowSendable { +public class Compressor extends SendableBase implements Sendable { private int m_compressorHandle; private byte m_module; @@ -42,6 +39,7 @@ public class Compressor extends SensorBase implements LiveWindowSendable { m_compressorHandle = CompressorJNI.initializeCompressor((byte) module); HAL.report(tResourceType.kResourceType_Compressor, module); + setName("Compressor", module); } /** @@ -51,7 +49,7 @@ public class Compressor extends SensorBase implements LiveWindowSendable { * specifying the CAN ID.} */ public Compressor() { - this(getDefaultSolenoidModule()); + this(SensorBase.getDefaultSolenoidModule()); } /** @@ -193,54 +191,15 @@ public class Compressor extends SensorBase implements LiveWindowSendable { } @Override - public void startLiveWindowMode() { - if (m_enabledEntry != null) { - m_enabledListener = m_enabledEntry.addListener((event) -> { - if (event.value.getBoolean()) { - start(); - } else { - stop(); - } - }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - } - } - - @Override - public void stopLiveWindowMode() { - if (m_enabledEntry != null) { - m_enabledEntry.removeListener(m_enabledListener); - m_enabledListener = 0; - } - } - - @Override - public String getSmartDashboardType() { - return "Compressor"; - } - - private NetworkTableEntry m_enabledEntry; - private NetworkTableEntry m_pressureSwitchEntry; - private int m_enabledListener; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_enabledEntry = subtable.getEntry("Enabled"); - m_pressureSwitchEntry = subtable.getEntry("Pressure Switch"); - updateTable(); - } else { - m_enabledEntry = null; - m_pressureSwitchEntry = null; - } - } - - @Override - public void updateTable() { - if (m_enabledEntry != null) { - m_enabledEntry.setBoolean(enabled()); - } - if (m_pressureSwitchEntry != null) { - m_pressureSwitchEntry.setBoolean(getPressureSwitchValue()); - } + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Compressor"); + builder.addBooleanProperty("Enabled", this::enabled, (value) -> { + if (value) { + start(); + } else { + stop(); + } + }); + builder.addBooleanProperty("Pressure switch", this::getPressureSwitchValue, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java index bd51c8c7b8..d0fa9c2eb3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java @@ -10,13 +10,11 @@ package edu.wpi.first.wpilibj; import java.nio.ByteBuffer; import java.nio.ByteOrder; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType; import edu.wpi.first.wpilibj.hal.CounterJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import static java.util.Objects.requireNonNull; @@ -30,7 +28,7 @@ import static java.util.Objects.requireNonNull; *

All counters will immediately start counting - reset() them if you need them to be zeroed * before use. */ -public class Counter extends SensorBase implements CounterBase, LiveWindowSendable, PIDSource { +public class Counter extends SensorBase implements CounterBase, Sendable, PIDSource { /** * Mode determines how and what the counter counts. @@ -88,6 +86,7 @@ public class Counter extends SensorBase implements CounterBase, LiveWindowSendab setMaxPeriod(.5); HAL.report(tResourceType.kResourceType_Counter, m_index, mode.value); + setName("Counter", m_index); } /** @@ -183,6 +182,7 @@ public class Counter extends SensorBase implements CounterBase, LiveWindowSendab @Override public void free() { + super.free(); setUpdateWhenEmpty(true); clearUpSource(); @@ -213,6 +213,7 @@ public class Counter extends SensorBase implements CounterBase, LiveWindowSendab public void setUpSource(int channel) { setUpSource(new DigitalInput(channel)); m_allocatedUpSource = true; + addChild(m_upSource); } /** @@ -279,6 +280,7 @@ public class Counter extends SensorBase implements CounterBase, LiveWindowSendab public void setDownSource(int channel) { setDownSource(new DigitalInput(channel)); m_allocatedDownSource = true; + addChild(m_downSource); } /** @@ -553,34 +555,8 @@ public class Counter extends SensorBase implements CounterBase, LiveWindowSendab } @Override - public String getSmartDashboardType() { - return "Counter"; - } - - private NetworkTableEntry m_valueEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setDouble(get()); - } - } - - @Override - public void startLiveWindowMode() { - } - - @Override - public void stopLiveWindowMode() { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Counter"); + builder.addDoubleProperty("Value", this::get, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java index b24393970a..36b6271265 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java @@ -12,6 +12,7 @@ import java.util.concurrent.locks.ReentrantLock; import edu.wpi.first.wpilibj.hal.DigitalGlitchFilterJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import edu.wpi.first.wpilibj.hal.HAL; /** @@ -35,6 +36,7 @@ public class DigitalGlitchFilter extends SensorBase { m_filterAllocated[index] = true; HAL.report(tResourceType.kResourceType_DigitalFilter, m_channelIndex, 0); + setName("DigitalGlitchFilter", index); } } } @@ -43,6 +45,7 @@ public class DigitalGlitchFilter extends SensorBase { * Free the resources used by this object. */ public void free() { + super.free(); if (m_channelIndex >= 0) { synchronized (m_mutex) { m_filterAllocated[m_channelIndex] = false; @@ -170,6 +173,10 @@ public class DigitalGlitchFilter extends SensorBase { / (long) (kSystemClockTicksPerMicrosecond / 4); } + @SuppressWarnings("PMD.UnusedFormalParameter") + public void initSendable(SendableBuilder builder) { + } + private int m_channelIndex = -1; private static final Lock m_mutex = new ReentrantLock(true); private static final boolean[] m_filterAllocated = new boolean[3]; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java index a1163092ef..5f92794570 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java @@ -7,13 +7,10 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.DIOJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Class to read a digital input. This class will read digital inputs and return the current value @@ -21,7 +18,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; * elsewhere will automatically allocate digital inputs and outputs as required. This class is only * for devices like switches etc. that aren't implemented anywhere else. */ -public class DigitalInput extends DigitalSource implements LiveWindowSendable { +public class DigitalInput extends DigitalSource implements Sendable { private int m_channel = 0; private int m_handle = 0; @@ -36,14 +33,15 @@ public class DigitalInput extends DigitalSource implements LiveWindowSendable { m_handle = DIOJNI.initializeDIOPort(DIOJNI.getPort((byte) channel), true); - LiveWindow.addSensor("DigitalInput", channel, this); HAL.report(tResourceType.kResourceType_DigitalInput, channel); + setName("DigitalInput", channel); } /** * Frees the resources for this output. */ public void free() { + super.free(); if (m_interrupt != 0) { cancelInterrupts(); } @@ -102,37 +100,8 @@ public class DigitalInput extends DigitalSource implements LiveWindowSendable { } @Override - public String getSmartDashboardType() { - return "Digital Input"; - } - - private NetworkTableEntry m_valueEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setBoolean(get()); - } - } - - - @Override - public void startLiveWindowMode() { - } - - - @Override - public void stopLiveWindowMode() { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Digital Input"); + builder.addBooleanProperty("Value", this::get, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java index 00c2cb37f2..fdf88cdb2b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java @@ -7,19 +7,16 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.EntryListenerFlags; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.DIOJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Class to write digital outputs. This class will write digital outputs. Other devices that are * implemented elsewhere will automatically allocate digital inputs and outputs as required. */ -public class DigitalOutput extends DigitalSource implements LiveWindowSendable { +public class DigitalOutput extends SendableBase implements Sendable { private static final int invalidPwmGenerator = 0; private int m_pwmGenerator = invalidPwmGenerator; @@ -35,12 +32,13 @@ public class DigitalOutput extends DigitalSource implements LiveWindowSendable { * the MXP */ public DigitalOutput(int channel) { - checkDigitalChannel(channel); + SensorBase.checkDigitalChannel(channel); m_channel = channel; m_handle = DIOJNI.initializeDIOPort(DIOJNI.getPort((byte) channel), false); HAL.report(tResourceType.kResourceType_DigitalOutput, channel); + setName("DigitalOutput", channel); } /** @@ -48,6 +46,7 @@ public class DigitalOutput extends DigitalSource implements LiveWindowSendable { */ @Override public void free() { + super.free(); // disable the pwm only if we have allocated it if (m_pwmGenerator != invalidPwmGenerator) { disablePWM(); @@ -79,7 +78,6 @@ public class DigitalOutput extends DigitalSource implements LiveWindowSendable { * * @return The GPIO channel number. */ - @Override public int getChannel() { return m_channel; } @@ -146,7 +144,7 @@ public class DigitalOutput extends DigitalSource implements LiveWindowSendable { return; } // Disable the output by routing to a dead bit. - DIOJNI.setDigitalPWMOutputChannel(m_pwmGenerator, kDigitalChannels); + DIOJNI.setDigitalPWMOutputChannel(m_pwmGenerator, SensorBase.kDigitalChannels); DIOJNI.freeDigitalPWM(m_pwmGenerator); m_pwmGenerator = invalidPwmGenerator; } @@ -167,76 +165,9 @@ public class DigitalOutput extends DigitalSource implements LiveWindowSendable { DIOJNI.setDigitalPWMDutyCycle(m_pwmGenerator, dutyCycle); } - /** - * Get the analog trigger type. - * - * @return false - */ @Override - public int getAnalogTriggerTypeForRouting() { - return 0; - } - - /** - * Is this an analog trigger. - * - * @return true if this is an analog trigger - */ - @Override - public boolean isAnalogTrigger() { - return false; - } - - /** - * Get the HAL Port Handle. - * - * @return The HAL Handle to the specified source. - */ - @Override - public int getPortHandleForRouting() { - return m_handle; - } - - /* - * Live Window code, only does anything if live window is activated. - */ - @Override - public String getSmartDashboardType() { - return "Digital Output"; - } - - private NetworkTableEntry m_valueEntry; - private int m_valueListener; - - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - - @Override - public void updateTable() { - // TODO: Put current value. - } - - - @Override - public void startLiveWindowMode() { - m_valueListener = m_valueEntry.addListener((event) -> set(event.value.getBoolean()), - EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - - } - - - @Override - public void stopLiveWindowMode() { - m_valueEntry.removeListener(m_valueListener); - m_valueListener = 0; + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Digital Output"); + builder.addBooleanProperty("Value", this::get, this::set); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java index b1952e946a..0b68debd28 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java @@ -7,14 +7,10 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.EntryListenerFlags; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; import edu.wpi.first.wpilibj.hal.SolenoidJNI; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * DoubleSolenoid class for running 2 channels of high voltage Digital Output on the PCM. @@ -22,7 +18,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; *

The DoubleSolenoid class is typically used for pneumatics solenoids that have two positions * controlled by two separate channels. */ -public class DoubleSolenoid extends SolenoidBase implements LiveWindowSendable { +public class DoubleSolenoid extends SolenoidBase implements Sendable { /** * Possible values for a DoubleSolenoid. @@ -45,7 +41,7 @@ public class DoubleSolenoid extends SolenoidBase implements LiveWindowSendable { * @param reverseChannel The reverse channel number on the PCM (0..7). */ public DoubleSolenoid(final int forwardChannel, final int reverseChannel) { - this(getDefaultSolenoidModule(), forwardChannel, reverseChannel); + this(SensorBase.getDefaultSolenoidModule(), forwardChannel, reverseChannel); } /** @@ -59,9 +55,9 @@ public class DoubleSolenoid extends SolenoidBase implements LiveWindowSendable { final int reverseChannel) { super(moduleNumber); - checkSolenoidModule(m_moduleNumber); - checkSolenoidChannel(forwardChannel); - checkSolenoidChannel(reverseChannel); + SensorBase.checkSolenoidModule(m_moduleNumber); + SensorBase.checkSolenoidChannel(forwardChannel); + SensorBase.checkSolenoidChannel(reverseChannel); int portHandle = SolenoidJNI.getPortWithModule((byte) m_moduleNumber, (byte) forwardChannel); m_forwardHandle = SolenoidJNI.initializeSolenoidPort(portHandle); @@ -84,13 +80,15 @@ public class DoubleSolenoid extends SolenoidBase implements LiveWindowSendable { m_moduleNumber); HAL.report(tResourceType.kResourceType_Solenoid, reverseChannel, m_moduleNumber); - LiveWindow.addActuator("DoubleSolenoid", m_moduleNumber, forwardChannel, this); + setName("DoubleSolenoid", m_moduleNumber, forwardChannel); } /** * Destructor. */ + @Override public synchronized void free() { + super.free(); SolenoidJNI.freeSolenoidPort(m_forwardHandle); SolenoidJNI.freeSolenoidPort(m_reverseHandle); super.free(); @@ -169,52 +167,18 @@ public class DoubleSolenoid extends SolenoidBase implements LiveWindowSendable { return (blackList & m_reverseMask) != 0; } - /* - * Live Window code, only does anything if live window is activated. - */ - public String getSmartDashboardType() { - return "Double Solenoid"; - } - - private NetworkTableEntry m_valueEntry; - private int m_valueListener; - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setString(get().name().substring(1)); - } - } - - @Override - public void startLiveWindowMode() { - set(Value.kOff); // Stop for safety - m_valueListener = m_valueEntry.addListener((event) -> { - String value = event.value.getString(); - if ("Reverse".equals(value)) { - set(Value.kReverse); - } else if ("Forward".equals(value)) { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Double Solenoid"); + builder.setSafeState(() -> set(Value.kOff)); + builder.addStringProperty("Value", () -> get().name().substring(1), (value) -> { + if ("Forward".equals(value)) { set(Value.kForward); + } else if ("Reverse".equals(value)) { + set(Value.kReverse); } else { set(Value.kOff); } - }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - } - - @Override - public void stopLiveWindowMode() { - set(Value.kOff); // Stop for safety - m_valueEntry.removeListener(m_valueListener); - m_valueListener = 0; + }); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java index bb60a1d843..abd17727bb 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java @@ -7,13 +7,10 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.EncoderJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import edu.wpi.first.wpilibj.util.AllocationException; import static java.util.Objects.requireNonNull; @@ -31,7 +28,7 @@ import static java.util.Objects.requireNonNull; *

All encoders will immediately start counting - reset() them if you need them to be zeroed * before use. */ -public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveWindowSendable { +public class Encoder extends SensorBase implements CounterBase, PIDSource, Sendable { public enum IndexingType { kResetWhileHigh(0), kResetWhileLow(1), kResetOnFallingEdge(2), kResetOnRisingEdge(3); @@ -81,8 +78,9 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW m_pidSource = PIDSourceType.kDisplacement; - HAL.report(tResourceType.kResourceType_Encoder, getFPGAIndex(), type.value); - LiveWindow.addSensor("Encoder", m_aSource.getChannel(), this); + int fpgaIndex = getFPGAIndex(); + HAL.report(tResourceType.kResourceType_Encoder, fpgaIndex, type.value); + setName("Encoder", fpgaIndex); } /** @@ -96,12 +94,7 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW * if necessary so forward represents positive values. */ public Encoder(final int channelA, final int channelB, boolean reverseDirection) { - m_allocatedA = true; - m_allocatedB = true; - m_allocatedI = false; - m_aSource = new DigitalInput(channelA); - m_bSource = new DigitalInput(channelB); - initEncoder(reverseDirection, EncodingType.k4X); + this(channelA, channelB, reverseDirection, EncodingType.k4X); } /** @@ -141,6 +134,8 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW m_allocatedI = false; m_aSource = new DigitalInput(channelA); m_bSource = new DigitalInput(channelB); + addChild(m_aSource); + addChild(m_bSource); initEncoder(reverseDirection, encodingType); } @@ -158,13 +153,10 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW */ public Encoder(final int channelA, final int channelB, final int indexChannel, boolean reverseDirection) { - m_allocatedA = true; - m_allocatedB = true; + this(channelA, channelB, reverseDirection); m_allocatedI = true; - m_aSource = new DigitalInput(channelA); - m_bSource = new DigitalInput(channelB); m_indexSource = new DigitalInput(indexChannel); - initEncoder(reverseDirection, EncodingType.k4X); + addChild(m_indexSource); setIndexSource(m_indexSource); } @@ -195,15 +187,7 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW * if necessary so forward represents positive values. */ public Encoder(DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection) { - requireNonNull(sourceA, "Digital Source A was null"); - requireNonNull(sourceB, "Digital Source B was null"); - - m_allocatedA = false; - m_allocatedB = false; - m_allocatedI = false; - m_aSource = sourceA; - m_bSource = sourceB; - initEncoder(reverseDirection, EncodingType.k4X); + this(sourceA, sourceB, reverseDirection, EncodingType.k4X); } /** @@ -267,16 +251,9 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW */ public Encoder(DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource, boolean reverseDirection) { - requireNonNull(sourceA, "Digital Source A was null"); - requireNonNull(sourceB, "Digital Source B was null"); - - m_allocatedA = false; - m_allocatedB = false; + this(sourceA, sourceB, reverseDirection); m_allocatedI = false; - m_aSource = sourceA; - m_bSource = sourceB; m_indexSource = indexSource; - initEncoder(reverseDirection, EncodingType.k4X); setIndexSource(indexSource); } @@ -317,7 +294,9 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW /** * Free the resources used by this object. */ + @Override public void free() { + super.free(); if (m_aSource != null && m_allocatedA) { m_aSource.free(); m_allocatedA = false; @@ -456,6 +435,15 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW EncoderJNI.setEncoderDistancePerPulse(m_encoder, distancePerPulse); } + /** + * Get the distance per pulse for this encoder. + * + * @return The scale factor that will be used to convert pulses to useful units. + */ + public double getDistancePerPulse() { + return EncoderJNI.getEncoderDistancePerPulse(m_encoder); + } + /** * Set the direction sensing for this encoder. This sets the direction sensing on the encoder so * that it could count in the correct software direction regardless of the mounting. @@ -567,52 +555,16 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW source.getAnalogTriggerTypeForRouting(), type.value); } - /** - * Live Window code, only does anything if live window is activated. - */ - public String getSmartDashboardType() { + @Override + public void initSendable(SendableBuilder builder) { if (EncoderJNI.getEncoderEncodingType(m_encoder) == EncodingType.k4X.value) { - return "Quadrature Encoder"; - } - return "Encoder"; - } - - private NetworkTableEntry m_speedEntry; - private NetworkTableEntry m_distanceEntry; - private NetworkTableEntry m_distancePerTickEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_speedEntry = subtable.getEntry("Speed"); - m_distanceEntry = subtable.getEntry("Distance"); - m_distancePerTickEntry = subtable.getEntry("Distance per Tick"); - updateTable(); + builder.setSmartDashboardType("Quadrature Encoder"); } else { - m_speedEntry = null; - m_distanceEntry = null; - m_distancePerTickEntry = null; + builder.setSmartDashboardType("Encoder"); } - } - @Override - public void updateTable() { - if (m_speedEntry != null) { - m_speedEntry.setDouble(getRate()); - } - if (m_distanceEntry != null) { - m_distanceEntry.setDouble(getDistance()); - } - if (m_distancePerTickEntry != null) { - m_distancePerTickEntry.setDouble(EncoderJNI.getEncoderDistancePerPulse(m_encoder)); - } - } - - @Override - public void startLiveWindowMode() { - } - - @Override - public void stopLiveWindowMode() { + builder.addDoubleProperty("Speed", this::getRate, null); + builder.addDoubleProperty("Distance", this::getDistance, null); + builder.addDoubleProperty("Distance per Tick", this::getDistancePerPulse, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java index bc7dfb6c26..0b392f5612 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java @@ -9,7 +9,7 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Alias for counter class. Implement the gear tooth sensor supplied by FIRST. Currently there is no @@ -56,7 +56,7 @@ public class GearTooth extends Counter { } else { HAL.report(tResourceType.kResourceType_GearTooth, channel, 0); } - LiveWindow.addSensor("GearTooth", channel, this); + setName("GearTooth", channel); } /** @@ -70,6 +70,12 @@ public class GearTooth extends Counter { public GearTooth(DigitalSource source, boolean directionSensitive) { super(source); enableDirectionSensing(directionSensitive); + if (directionSensitive) { + HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel(), 0, "D"); + } else { + HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel(), 0); + } + setName("GearTooth", source.getChannel()); } /** @@ -84,10 +90,9 @@ public class GearTooth extends Counter { this(source, false); } - /* - * Live Window code, only does anything if live window is activated. - */ - public String getSmartDashboardType() { - return "Gear Tooth"; + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + builder.setSmartDashboardType("Gear Tooth"); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java index 7fbcc7542a..d38a65e347 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java @@ -7,15 +7,13 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * GyroBase is the common base class for Gyro implementations such as AnalogGyro. */ -public abstract class GyroBase extends SensorBase implements Gyro, PIDSource, LiveWindowSendable { +public abstract class GyroBase extends SensorBase implements Gyro, PIDSource, Sendable { private PIDSourceType m_pidSource = PIDSourceType.kDisplacement; /** @@ -52,38 +50,9 @@ public abstract class GyroBase extends SensorBase implements Gyro, PIDSource, Li } } - /* - * Live Window code, only does anything if live window is activated. - */ @Override - public String getSmartDashboardType() { - return "Gyro"; - } - - private NetworkTableEntry m_valueEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setDouble(getAngle()); - } - } - - @Override - public void startLiveWindowMode() { - } - - @Override - public void stopLiveWindowMode() { + 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/I2C.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java index 02c27f63f9..3fe4c65f98 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java @@ -22,7 +22,7 @@ import static java.util.Objects.requireNonNull; *

This class is intended to be used by sensor (and other I2C device) drivers. It probably should * not be used directly. */ -public class I2C extends SensorBase { +public class I2C { public enum Port { kOnboard(0), kMXP(1); @@ -56,6 +56,7 @@ public class I2C extends SensorBase { * Destructor. */ public void free() { + I2CJNI.i2CClose(m_port); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java index c5f045e42b..e2da074f51 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java @@ -45,6 +45,17 @@ public abstract class InterruptableSensorBase extends SensorBase { m_interrupt = 0; } + /** + * Frees the resources for this output. + */ + @Override + public void free() { + super.free(); + if (m_interrupt != 0) { + cancelInterrupts(); + } + } + /** * If this is an analog trigger. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java index 6847383dc6..4e08409f63 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java @@ -222,5 +222,6 @@ public abstract class IterativeRobotBase extends RobotBase { testPeriodic(); } robotPeriodic(); + LiveWindow.updateValues(); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java index b4e177bbbe..80f3b944b9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java @@ -9,7 +9,6 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * Texas Instruments / Vex Robotics Jaguar Speed Controller as a PWM device. @@ -39,6 +38,6 @@ public class Jaguar extends PWMSpeedController { setZeroLatch(); HAL.report(tResourceType.kResourceType_Jaguar, getChannel()); - LiveWindow.addActuator("Jaguar", getChannel(), this); + setName("Jaguar", getChannel()); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/NamedSendable.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NamedSendable.java deleted file mode 100644 index bc74dc1d38..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/NamedSendable.java +++ /dev/null @@ -1,23 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2016-2017 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj; - - -/** - * The interface for sendable objects that gives the sendable a default name in the Smart - * Dashboard. - */ -public interface NamedSendable extends Sendable { - - /** - * The name of the subtable. - * - * @return the name of the subtable of SmartDashboard that the Sendable object will use. - */ - String getName(); -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java index 405ca014b7..07bc135b6a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java @@ -7,18 +7,14 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.EntryListenerFlags; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Nidec Brushless Motor. */ -public class NidecBrushless implements SpeedController, MotorSafety, LiveWindowSendable { +public class NidecBrushless extends SendableBase implements SpeedController, MotorSafety, Sendable { private final MotorSafetyHelper m_safetyHelper; private boolean m_isInverted = false; private DigitalOutput m_dio; @@ -41,14 +37,26 @@ public class NidecBrushless implements SpeedController, MotorSafety, LiveWindowS // the dio controls the output (in PWM mode) m_dio = new DigitalOutput(dioChannel); + addChild(m_dio); m_dio.setPWMRate(15625); m_dio.enablePWM(0.5); // the pwm enables the controller m_pwm = new PWM(pwmChannel); + addChild(m_pwm); - LiveWindow.addActuator("Nidec Brushless", pwmChannel, this); HAL.report(tResourceType.kResourceType_NidecBrushless, pwmChannel); + setName("Nidec Brushless", pwmChannel); + } + + /** + * Free the resources used by this object. + */ + @Override + public void free() { + super.free(); + m_dio.free(); + m_pwm.free(); } /** @@ -188,45 +196,10 @@ public class NidecBrushless implements SpeedController, MotorSafety, LiveWindowS return m_pwm.getChannel(); } - /* - * Live Window code, only does anything if live window is activated. - */ @Override - public String getSmartDashboardType() { - return "Nidec Brushless"; - } - - private NetworkTableEntry m_valueEntry; - private int m_valueListener; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setDouble(get()); - } - } - - @Override - public void startLiveWindowMode() { - set(0); // Stop for safety - m_valueListener = m_valueEntry.addListener((event) -> set(event.value.getDouble()), - EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - } - - @Override - public void stopLiveWindowMode() { - set(0); // Stop for safety - m_valueEntry.removeListener(m_valueListener); - m_valueListener = 0; + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Nidec Brushless"); + builder.setSafeState(this::stopMotor); + builder.addDoubleProperty("Value", this::get, this::set); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java index f300106cc4..bfdc118b55 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java @@ -10,11 +10,8 @@ package edu.wpi.first.wpilibj; import java.util.TimerTask; import java.util.concurrent.locks.ReentrantLock; -import edu.wpi.first.networktables.EntryListenerFlags; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.filters.LinearDigitalFilter; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import edu.wpi.first.wpilibj.util.BoundaryException; import static java.util.Objects.requireNonNull; @@ -29,7 +26,7 @@ import static java.util.Objects.requireNonNull; * and derivative calculations. Therefore, the sample rate affects the controller's behavior for a * given set of PID constants. */ -public class PIDController implements PIDInterface, LiveWindowSendable, Controller { +public class PIDController extends SendableBase implements PIDInterface, Sendable, Controller { public static final double kDefaultPeriod = .05; private static int instances = 0; @@ -153,6 +150,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll @SuppressWarnings("ParameterName") public PIDController(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output, double period) { + super(false); requireNonNull(source, "Null PIDSource was given"); requireNonNull(output, "Null PIDOutput was given"); @@ -180,6 +178,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll instances++; HLUsageReporting.reportPIDController(instances); m_tolerance = new NullTolerance(); + setName("PIDController", instances); } /** @@ -232,7 +231,9 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll /** * Free the PID object. */ + @Override public void free() { + super.free(); m_controlLoop.cancel(); m_thisMutex.lock(); try { @@ -242,7 +243,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll } finally { m_thisMutex.unlock(); } - removeListeners(); } /** @@ -395,16 +395,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll } finally { m_thisMutex.unlock(); } - - if (m_pEntry != null) { - m_pEntry.setDouble(p); - } - if (m_iEntry != null) { - m_iEntry.setDouble(i); - } - if (m_dEntry != null) { - m_dEntry.setDouble(d); - } } /** @@ -427,18 +417,65 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll } finally { m_thisMutex.unlock(); } + } - if (m_pEntry != null) { - m_pEntry.setDouble(p); + /** + * Set the Proportional coefficient of the PID controller gain. + * + * @param p Proportional coefficient + */ + @SuppressWarnings("ParameterName") + public void setP(double p) { + m_thisMutex.lock(); + try { + m_P = p; + } finally { + m_thisMutex.unlock(); } - if (m_iEntry != null) { - m_iEntry.setDouble(i); + } + + /** + * Set the Integral coefficient of the PID controller gain. + * + * @param i Integral coefficient + */ + @SuppressWarnings("ParameterName") + public void setI(double i) { + m_thisMutex.lock(); + try { + m_I = i; + } finally { + m_thisMutex.unlock(); } - if (m_dEntry != null) { - m_dEntry.setDouble(d); + } + + /** + * Set the Differential coefficient of the PID controller gain. + * + * @param d differential coefficient + */ + @SuppressWarnings("ParameterName") + public void setD(double d) { + m_thisMutex.lock(); + try { + m_D = d; + } finally { + m_thisMutex.unlock(); } - if (m_fEntry != null) { - m_fEntry.setDouble(f); + } + + /** + * Set the Feed forward coefficient of the PID controller gain. + * + * @param f feed forward coefficient + */ + @SuppressWarnings("ParameterName") + public void setF(double f) { + m_thisMutex.lock(); + try { + m_F = f; + } finally { + m_thisMutex.unlock(); } } @@ -601,10 +638,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll } finally { m_thisMutex.unlock(); } - - if (m_setpointEntry != null) { - m_setpointEntry.setDouble(m_setpoint); - } } /** @@ -776,10 +809,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll } finally { m_thisMutex.unlock(); } - - if (m_enabledEntry != null) { - m_enabledEntry.setBoolean(true); - } } /** @@ -801,9 +830,16 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll } finally { m_pidWriteMutex.unlock(); } + } - if (m_enabledEntry != null) { - m_enabledEntry.setBoolean(false); + /** + * Set the enabled state of the PIDController. + */ + public void setEnabled(boolean enable) { + if (enable) { + enable(); + } else { + disable(); } } @@ -824,7 +860,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll * Reset the previous error,, the integral term, and disable the controller. */ @Override - public synchronized void reset() { + public void reset() { disable(); m_thisMutex.lock(); @@ -838,130 +874,15 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll } @Override - public String getSmartDashboardType() { - return "PIDController"; - } - - @SuppressWarnings("MemberName") - private NetworkTableEntry m_pEntry; - @SuppressWarnings("MemberName") - private NetworkTableEntry m_iEntry; - @SuppressWarnings("MemberName") - private NetworkTableEntry m_dEntry; - @SuppressWarnings("MemberName") - private NetworkTableEntry m_fEntry; - private NetworkTableEntry m_setpointEntry; - private NetworkTableEntry m_enabledEntry; - @SuppressWarnings("MemberName") - private int m_pListener; - @SuppressWarnings("MemberName") - private int m_iListener; - @SuppressWarnings("MemberName") - private int m_dListener; - @SuppressWarnings("MemberName") - private int m_fListener; - private int m_setpointListener; - private int m_enabledListener; - - private void removeListeners() { - if (m_pEntry != null) { - m_pEntry.removeListener(m_pListener); - } - if (m_iEntry != null) { - m_iEntry.removeListener(m_iListener); - } - if (m_dEntry != null) { - m_dEntry.removeListener(m_dListener); - } - if (m_fEntry != null) { - m_fEntry.removeListener(m_fListener); - } - if (m_setpointEntry != null) { - m_setpointEntry.removeListener(m_setpointListener); - } - if (m_enabledEntry != null) { - m_enabledEntry.removeListener(m_enabledListener); - } - } - - @Override - public void initTable(NetworkTable table) { - removeListeners(); - if (table != null) { - m_pEntry = table.getEntry("p"); - m_pEntry.setDouble(getP()); - m_iEntry = table.getEntry("i"); - m_iEntry.setDouble(getI()); - m_dEntry = table.getEntry("d"); - m_dEntry.setDouble(getD()); - m_fEntry = table.getEntry("f"); - m_fEntry.setDouble(getF()); - m_setpointEntry = table.getEntry("setpoint"); - m_setpointEntry.setDouble(getSetpoint()); - m_enabledEntry = table.getEntry("enabled"); - m_enabledEntry.setBoolean(isEnabled()); - - m_pListener = m_pEntry.addListener((entry) -> { - m_thisMutex.lock(); - try { - m_P = entry.value.getDouble(); - } finally { - m_thisMutex.unlock(); - } - }, EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - - m_iListener = m_iEntry.addListener((entry) -> { - m_thisMutex.lock(); - try { - m_I = entry.value.getDouble(); - } finally { - m_thisMutex.unlock(); - } - }, EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - - m_dListener = m_dEntry.addListener((entry) -> { - m_thisMutex.lock(); - try { - m_D = entry.value.getDouble(); - } finally { - m_thisMutex.unlock(); - } - }, EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - - m_fListener = m_fEntry.addListener((entry) -> { - m_thisMutex.lock(); - try { - m_F = entry.value.getDouble(); - } finally { - m_thisMutex.unlock(); - } - }, EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - - m_setpointListener = m_setpointEntry.addListener((entry) -> { - double val = entry.value.getDouble(); - if (getSetpoint() != val) { - setSetpoint(val); - } - }, EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - - m_enabledListener = m_enabledEntry.addListener((entry) -> { - boolean val = entry.value.getBoolean(); - if (isEnabled() != val) { - if (val) { - enable(); - } else { - disable(); - } - } - }, EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - } else { - m_pEntry = null; - m_iEntry = null; - m_dEntry = null; - m_fEntry = null; - m_setpointEntry = null; - m_enabledEntry = null; - } + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("PIDController"); + builder.setSafeState(this::reset); + builder.addDoubleProperty("p", this::getP, this::setP); + builder.addDoubleProperty("i", this::getI, this::setI); + builder.addDoubleProperty("d", this::getD, this::setD); + builder.addDoubleProperty("f", this::getF, this::setF); + builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint); + builder.addBooleanProperty("enabled", this::isEnabled, this::setEnabled); } /** @@ -986,21 +907,6 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll return error; } - @Override - public void updateTable() { - } - - - @Override - public void startLiveWindowMode() { - disable(); - } - - - @Override - public void stopLiveWindowMode() { - } - private static double clamp(double value, double low, double high) { return Math.max(low, Math.min(value, high)); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java index b8e88debc4..ad466a195d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java @@ -7,14 +7,11 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.EntryListenerFlags; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.DIOJNI; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; import edu.wpi.first.wpilibj.hal.PWMJNI; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Class implements the PWM generation in the FPGA. @@ -28,7 +25,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; * center value - 999 to 2 = linear scaling from "center" to "full reverse" - 1 = minimum pulse * width (currently .5ms) - 0 = disabled (i.e. PWM output is held low) */ -public class PWM extends SensorBase implements LiveWindowSendable { +public class PWM extends SendableBase implements Sendable { /** * Represents the amount to multiply the minimum servo-pulse pwm period by. */ @@ -56,7 +53,7 @@ public class PWM extends SensorBase implements LiveWindowSendable { * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port */ public PWM(final int channel) { - checkPWMChannel(channel); + SensorBase.checkPWMChannel(channel); m_channel = channel; m_handle = PWMJNI.initializePWMPort(DIOJNI.getPort((byte) channel)); @@ -66,6 +63,7 @@ public class PWM extends SensorBase implements LiveWindowSendable { PWMJNI.setPWMEliminateDeadband(m_handle, false); HAL.report(tResourceType.kResourceType_PWM, channel); + setName("PWM", channel); } /** @@ -73,7 +71,9 @@ public class PWM extends SensorBase implements LiveWindowSendable { * *

Free the resource associated with the PWM channel and set the value to 0. */ + @Override public void free() { + super.free(); if (m_handle == 0) { return; } @@ -242,45 +242,10 @@ public class PWM extends SensorBase implements LiveWindowSendable { PWMJNI.latchPWMZero(m_handle); } - /* - * Live Window code, only does anything if live window is activated. - */ @Override - public String getSmartDashboardType() { - return "Speed Controller"; - } - - private NetworkTableEntry m_valueEntry; - private int m_valueListener; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setDouble(getSpeed()); - } - } - - @Override - public void startLiveWindowMode() { - setSpeed(0); // Stop for safety - m_valueListener = m_valueEntry.addListener((event) -> setSpeed(event.value.getDouble()), - EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - } - - @Override - public void stopLiveWindowMode() { - setSpeed(0); // Stop for safety - m_valueEntry.removeListener(m_valueListener); - m_valueListener = 0; + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Speed Controller"); + builder.setSafeState(this::setDisabled); + builder.addDoubleProperty("Value", this::getSpeed, this::setSpeed); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java index 4e8239701c..bb315e06df 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java @@ -9,7 +9,6 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM control. @@ -41,7 +40,7 @@ public class PWMTalonSRX extends PWMSpeedController { setSpeed(0.0); setZeroLatch(); - LiveWindow.addActuator("PWMTalonSRX", getChannel(), this); HAL.report(tResourceType.kResourceType_PWMTalonSRX, getChannel()); + setName("PWMTalonSRX", getChannel()); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java index 203e9aa556..1e57a2e616 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java @@ -7,16 +7,14 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.PDPJNI; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Class for getting voltage, current, temperature, power and energy from the Power Distribution * Panel over CAN. */ -public class PowerDistributionPanel extends SensorBase implements LiveWindowSendable { +public class PowerDistributionPanel extends SensorBase implements Sendable { private final int m_module; @@ -29,6 +27,7 @@ public class PowerDistributionPanel extends SensorBase implements LiveWindowSend m_module = module; checkPDPModule(module); PDPJNI.initializePDP(module); + setName("PowerDistributionPanel", module); } /** @@ -111,61 +110,13 @@ public class PowerDistributionPanel extends SensorBase implements LiveWindowSend } @Override - public String getSmartDashboardType() { - return "PowerDistributionPanel"; - } - - /* - * Live Window code, only does anything if live window is activated. - */ - private NetworkTableEntry[] m_chanEntry; - private NetworkTableEntry m_voltageEntry; - private NetworkTableEntry m_totalCurrentEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_chanEntry = new NetworkTableEntry[16]; - for (int i = 0; i < m_chanEntry.length; i++) { - m_chanEntry[i] = subtable.getEntry("Chan" + i); - } - m_voltageEntry = subtable.getEntry("Voltage"); - m_totalCurrentEntry = subtable.getEntry("TotalCurrent"); - updateTable(); - } else { - m_chanEntry = null; - m_voltageEntry = null; - m_totalCurrentEntry = null; + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("PowerDistributionPanel"); + for (int i = 0; i < kPDPChannels; ++i) { + final int chan = i; + builder.addDoubleProperty("Chan" + i, () -> getCurrent(chan), null); } + builder.addDoubleProperty("Voltage", this::getVoltage, null); + builder.addDoubleProperty("TotalCurrent", this::getTotalCurrent, null); } - - @Override - public void updateTable() { - if (m_chanEntry != null) { - for (int i = 0; i < m_chanEntry.length; i++) { - m_chanEntry[i].setDouble(getCurrent(i)); - } - } - if (m_voltageEntry != null) { - m_voltageEntry.setDouble(getVoltage()); - } - if (m_totalCurrentEntry != null) { - m_totalCurrentEntry.setDouble(getTotalCurrent()); - } - } - - /** - * PDP doesn't have to do anything special when entering the LiveWindow. {@inheritDoc} - */ - @Override - public void startLiveWindowMode() { - } - - /** - * PDP doesn't have to do anything special when exiting the LiveWindow. {@inheritDoc} - */ - @Override - public void stopLiveWindowMode() { - } - } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java index 9bb1720c6e..56a6c446d6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java @@ -7,14 +7,10 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.EntryListenerFlags; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; import edu.wpi.first.wpilibj.hal.RelayJNI; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import java.util.Arrays; import java.util.Optional; @@ -30,7 +26,7 @@ import static java.util.Objects.requireNonNull; * channels (forward and reverse) to be used independently for something that does not care about * voltage polarity (like a solenoid). */ -public class Relay extends SensorBase implements MotorSafety, LiveWindowSendable { +public class Relay extends SendableBase implements MotorSafety, Sendable { private MotorSafetyHelper m_safetyHelper; /** @@ -120,7 +116,7 @@ public class Relay extends SensorBase implements MotorSafety, LiveWindowSendable m_safetyHelper = new MotorSafetyHelper(this); m_safetyHelper.setSafetyEnabled(false); - LiveWindow.addActuator("Relay", m_channel, this); + setName("Relay", m_channel); } /** @@ -147,6 +143,11 @@ public class Relay extends SensorBase implements MotorSafety, LiveWindowSendable @Override public void free() { + super.free(); + freeRelay(); + } + + private void freeRelay() { try { RelayJNI.setRelay(m_forwardHandle, false); } catch (RuntimeException ex) { @@ -324,49 +325,16 @@ public class Relay extends SensorBase implements MotorSafety, LiveWindowSendable return; } - free(); + freeRelay(); m_direction = direction; initRelay(); } - /* - * Live Window code, only does anything if live window is activated. - */ @Override - public String getSmartDashboardType() { - return "Relay"; - } - - private NetworkTableEntry m_valueEntry; - private int m_valueListener; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setString(get().getPrettyValue()); - } - } - - @Override - public void startLiveWindowMode() { - m_valueListener = m_valueEntry.addListener( - (event) -> set(Value.getValueOf(event.value.getString()).orElse(Value.kOff)), - EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - } - - @Override - public void stopLiveWindowMode() { - m_valueEntry.removeListener(m_valueListener); - m_valueListener = 0; + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Relay"); + builder.setSafeState(() -> set(Value.kOff)); + builder.addStringProperty("Value", () -> get().getPrettyValue(), + (value) -> set(Value.getValueOf(value).orElse(Value.kOff))); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java index a183911d2c..6c4b3cbb4b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java @@ -9,7 +9,6 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * Mindsensors SD540 Speed Controller. @@ -35,8 +34,8 @@ public class SD540 extends PWMSpeedController { setSpeed(0.0); setZeroLatch(); - LiveWindow.addActuator("SD540", getChannel(), this); HAL.report(tResourceType.kResourceType_MindsensorsSD540, getChannel()); + setName("SD540", getChannel()); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java index 85eb8a82ea..58f3eb992c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java @@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.hal.SPIJNI; /** * Represents a SPI bus port. */ -public class SPI extends SensorBase { +public class SPI { public enum Port { kOnboardCS0(0), kOnboardCS1(1), kOnboardCS2(2), kOnboardCS3(3), kMXP(4); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java index 005c0aac7a..c93df452c8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java @@ -7,7 +7,7 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** @@ -15,17 +15,48 @@ import edu.wpi.first.networktables.NetworkTable; */ public interface Sendable { /** - * Initializes a table for this {@link Sendable} object. + * Gets the name of this {@link Sendable} object. * - * @param subtable The table to put the values in. + * @return Name */ - void initTable(NetworkTable subtable); + String getName(); /** - * The string representation of the named data type that will be used by the smart dashboard for - * this {@link Sendable}. + * Sets the name of this {@link Sendable} object. * - * @return The type of this {@link Sendable}. + * @param name name */ - String getSmartDashboardType(); + void setName(String name); + + /** + * Sets both the subsystem name and device name of this {@link Sendable} object. + * + * @param subsystem subsystem name + * @param name device name + */ + default void setName(String subsystem, String name) { + setSubsystem(subsystem); + setName(name); + } + + /** + * Gets the subsystem name of this {@link Sendable} object. + * + * @return Subsystem name + */ + String getSubsystem(); + + /** + * Sets the subsystem name of this {@link Sendable} object. + * + * @param subsystem subsystem name + */ + void setSubsystem(String subsystem); + + /** + * Initializes this {@link Sendable} object. + * + * @param builder sendable builder + */ + void initSendable(SendableBuilder builder); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java new file mode 100644 index 0000000000..2d727a1f66 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java @@ -0,0 +1,94 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.wpilibj.livewindow.LiveWindow; + +/** + * Base class for all sensors. Stores most recent status information as well as containing utility + * functions for checking channels and error processing. + */ +public abstract class SendableBase implements Sendable { + private String m_name = ""; + private String m_subsystem = "Ungrouped"; + + /** + * Creates an instance of the sensor base. + */ + public SendableBase() { + this(true); + } + + /** + * Creates an instance of the sensor base. + * + * @param addLiveWindow if true, add this Sendable to LiveWindow + */ + public SendableBase(boolean addLiveWindow) { + if (addLiveWindow) { + LiveWindow.add(this); + } + } + + /** + * Free the resources used by this object. + */ + public void free() { + LiveWindow.remove(this); + } + + @Override + public final synchronized String getName() { + return m_name; + } + + @Override + public final synchronized void setName(String name) { + m_name = name; + } + + /** + * Sets the name of the sensor with a channel number. + * + * @param moduleType A string that defines the module name in the label for the value + * @param channel The channel number the device is plugged into + */ + protected final void setName(String moduleType, int channel) { + setName(moduleType + "[" + channel + "]"); + } + + /** + * Sets the name of the sensor with a module and channel number. + * + * @param moduleType A string that defines the module name in the label for the value + * @param moduleNumber The number of the particular module type + * @param channel The channel number the device is plugged into (usually PWM) + */ + protected final void setName(String moduleType, int moduleNumber, int channel) { + setName(moduleType + "[" + moduleNumber + "," + channel + "]"); + } + + @Override + public final synchronized String getSubsystem() { + return m_subsystem; + } + + @Override + public final synchronized void setSubsystem(String subsystem) { + m_subsystem = subsystem; + } + + /** + * Add a child component. + * + * @param child child component + */ + protected final void addChild(Object child) { + LiveWindow.addChild(this, child); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorBase.java index 5a505b9402..86185732cd 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorBase.java @@ -20,7 +20,7 @@ import edu.wpi.first.wpilibj.hal.SolenoidJNI; * Base class for all sensors. Stores most recent status information as well as containing utility * functions for checking channels and error processing. */ -public abstract class SensorBase { +public abstract class SensorBase extends SendableBase { /** * Ticks per microsecond. */ @@ -65,12 +65,6 @@ public abstract class SensorBase { private static int m_defaultSolenoidModule = 0; - /** - * Creates an instance of the sensor base and gets an FPGA handle. - */ - public SensorBase() { - } - /** * Set the default location for the Solenoid module. * @@ -86,7 +80,7 @@ public abstract class SensorBase { * * @param moduleNumber The solenoid module module number to check. */ - protected static void checkSolenoidModule(final int moduleNumber) { + public static void checkSolenoidModule(final int moduleNumber) { if (!SolenoidJNI.checkSolenoidModule(moduleNumber)) { StringBuilder buf = new StringBuilder(); buf.append("Requested solenoid module is out of range. Minimum: 0, Maximum: ") @@ -103,7 +97,7 @@ public abstract class SensorBase { * * @param channel The channel number to check. */ - protected static void checkDigitalChannel(final int channel) { + public static void checkDigitalChannel(final int channel) { if (!DIOJNI.checkDIOChannel(channel)) { StringBuilder buf = new StringBuilder(); buf.append("Requested DIO channel is out of range. Minimum: 0, Maximum: ") @@ -120,7 +114,7 @@ public abstract class SensorBase { * * @param channel The channel number to check. */ - protected static void checkRelayChannel(final int channel) { + public static void checkRelayChannel(final int channel) { if (!RelayJNI.checkRelayChannel(channel)) { StringBuilder buf = new StringBuilder(); buf.append("Requested relay channel is out of range. Minimum: 0, Maximum: ") @@ -137,7 +131,7 @@ public abstract class SensorBase { * * @param channel The channel number to check. */ - protected static void checkPWMChannel(final int channel) { + public static void checkPWMChannel(final int channel) { if (!PWMJNI.checkPWMChannel(channel)) { StringBuilder buf = new StringBuilder(); buf.append("Requested PWM channel is out of range. Minimum: 0, Maximum: ") @@ -154,7 +148,7 @@ public abstract class SensorBase { * * @param channel The channel number to check. */ - protected static void checkAnalogInputChannel(final int channel) { + public static void checkAnalogInputChannel(final int channel) { if (!AnalogJNI.checkAnalogInputChannel(channel)) { StringBuilder buf = new StringBuilder(); buf.append("Requested analog input channel is out of range. Minimum: 0, Maximum: ") @@ -171,7 +165,7 @@ public abstract class SensorBase { * * @param channel The channel number to check. */ - protected static void checkAnalogOutputChannel(final int channel) { + public static void checkAnalogOutputChannel(final int channel) { if (!AnalogJNI.checkAnalogOutputChannel(channel)) { StringBuilder buf = new StringBuilder(); buf.append("Requested analog output channel is out of range. Minimum: 0, Maximum: ") @@ -187,7 +181,7 @@ public abstract class SensorBase { * * @param channel The channel number to check. */ - protected static void checkSolenoidChannel(final int channel) { + public static void checkSolenoidChannel(final int channel) { if (!SolenoidJNI.checkSolenoidChannel(channel)) { StringBuilder buf = new StringBuilder(); buf.append("Requested solenoid channel is out of range. Minimum: 0, Maximum: ") @@ -204,7 +198,7 @@ public abstract class SensorBase { * * @param channel The channel number to check. */ - protected static void checkPDPChannel(final int channel) { + public static void checkPDPChannel(final int channel) { if (!PDPJNI.checkPDPChannel(channel)) { StringBuilder buf = new StringBuilder(); buf.append("Requested PDP channel is out of range. Minimum: 0, Maximum: ") @@ -220,7 +214,7 @@ public abstract class SensorBase { * * @param module The module number to check. */ - protected static void checkPDPModule(final int module) { + public static void checkPDPModule(final int module) { if (!PDPJNI.checkPDPModule(module)) { StringBuilder buf = new StringBuilder(); buf.append("Requested PDP module is out of range. Minimum: 0, Maximum: ") @@ -239,10 +233,4 @@ public abstract class SensorBase { public static int getDefaultSolenoidModule() { return SensorBase.m_defaultSolenoidModule; } - - /** - * Free the resources used by this object. - */ - public void free() { - } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java index 3c7640da7f..5c2517787c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java @@ -7,12 +7,9 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.EntryListenerFlags; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Standard hobby style servo. @@ -42,8 +39,8 @@ public class Servo extends PWM { setBounds(kDefaultMaxServoPWM, 0, 0, 0, kDefaultMinServoPWM); setPeriodMultiplier(PeriodMultiplier.k4X); - LiveWindow.addActuator("Servo", getChannel(), this); HAL.report(tResourceType.kResourceType_Servo, getChannel()); + setName("Servo", getChannel()); } @@ -108,44 +105,9 @@ public class Servo extends PWM { return kMaxServoAngle - kMinServoAngle; } - /* - * Live Window code, only does anything if live window is activated. - */ - public String getSmartDashboardType() { - return "Servo"; - } - - private NetworkTable m_table; - private NetworkTableEntry m_valueEntry; - private int m_valueListener; - @Override - public void initTable(NetworkTable subtable) { - m_table = subtable; - if (m_table != null) { - m_valueEntry = m_table.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setDouble(get()); - } - } - - @Override - public void startLiveWindowMode() { - m_valueListener = m_valueEntry.addListener((event) -> set(event.value.getDouble()), - EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - } - - @Override - public void stopLiveWindowMode() { - m_valueEntry.removeListener(m_valueListener); - m_valueListener = 0; + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Servo"); + builder.addDoubleProperty("Value", this::get, this::set); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java index 8b77f03c87..c63c888bca 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java @@ -7,14 +7,10 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.EntryListenerFlags; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; import edu.wpi.first.wpilibj.hal.SolenoidJNI; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * Solenoid class for running high voltage Digital Output on the PCM. @@ -22,7 +18,7 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; *

The Solenoid class is typically used for pneumatic solenoids, but could be used for any * device within the current spec of the PCM. */ -public class Solenoid extends SolenoidBase implements LiveWindowSendable { +public class Solenoid extends SolenoidBase implements Sendable { private final int m_channel; // The channel to control. private int m_solenoidHandle; @@ -33,7 +29,7 @@ public class Solenoid extends SolenoidBase implements LiveWindowSendable { * @param channel The channel on the PCM to control (0..7). */ public Solenoid(final int channel) { - this(getDefaultSolenoidModule(), channel); + this(SensorBase.getDefaultSolenoidModule(), channel); } /** @@ -46,26 +42,24 @@ public class Solenoid extends SolenoidBase implements LiveWindowSendable { super(moduleNumber); m_channel = channel; - checkSolenoidModule(m_moduleNumber); - checkSolenoidChannel(m_channel); + SensorBase.checkSolenoidModule(m_moduleNumber); + SensorBase.checkSolenoidChannel(m_channel); int portHandle = SolenoidJNI.getPortWithModule((byte) m_moduleNumber, (byte) m_channel); m_solenoidHandle = SolenoidJNI.initializeSolenoidPort(portHandle); - LiveWindow.addActuator("Solenoid", m_moduleNumber, m_channel, this); HAL.report(tResourceType.kResourceType_Solenoid, m_channel, m_moduleNumber); + setName("Solenoid", m_moduleNumber, m_channel); } /** * Destructor. */ + @Override public synchronized void free() { + super.free(); SolenoidJNI.freeSolenoidPort(m_solenoidHandle); m_solenoidHandle = 0; - if (m_valueEntry != null) { - m_valueEntry.removeListener(m_valueListener); - } - super.free(); } /** @@ -122,44 +116,10 @@ public class Solenoid extends SolenoidBase implements LiveWindowSendable { SolenoidJNI.fireOneShot(m_solenoidHandle); } - /* - * Live Window code, only does anything if live window is activated. - */ - public String getSmartDashboardType() { - return "Solenoid"; - } - - private NetworkTableEntry m_valueEntry; - private int m_valueListener; - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setBoolean(get()); - } - } - - @Override - public void startLiveWindowMode() { - set(false); // Stop for safety - m_valueListener = m_valueEntry.addListener((event) -> set(event.value.getBoolean()), - EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - } - - @Override - public void stopLiveWindowMode() { - set(false); // Stop for safety - m_valueEntry.removeListener(m_valueListener); - m_valueListener = 0; + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Solenoid"); + builder.setSafeState(() -> set(false)); + builder.addBooleanProperty("Value", this::get, this::set); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java index 68c2c7e437..e81820acc4 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java @@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj.hal.SolenoidJNI; * SolenoidBase class is the common base class for the {@link Solenoid} and {@link DoubleSolenoid} * classes. */ -public abstract class SolenoidBase extends SensorBase { +public abstract class SolenoidBase extends SendableBase { protected final int m_moduleNumber; // The number of the solenoid module being used. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java index 978cb25c42..e6274e35a8 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java @@ -9,7 +9,6 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * REV Robotics SPARK Speed Controller. @@ -35,8 +34,8 @@ public class Spark extends PWMSpeedController { setSpeed(0.0); setZeroLatch(); - LiveWindow.addActuator("Spark", getChannel(), this); HAL.report(tResourceType.kResourceType_RevSPARK, getChannel()); + setName("Spark", getChannel()); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java index 559022e0cb..53c4cca9ec 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java @@ -7,13 +7,16 @@ package edu.wpi.first.wpilibj; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; + /** * Allows multiple {@link SpeedController} objects to be linked together. */ -public class SpeedControllerGroup implements SpeedController { +public class SpeedControllerGroup extends SendableBase implements SpeedController { private boolean m_isInverted = false; private final SpeedController[] m_speedControllers; + private static int instances = 0; /** * Create a new SpeedControllerGroup with the provided SpeedControllers. @@ -24,9 +27,13 @@ public class SpeedControllerGroup implements SpeedController { SpeedController... speedControllers) { m_speedControllers = new SpeedController[speedControllers.length + 1]; m_speedControllers[0] = speedController; + addChild(speedController); for (int i = 0; i < speedControllers.length; i++) { m_speedControllers[i + 1] = speedControllers[i]; + addChild(speedControllers[i]); } + instances++; + setName("SpeedControllerGroup", instances); } @Override @@ -74,4 +81,11 @@ public class SpeedControllerGroup implements SpeedController { speedController.pidWrite(output); } } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Speed Controller"); + builder.setSafeState(this::stopMotor); + builder.addDoubleProperty("Value", this::get, this::set); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java index a484a78372..6cb46d8cac 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java @@ -9,7 +9,6 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * Cross the Road Electronics (CTRE) Talon and Talon SR Speed Controller. @@ -40,7 +39,7 @@ public class Talon extends PWMSpeedController { setSpeed(0.0); setZeroLatch(); - LiveWindow.addActuator("Talon", getChannel(), this); HAL.report(tResourceType.kResourceType_Talon, getChannel()); + setName("Talon", getChannel()); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java index 50615a68fb..f5b028a5db 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java @@ -7,12 +7,9 @@ package edu.wpi.first.wpilibj; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; import static java.util.Objects.requireNonNull; @@ -25,7 +22,7 @@ import static java.util.Objects.requireNonNull; * echo is received. The time that the line is high determines the round trip distance (time of * flight). */ -public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSendable { +public class Ultrasonic extends SensorBase implements PIDSource, Sendable { /** * The units to return when PIDGet is called. @@ -107,6 +104,7 @@ public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSenda m_firstSensor = this; m_counter = new Counter(m_echoChannel); // set up counter for this + addChild(m_counter); // sensor m_counter.setMaxPeriod(1.0); m_counter.setSemiPeriodMode(true); @@ -116,7 +114,7 @@ public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSenda m_instances++; HAL.report(tResourceType.kResourceType_Ultrasonic, m_instances); - LiveWindow.addSensor("Ultrasonic", m_echoChannel.getChannel(), this); + setName("Ultrasonic", m_echoChannel.getChannel()); } /** @@ -133,6 +131,8 @@ public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSenda public Ultrasonic(final int pingChannel, final int echoChannel, Unit units) { m_pingChannel = new DigitalOutput(pingChannel); m_echoChannel = new DigitalInput(echoChannel); + addChild(m_pingChannel); + addChild(m_echoChannel); m_allocatedChannels = true; m_units = units; initialize(); @@ -194,6 +194,7 @@ public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSenda */ @Override public synchronized void free() { + super.free(); final boolean wasAutomaticMode = m_automaticEnabled; setAutomaticMode(false); if (m_allocatedChannels) { @@ -391,38 +392,9 @@ public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSenda m_enabled = enable; } - /** - * Live Window code, only does anything if live window is activated. - */ @Override - public String getSmartDashboardType() { - return "Ultrasonic"; - } - - private NetworkTableEntry m_valueEntry; - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_valueEntry = subtable.getEntry("Value"); - updateTable(); - } else { - m_valueEntry = null; - } - } - - @Override - public void updateTable() { - if (m_valueEntry != null) { - m_valueEntry.setDouble(getRangeInches()); - } - } - - @Override - public void startLiveWindowMode() { - } - - @Override - public void stopLiveWindowMode() { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Ultrasonic"); + builder.addDoubleProperty("Value", this::getRangeInches, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java index ca6c587465..0c6d0de727 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java @@ -9,7 +9,6 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * VEX Robotics Victor 888 Speed Controller The Vex Robotics Victor 884 Speed Controller can also @@ -42,7 +41,7 @@ public class Victor extends PWMSpeedController { setSpeed(0.0); setZeroLatch(); - LiveWindow.addActuator("Victor", getChannel(), this); HAL.report(tResourceType.kResourceType_Victor, getChannel()); + setName("Victor", getChannel()); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java index 474e8d1b23..2ae3bfc305 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java @@ -9,7 +9,6 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * VEX Robotics Victor SP Speed Controller. @@ -40,7 +39,7 @@ public class VictorSP extends PWMSpeedController { setSpeed(0.0); setZeroLatch(); - LiveWindow.addActuator("VictorSP", getChannel(), this); HAL.report(tResourceType.kResourceType_VictorSP, getChannel()); + setName("VictorSP", getChannel()); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java index 674176225a..6f6f5b7193 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java @@ -7,11 +7,10 @@ package edu.wpi.first.wpilibj.buttons; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.SendableBase; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * This class provides an easy way to link commands to inputs. @@ -24,7 +23,9 @@ import edu.wpi.first.wpilibj.command.Scheduler; * certain sensor input). For this, they only have to write the {@link Trigger#get()} method to get * the full functionality of the Trigger class. */ -public abstract class Trigger implements Sendable { +public abstract class Trigger extends SendableBase { + + private volatile boolean m_sendablePressed = false; /** * Returns whether or not the trigger is active. @@ -42,8 +43,7 @@ public abstract class Trigger implements Sendable { */ @SuppressWarnings("PMD.UselessParentheses") private boolean grab() { - return get() || (m_pressedEntry != null && m_pressedEntry.getBoolean(false)); - + return get() || m_sendablePressed; } /** @@ -186,24 +186,14 @@ public abstract class Trigger implements Sendable { } } - /** - * These methods continue to return the "Button" SmartDashboard type until we decided to create a - * Trigger widget type for the dashboard. - */ @Override - public String getSmartDashboardType() { - return "Button"; - } - - private NetworkTableEntry m_pressedEntry; - - @Override - public void initTable(NetworkTable table) { - if (table != null) { - m_pressedEntry = table.getEntry("pressed"); - m_pressedEntry.setBoolean(get()); - } else { - m_pressedEntry = null; - } + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Button"); + builder.setSafeState(() -> { + m_sendablePressed = false; + }); + builder.addBooleanProperty("pressed", this::grab, (value) -> { + m_sendablePressed = value; + }); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java index 6b543cb4bc..1900fc36bc 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java @@ -9,12 +9,11 @@ package edu.wpi.first.wpilibj.command; import java.util.Enumeration; -import edu.wpi.first.networktables.EntryListenerFlags; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.wpilibj.NamedSendable; import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.SendableBase; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * The Command class is at the very core of the entire command framework. Every command can be @@ -41,12 +40,7 @@ import edu.wpi.first.wpilibj.Timer; * @see CommandGroup * @see IllegalUseOfCommandException */ -public abstract class Command implements NamedSendable { - /** - * The name of this command. - */ - private String m_name; - +public abstract class Command extends SendableBase implements Sendable { /** * The time since this command was initialized. */ @@ -101,8 +95,9 @@ public abstract class Command implements NamedSendable { * Creates a new command. The name of this command will be set to its class name. */ public Command() { - m_name = getClass().getName(); - m_name = m_name.substring(m_name.lastIndexOf('.') + 1); + super(false); + String name = getClass().getName(); + setName(name.substring(name.lastIndexOf('.') + 1)); } /** @@ -112,10 +107,11 @@ public abstract class Command implements NamedSendable { * @throws IllegalArgumentException if name is null */ public Command(String name) { + super(false); if (name == null) { throw new IllegalArgumentException("Name must not be null."); } - m_name = name; + setName(name); } /** @@ -150,16 +146,6 @@ public abstract class Command implements NamedSendable { m_timeout = timeout; } - /** - * Returns the name of this command. If no name was specified in the constructor, then the default - * is the name of the class. - * - * @return the name of this command - */ - public String getName() { - return m_name; - } - /** * Sets the timeout of this command. * @@ -222,9 +208,6 @@ public abstract class Command implements NamedSendable { m_initialized = false; m_canceled = false; m_running = false; - if (m_runningEntry != null) { - m_runningEntry.setBoolean(false); - } } /** @@ -387,9 +370,15 @@ public abstract class Command implements NamedSendable { } lockChanges(); m_parent = parent; - if (m_isParentedEntry != null) { - m_isParentedEntry.setBoolean(true); - } + } + + /** + * Returns whether the command has a parent. + * + * @param True if the command has a parent. + */ + synchronized boolean isParented() { + return m_parent != null; } /** @@ -429,9 +418,6 @@ public abstract class Command implements NamedSendable { synchronized void startRunning() { m_running = true; m_startTime = -1; - if (m_runningEntry != null) { - m_runningEntry.setBoolean(true); - } } /** @@ -550,36 +536,21 @@ public abstract class Command implements NamedSendable { return getName(); } - - public String getSmartDashboardType() { - return "Command"; - } - - private NetworkTableEntry m_runningEntry; - private NetworkTableEntry m_isParentedEntry; - private int m_runningListener; - @Override - public void initTable(NetworkTable table) { - if (m_runningEntry != null) { - m_runningEntry.removeListener(m_runningListener); - } - if (table != null) { - m_runningEntry = table.getEntry("running"); - m_isParentedEntry = table.getEntry(".isParented"); - table.getEntry(".name").setString(getName()); - m_runningEntry.setBoolean(isRunning()); - m_isParentedEntry.setBoolean(m_parent != null); - m_runningListener = m_runningEntry.addListener((event) -> { - if (event.value.getBoolean()) { + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Command"); + builder.addStringProperty(".name", this::getName, null); + builder.addBooleanProperty("running", this::isRunning, (value) -> { + if (value) { + if (!isRunning()) { start(); - } else { + } + } else { + if (isRunning()) { cancel(); } - }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); - } else { - m_runningEntry = null; - m_isParentedEntry = null; - } + } + }); + builder.addBooleanProperty(".isParented", this::isParented, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java index ddca1f11ff..9a987aa3dc 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java @@ -7,12 +7,12 @@ package edu.wpi.first.wpilibj.command; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDOutput; import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.PIDSourceType; import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * This class defines a {@link Command} which interacts heavily with a PID loop. @@ -209,12 +209,10 @@ public abstract class PIDCommand extends Command implements Sendable { */ protected abstract void usePIDOutput(double output); - public String getSmartDashboardType() { - return "PIDCommand"; - } - - public void initTable(NetworkTable table) { - m_controller.initTable(table); - super.initTable(table); + @Override + public void initSendable(SendableBuilder builder) { + m_controller.initSendable(builder); + super.initSendable(builder); + builder.setSmartDashboardType("PIDCommand"); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java index 8f540aeaa7..55487b3106 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java @@ -7,7 +7,6 @@ package edu.wpi.first.wpilibj.command; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDOutput; import edu.wpi.first.wpilibj.PIDSource; @@ -62,6 +61,7 @@ public abstract class PIDSubsystem extends Subsystem implements Sendable { public PIDSubsystem(String name, double p, double i, double d) { super(name); m_controller = new PIDController(p, i, d, m_source, m_output); + addChild("PIDController", m_controller); } /** @@ -77,6 +77,7 @@ public abstract class PIDSubsystem extends Subsystem implements Sendable { public PIDSubsystem(String name, double p, double i, double d, double f) { super(name); m_controller = new PIDController(p, i, d, f, m_source, m_output); + addChild("PIDController", m_controller); } /** @@ -93,6 +94,7 @@ public abstract class PIDSubsystem extends Subsystem implements Sendable { public PIDSubsystem(String name, double p, double i, double d, double f, double period) { super(name); m_controller = new PIDController(p, i, d, f, m_source, m_output, period); + addChild("PIDController", m_controller); } /** @@ -106,6 +108,7 @@ public abstract class PIDSubsystem extends Subsystem implements Sendable { @SuppressWarnings("ParameterName") public PIDSubsystem(double p, double i, double d) { m_controller = new PIDController(p, i, d, m_source, m_output); + addChild("PIDController", m_controller); } /** @@ -122,6 +125,7 @@ public abstract class PIDSubsystem extends Subsystem implements Sendable { @SuppressWarnings("ParameterName") public PIDSubsystem(double p, double i, double d, double period, double f) { m_controller = new PIDController(p, i, d, f, m_source, m_output, period); + addChild("PIDController", m_controller); } /** @@ -137,6 +141,7 @@ public abstract class PIDSubsystem extends Subsystem implements Sendable { @SuppressWarnings("ParameterName") public PIDSubsystem(double p, double i, double d, double period) { m_controller = new PIDController(p, i, d, m_source, m_output, period); + addChild("PIDController", m_controller); } /** @@ -277,15 +282,4 @@ public abstract class PIDSubsystem extends Subsystem implements Sendable { public void disable() { m_controller.disable(); } - - @Override - public String getSmartDashboardType() { - return "PIDSubsystem"; - } - - @Override - public void initTable(NetworkTable table) { - m_controller.initTable(table); - super.initTable(table); - } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java index ec06f93ccf..2c9b3a0fc9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java @@ -11,11 +11,12 @@ import java.util.Enumeration; import java.util.Hashtable; import java.util.Vector; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.HLUsageReporting; -import edu.wpi.first.wpilibj.NamedSendable; +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.SendableBase; import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * The {@link Scheduler} is a singleton which holds the top-level running commands. It is in charge @@ -29,7 +30,7 @@ import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler; * * @see Command */ -public class Scheduler implements NamedSendable { +public class Scheduler extends SendableBase implements Sendable { /** * The Singleton Instance. @@ -91,6 +92,7 @@ public class Scheduler implements NamedSendable { */ private Scheduler() { HLUsageReporting.reportScheduler(); + setName("Scheduler"); } /** @@ -237,8 +239,6 @@ public class Scheduler implements NamedSendable { } lock.confirmCommand(); } - - updateTable(); } /** @@ -307,67 +307,44 @@ public class Scheduler implements NamedSendable { } @Override - public String getName() { - return "Scheduler"; - } - - public String getType() { - return "Scheduler"; - } - - @Override - public void initTable(NetworkTable subtable) { - if (subtable != null) { - m_namesEntry = subtable.getEntry("Names"); - m_idsEntry = subtable.getEntry("Ids"); - m_cancelEntry = subtable.getEntry("Cancel"); - m_namesEntry.setStringArray(new String[0]); - m_idsEntry.setDoubleArray(new double[0]); - m_cancelEntry.setDoubleArray(new double[0]); - } else { - m_namesEntry = null; - m_idsEntry = null; - m_cancelEntry = null; - } - } - - private void updateTable() { - if (m_namesEntry != null && m_idsEntry != null && m_cancelEntry != null) { - // Get the commands to cancel - double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]); - if (toCancel.length > 0) { - for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) { - for (double d : toCancel) { - if (e.getData().hashCode() == d) { - e.getData().cancel(); + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Scheduler"); + m_namesEntry = builder.getEntry("Names"); + m_idsEntry = builder.getEntry("Ids"); + m_cancelEntry = builder.getEntry("Cancel"); + builder.setUpdateTable(() -> { + if (m_namesEntry != null && m_idsEntry != null && m_cancelEntry != null) { + // Get the commands to cancel + double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]); + if (toCancel.length > 0) { + for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) { + for (double d : toCancel) { + if (e.getData().hashCode() == d) { + e.getData().cancel(); + } } } + m_cancelEntry.setDoubleArray(new double[0]); } - m_cancelEntry.setDoubleArray(new double[0]); - } - if (m_runningCommandsChanged) { - // Set the the running commands - int number = 0; - for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) { - number++; + if (m_runningCommandsChanged) { + // Set the the running commands + int number = 0; + for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) { + number++; + } + String[] commands = new String[number]; + double[] ids = new double[number]; + number = 0; + for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) { + commands[number] = e.getData().getName(); + ids[number] = e.getData().hashCode(); + number++; + } + m_namesEntry.setStringArray(commands); + m_idsEntry.setDoubleArray(ids); } - String[] commands = new String[number]; - double[] ids = new double[number]; - number = 0; - for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) { - commands[number] = e.getData().getName(); - ids[number] = e.getData().hashCode(); - number++; - } - m_namesEntry.setStringArray(commands); - m_idsEntry.setDoubleArray(ids); } - } - } - - @Override - public String getSmartDashboardType() { - return "Scheduler"; + }); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java index 8b04ff4e95..73785254ac 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java @@ -9,9 +9,10 @@ package edu.wpi.first.wpilibj.command; import java.util.Enumeration; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.wpilibj.NamedSendable; +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.SendableBase; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * This class defines a major component of the robot. @@ -27,7 +28,7 @@ import edu.wpi.first.wpilibj.NamedSendable; * * @see Command */ -public abstract class Subsystem implements NamedSendable { +public abstract class Subsystem extends SendableBase implements Sendable { /** * Whether or not getDefaultCommand() was called. @@ -43,10 +44,6 @@ public abstract class Subsystem implements NamedSendable { * The default command. */ private Command m_defaultCommand; - /** - * The name. - */ - private String m_name; /** * Creates a subsystem with the given name. @@ -54,7 +51,7 @@ public abstract class Subsystem implements NamedSendable { * @param name the name of the subsystem */ public Subsystem(String name) { - m_name = name; + setName(name, name); Scheduler.getInstance().registerSubsystem(this); } @@ -62,7 +59,9 @@ public abstract class Subsystem implements NamedSendable { * Creates a subsystem. This will set the name to the name of the class. */ public Subsystem() { - m_name = getClass().getName().substring(getClass().getName().lastIndexOf('.') + 1); + String name = getClass().getName(); + name = name.substring(name.lastIndexOf('.') + 1); + setName(name, name); Scheduler.getInstance().registerSubsystem(this); m_currentCommandChanged = true; } @@ -110,14 +109,6 @@ public abstract class Subsystem implements NamedSendable { } m_defaultCommand = command; } - if (m_hasDefaultEntry != null && m_defaultEntry != null) { - if (m_defaultCommand != null) { - m_hasDefaultEntry.setBoolean(true); - m_defaultEntry.setString(m_defaultCommand.getName()); - } else { - m_hasDefaultEntry.setBoolean(false); - } - } } /** @@ -125,7 +116,7 @@ public abstract class Subsystem implements NamedSendable { * * @return the default command */ - protected Command getDefaultCommand() { + public Command getDefaultCommand() { if (!m_initializedDefaultCommand) { m_initializedDefaultCommand = true; initDefaultCommand(); @@ -133,6 +124,20 @@ public abstract class Subsystem implements NamedSendable { return m_defaultCommand; } + /** + * Returns the default command name, or empty string is there is none. + * + * @return the default command name + */ + public String getDefaultCommandName() { + Command defaultCommand = getDefaultCommand(); + if (defaultCommand != null) { + return defaultCommand.getName(); + } else { + return ""; + } + } + /** * Sets the current command. * @@ -150,14 +155,6 @@ public abstract class Subsystem implements NamedSendable { */ void confirmCommand() { if (m_currentCommandChanged) { - if (m_hasCommandEntry != null && m_commandEntry != null) { - if (m_currentCommand != null) { - m_hasCommandEntry.setBoolean(true); - m_commandEntry.setString(m_currentCommand.getName()); - } else { - m_hasCommandEntry.setBoolean(false); - } - } m_currentCommandChanged = false; } } @@ -171,51 +168,54 @@ public abstract class Subsystem implements NamedSendable { return m_currentCommand; } - @Override - public String toString() { - return getName(); + /** + * Returns the current command name, or empty string if no current command. + * + * @return the current command name + */ + public String getCurrentCommandName() { + Command currentCommand = getCurrentCommand(); + if (currentCommand != null) { + return currentCommand.getName(); + } else { + return ""; + } } /** - * Returns the name of this subsystem, which is by default the class name. + * Associate a {@link Sendable} with this Subsystem. + * Also update the child's name. * - * @return the name of this subsystem + * @param name name to give child + * @param child sendable */ - @Override - public String getName() { - return m_name; + public void addChild(String name, Sendable child) { + child.setName(getSubsystem(), name); + LiveWindow.add(child); + } + + /** + * Associate a {@link Sendable} with this Subsystem. + * + * @param child sendable + */ + public void addChild(Sendable child) { + child.setSubsystem(getSubsystem()); + LiveWindow.add(child); } @Override - public String getSmartDashboardType() { - return "Subsystem"; + public String toString() { + return getSubsystem(); } - private NetworkTableEntry m_hasDefaultEntry; - private NetworkTableEntry m_defaultEntry; - private NetworkTableEntry m_hasCommandEntry; - private NetworkTableEntry m_commandEntry; - @Override - public void initTable(NetworkTable table) { - if (table != null) { - m_hasDefaultEntry = table.getEntry("hasDefault"); - m_defaultEntry = table.getEntry("default"); - m_hasCommandEntry = table.getEntry("hasCommand"); - m_commandEntry = table.getEntry("command"); + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Subsystem"); - if (m_defaultCommand != null) { - m_hasDefaultEntry.setBoolean(true); - m_defaultEntry.setString(m_defaultCommand.getName()); - } else { - m_hasDefaultEntry.setBoolean(false); - } - if (m_currentCommand != null) { - m_hasCommandEntry.setBoolean(true); - m_commandEntry.setString(m_currentCommand.getName()); - } else { - m_hasCommandEntry.setBoolean(false); - } - } + builder.addBooleanProperty("hasDefault", () -> m_defaultCommand != null, null); + builder.addStringProperty("default", this::getDefaultCommandName, null); + builder.addBooleanProperty("hasCommand", () -> m_currentCommand != null, null); + builder.addStringProperty("command", this::getCurrentCommandName, null); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java index 0ab58598be..bb6a7e3e0e 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive @@ -93,6 +94,8 @@ public class DifferentialDrive extends RobotDriveBase { public static final double kDefaultQuickStopThreshold = 0.2; public static final double kDefaultQuickStopAlpha = 0.1; + private static int instances = 0; + private SpeedController m_leftMotor; private SpeedController m_rightMotor; @@ -110,6 +113,10 @@ public class DifferentialDrive extends RobotDriveBase { public DifferentialDrive(SpeedController leftMotor, SpeedController rightMotor) { m_leftMotor = leftMotor; m_rightMotor = rightMotor; + addChild(m_leftMotor); + addChild(m_rightMotor); + instances++; + setName("DifferentialDrive", instances); } /** @@ -349,4 +356,11 @@ public class DifferentialDrive extends RobotDriveBase { public String getDescription() { return "DifferentialDrive"; } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("DifferentialDrive"); + builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set); + builder.addDoubleProperty("Right Motor Speed", m_rightMotor::get, m_rightMotor::set); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java index 5fec77db19..627fdc99d3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java @@ -8,6 +8,7 @@ package edu.wpi.first.wpilibj.drive; import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; // import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances; // import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; // import edu.wpi.first.wpilibj.hal.HAL; @@ -41,6 +42,8 @@ public class KilloughDrive extends RobotDriveBase { public static final double kDefaultRightMotorAngle = 120.0; public static final double kDefaultBackMotorAngle = 270.0; + private static int instances = 0; + private SpeedController m_leftMotor; private SpeedController m_rightMotor; private SpeedController m_backMotor; @@ -93,6 +96,11 @@ public class KilloughDrive extends RobotDriveBase { Math.sin(rightMotorAngle * (Math.PI / 180.0))); m_backVec = new Vector2d(Math.cos(backMotorAngle * (Math.PI / 180.0)), Math.sin(backMotorAngle * (Math.PI / 180.0))); + addChild(m_leftMotor); + addChild(m_rightMotor); + addChild(m_backMotor); + instances++; + setName("KilloughDrive", instances); } /** @@ -192,4 +200,12 @@ public class KilloughDrive extends RobotDriveBase { public String getDescription() { return "KilloughDrive"; } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("KilloughDrive"); + builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set); + builder.addDoubleProperty("Right Motor Speed", m_rightMotor::get, m_rightMotor::set); + builder.addDoubleProperty("Back Motor Speed", m_backMotor::get, m_backMotor::set); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java index 592738bc8b..1ca340f486 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; /** * A class for driving Mecanum drive platforms. @@ -56,6 +57,8 @@ import edu.wpi.first.wpilibj.hal.HAL; * deadband of 0 is used. */ public class MecanumDrive extends RobotDriveBase { + private static int instances = 0; + private SpeedController m_frontLeftMotor; private SpeedController m_rearLeftMotor; private SpeedController m_frontRightMotor; @@ -74,6 +77,12 @@ public class MecanumDrive extends RobotDriveBase { m_rearLeftMotor = rearLeftMotor; m_frontRightMotor = frontRightMotor; m_rearRightMotor = rearRightMotor; + addChild(m_frontLeftMotor); + addChild(m_rearLeftMotor); + addChild(m_frontRightMotor); + addChild(m_rearRightMotor); + instances++; + setName("MecanumDrive", instances); } /** @@ -174,4 +183,17 @@ public class MecanumDrive extends RobotDriveBase { public String getDescription() { return "MecanumDrive"; } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("MecanumDrive"); + builder.addDoubleProperty("Front Left Motor Speed", m_frontLeftMotor::get, + m_frontLeftMotor::set); + builder.addDoubleProperty("Front Right Motor Speed", m_frontRightMotor::get, + m_frontRightMotor::set); + builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get, + m_rearLeftMotor::set); + builder.addDoubleProperty("Rear Right Motor Speed", m_rearRightMotor::get, + m_rearRightMotor::set); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java index 2b438cb8d2..8ec1bd2b5b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java @@ -9,11 +9,12 @@ package edu.wpi.first.wpilibj.drive; import edu.wpi.first.wpilibj.MotorSafety; import edu.wpi.first.wpilibj.MotorSafetyHelper; +import edu.wpi.first.wpilibj.SendableBase; /** * Common base class for drive platforms. */ -public abstract class RobotDriveBase implements MotorSafety { +public abstract class RobotDriveBase extends SendableBase implements MotorSafety { public static final double kDefaultDeadband = 0.02; public static final double kDefaultMaxOutput = 1.0; @@ -38,6 +39,7 @@ public abstract class RobotDriveBase implements MotorSafety { public RobotDriveBase() { m_safetyHelper.setSafetyEnabled(true); + setName("RobotDriveBase"); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java index bd5c76695e..378f1d86bb 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java @@ -7,14 +7,15 @@ package edu.wpi.first.wpilibj.livewindow; -import java.util.Enumeration; -import java.util.Hashtable; -import java.util.Vector; +import java.util.HashMap; +import java.util.Map; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; +import edu.wpi.first.wpilibj.Sendable; /** @@ -23,42 +24,30 @@ import edu.wpi.first.wpilibj.command.Scheduler; */ public class LiveWindow { - private static Vector sensors = new Vector<>(); - // private static Vector actuators = new Vector(); - private static Hashtable components = new Hashtable<>(); - private static NetworkTable livewindowTable; - private static NetworkTable statusTable; - private static NetworkTableEntry enabledEntry; - private static boolean liveWindowEnabled = false; - private static boolean firstTime = true; - - /** - * Initialize all the LiveWindow elements the first time we enter LiveWindow mode. By holding off - * creating the NetworkTable entries, it allows them to be redefined before the first time in - * LiveWindow mode. This allows default sensor and actuator values to be created that are replaced - * with the custom names from users calling addActuator and addSensor. - */ - private static void initializeLiveWindowComponents() { - System.out.println("Initializing the components first time"); - livewindowTable = NetworkTableInstance.getDefault().getTable("LiveWindow"); - statusTable = livewindowTable.getSubTable(".status"); - enabledEntry = statusTable.getEntry("LW Enabled"); - for (Enumeration e = components.keys(); e.hasMoreElements(); ) { - LiveWindowSendable component = (LiveWindowSendable) e.nextElement(); - LiveWindowComponent liveWindowComponent = components.get(component); - String subsystem = liveWindowComponent.getSubsystem(); - String name = liveWindowComponent.getName(); - System.out.println("Initializing table for '" + subsystem + "' '" + name + "'"); - livewindowTable.getSubTable(subsystem).getEntry(".type").setString("LW Subsystem"); - NetworkTable table = livewindowTable.getSubTable(subsystem).getSubTable(name); - table.getEntry(".type").setString(component.getSmartDashboardType()); - table.getEntry(".name").setString(name); - table.getEntry(".subsystem").setString(subsystem); - component.initTable(table); - if (liveWindowComponent.isSensor()) { - sensors.addElement(component); - } + private static class Component { + Component(Sendable sendable, Sendable parent) { + m_sendable = sendable; + m_parent = parent; } + + final Sendable m_sendable; + Sendable m_parent; + final SendableBuilderImpl m_builder = new SendableBuilderImpl(); + boolean m_firstTime = true; + boolean m_telemetryEnabled = true; + } + + private static final Map components = new HashMap<>(); + private static final NetworkTable liveWindowTable = + NetworkTableInstance.getDefault().getTable("LiveWindow"); + private static final NetworkTable statusTable = liveWindowTable.getSubTable(".status"); + private static final NetworkTableEntry enabledEntry = statusTable.getEntry("LW Enabled"); + private static boolean startLiveWindow = false; + private static boolean liveWindowEnabled = false; + private static boolean telemetryEnabled = true; + + public static synchronized boolean isEnabled() { + return liveWindowEnabled; } /** @@ -69,28 +58,21 @@ public class LiveWindow { * themselves when they get rescheduled. This prevents arms from starting to move around, etc. * after a period of adjusting them in LiveWindow mode. */ - public static void setEnabled(boolean enabled) { + public static synchronized void setEnabled(boolean enabled) { if (liveWindowEnabled != enabled) { + Scheduler scheduler = Scheduler.getInstance(); if (enabled) { System.out.println("Starting live window mode."); - if (firstTime) { - initializeLiveWindowComponents(); - firstTime = false; - } - Scheduler.getInstance().disable(); - Scheduler.getInstance().removeAll(); - for (Enumeration e = components.keys(); e.hasMoreElements(); ) { - LiveWindowSendable component = (LiveWindowSendable) e.nextElement(); - component.startLiveWindowMode(); - } + scheduler.disable(); + scheduler.removeAll(); } else { System.out.println("stopping live window mode."); - for (Enumeration e = components.keys(); e.hasMoreElements(); ) { - LiveWindowSendable component = (LiveWindowSendable) e.nextElement(); - component.stopLiveWindowMode(); + for (Component component : components.values()) { + component.m_builder.stopLiveWindowMode(); } - Scheduler.getInstance().enable(); + scheduler.enable(); } + startLiveWindow = enabled; liveWindowEnabled = enabled; enabledEntry.setBoolean(enabled); } @@ -98,7 +80,9 @@ public class LiveWindow { /** * The run method is called repeatedly to keep the values refreshed on the screen in test mode. + * @deprecated No longer required */ + @Deprecated public static void run() { updateValues(); } @@ -109,9 +93,12 @@ public class LiveWindow { * @param subsystem The subsystem this component is part of. * @param name The name of this component. * @param component A LiveWindowSendable component that represents a sensor. + * @deprecated Use {@link Sendable#setName(String, String)} instead. */ - public static void addSensor(String subsystem, String name, LiveWindowSendable component) { - components.put(component, new LiveWindowComponent(subsystem, name, true)); + @Deprecated + public static synchronized void addSensor(String subsystem, String name, Sendable component) { + add(component); + component.setName(subsystem, name); } /** @@ -121,13 +108,12 @@ public class LiveWindow { * @param moduleType A string indicating the type of the module used in the naming (above) * @param channel The channel number the device is connected to * @param component A reference to the object being added + * @deprecated Use {@link edu.wpi.first.wpilibj.SensorBase#setName(String, int)} instead. */ - public static void addSensor(String moduleType, int channel, LiveWindowSendable component) { - addSensor("Ungrouped", moduleType + "[" + channel + "]", component); - if (sensors.contains(component)) { - sensors.removeElement(component); - } - sensors.addElement(component); + @Deprecated + public static void addSensor(String moduleType, int channel, Sendable component) { + add(component); + component.setName("Ungrouped", moduleType + "[" + channel + "]"); } /** @@ -136,9 +122,12 @@ public class LiveWindow { * @param subsystem The subsystem this component is part of. * @param name The name of this component. * @param component A LiveWindowSendable component that represents a actuator. + * @deprecated Use {@link Sendable#setName(String, String)} instead. */ - public static void addActuator(String subsystem, String name, LiveWindowSendable component) { - components.put(component, new LiveWindowComponent(subsystem, name, false)); + @Deprecated + public static synchronized void addActuator(String subsystem, String name, Sendable component) { + add(component); + component.setName(subsystem, name); } /** @@ -148,9 +137,12 @@ public class LiveWindow { * @param moduleType A string that defines the module name in the label for the value * @param channel The channel number the device is plugged into (usually PWM) * @param component The reference to the object being added + * @deprecated Use {@link edu.wpi.first.wpilibj.SensorBase#setName(String, int)} instead. */ - public static void addActuator(String moduleType, int channel, LiveWindowSendable component) { - addActuator("Ungrouped", moduleType + "[" + channel + "]", component); + @Deprecated + public static void addActuator(String moduleType, int channel, Sendable component) { + add(component); + component.setName("Ungrouped", moduleType + "[" + channel + "]"); } /** @@ -161,22 +153,137 @@ public class LiveWindow { * @param moduleNumber The number of the particular module type * @param channel The channel number the device is plugged into (usually PWM) * @param component The reference to the object being added + * @deprecated Use {@link edu.wpi.first.wpilibj.SensorBase#setName(String, int, int)} instead. */ + @Deprecated public static void addActuator(String moduleType, int moduleNumber, int channel, - LiveWindowSendable component) { - addActuator("Ungrouped", moduleType + "[" + moduleNumber + "," + channel + "]", component); + Sendable component) { + add(component); + component.setName("Ungrouped", moduleType + "[" + moduleNumber + "," + channel + "]"); } /** - * Puts all sensor values on the live window. + * Add a component to the LiveWindow. + * + * @param sendable component to add */ - private static void updateValues() { - // TODO: gross - needs to be sped up - for (int i = 0; i < sensors.size(); i++) { - LiveWindowSendable lws = sensors.elementAt(i); - lws.updateTable(); + public static synchronized void add(Sendable sendable) { + components.putIfAbsent(sendable, new Component(sendable, null)); + } + + /** + * Add a child component to a component. + * + * @param parent parent component + * @param child child component + */ + public static synchronized void addChild(Sendable parent, Object child) { + Component component = components.get(child); + if (component == null) { + component = new Component(null, parent); + components.put(child, component); + } else { + component.m_parent = parent; } - // TODO: Add actuators? - // TODO: Add better rate limiting. + component.m_telemetryEnabled = false; + } + + /** + * Remove a component from the LiveWindow. + * + * @param sendable component to remove + */ + public static synchronized void remove(Sendable sendable) { + Component component = components.remove(sendable); + if (component != null && isEnabled()) { + component.m_builder.stopLiveWindowMode(); + } + } + + /** + * Enable telemetry for a single component. + * + * @param sendable component + */ + public static synchronized void enableTelemetry(Sendable sendable) { + // Re-enable global setting in case disableAllTelemetry() was called. + telemetryEnabled = true; + Component component = components.get(sendable); + if (component != null) { + component.m_telemetryEnabled = true; + } + } + + /** + * Disable telemetry for a single component. + * + * @param sendable component + */ + public static synchronized void disableTelemetry(Sendable sendable) { + Component component = components.get(sendable); + if (component != null) { + component.m_telemetryEnabled = false; + } + } + + /** + * Disable ALL telemetry. + */ + public static synchronized void disableAllTelemetry() { + telemetryEnabled = false; + for (Component component : components.values()) { + component.m_telemetryEnabled = false; + } + } + + /** + * Tell all the sensors to update (send) their values. + * + *

Actuators are handled through callbacks on their value changing from the + * SmartDashboard widgets. + */ + public static synchronized void updateValues() { + // Only do this if either LiveWindow mode or telemetry is enabled. + if (!liveWindowEnabled && !telemetryEnabled) { + return; + } + + for (Component component : components.values()) { + if (component.m_sendable != null && component.m_parent == null + && (liveWindowEnabled || component.m_telemetryEnabled)) { + if (component.m_firstTime) { + // By holding off creating the NetworkTable entries, it allows the + // components to be redefined. This allows default sensor and actuator + // values to be created that are replaced with the custom names from + // users calling setName. + String name = component.m_sendable.getName(); + if (name.isEmpty()) { + continue; + } + String subsystem = component.m_sendable.getSubsystem(); + NetworkTable ssTable = liveWindowTable.getSubTable(subsystem); + NetworkTable table; + // Treat name==subsystem as top level of subsystem + if (name.equals(subsystem)) { + table = ssTable; + } else { + table = ssTable.getSubTable(name); + } + table.getEntry(".name").setString(name); + component.m_builder.setTable(table); + component.m_sendable.initSendable(component.m_builder); + ssTable.getEntry(".type").setString("LW Subsystem"); + + component.m_firstTime = false; + } + + if (startLiveWindow) { + component.m_builder.startLiveWindowMode(); + } + component.m_builder.updateTable(); + } + } + + startLiveWindow = false; } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowComponent.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowComponent.java deleted file mode 100644 index 14c469bbfd..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowComponent.java +++ /dev/null @@ -1,41 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2016-2017 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.livewindow; - -/** - * A LiveWindow component is a device (sensor or actuator) that should be added to the - * SmartDashboard in test mode. The components are cached until the first time the robot enters Test - * mode. This allows the components to be inserted, then renamed. - */ -/* - * This class is intentionally package private. - */ -class LiveWindowComponent { - - String m_subsystem; - String m_name; - boolean m_isSensor; - - LiveWindowComponent(String subsystem, String name, boolean isSensor) { - m_subsystem = subsystem; - m_name = name; - m_isSensor = isSensor; - } - - public String getName() { - return m_name; - } - - public String getSubsystem() { - return m_subsystem; - } - - public boolean isSensor() { - return m_isSensor; - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowSendable.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowSendable.java deleted file mode 100644 index 18d539ea68..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowSendable.java +++ /dev/null @@ -1,31 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2008-2017 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.livewindow; - -import edu.wpi.first.wpilibj.Sendable; - -/** - * Live Window Sendable is a special type of object sendable to the live window. - */ -public interface LiveWindowSendable extends Sendable { - /** - * Update the table for this sendable object with the latest values. - */ - void updateTable(); - - /** - * Start having this sendable object automatically respond to value changes reflect the value on - * the table. - */ - void startLiveWindowMode(); - - /** - * Stop having this sendable object automatically respond to value changes. - */ - void stopLiveWindowMode(); -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java new file mode 100644 index 0000000000..bf97920a6a --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java @@ -0,0 +1,143 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.smartdashboard; + +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableValue; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +public interface SendableBuilder { + /** + * Set the string representation of the named data type that will be used + * by the smart dashboard for this sendable. + * + * @param type data type + */ + void setSmartDashboardType(String type); + + /** + * Set the function that should be called to set the Sendable into a safe + * state. This is called when entering and exiting Live Window mode. + * + * @param func function + */ + void setSafeState(Runnable func); + + /** + * Set the function that should be called to update the network table + * for things other than properties. Note this function is not passed + * the network table object; instead it should use the entry handles + * returned by getEntry(). + * + * @param func function + */ + void setUpdateTable(Runnable func); + + /** + * Add a property without getters or setters. This can be used to get + * entry handles for the function called by setUpdateTable(). + * + * @param key property name + * @return Network table entry + */ + NetworkTableEntry getEntry(String key); + + /** + * Represents an operation that accepts a single boolean-valued argument and + * returns no result. This is the primitive type specialization of Consumer + * for boolean. Unlike most other functional interfaces, BooleanConsumer is + * expected to operate via side-effects. + * + *

This is a functional interface whose functional method is accept(boolean). + */ + @FunctionalInterface + interface BooleanConsumer { + /** + * Performs the operation on the given value. + * @param value the value + */ + void accept(boolean value); + } + + /** + * Add a boolean property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter); + + /** + * Add a double property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter); + + /** + * Add a string property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + void addStringProperty(String key, Supplier getter, Consumer setter); + + /** + * Add a boolean array property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + void addBooleanArrayProperty(String key, Supplier getter, Consumer setter); + + /** + * Add a double array property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + void addDoubleArrayProperty(String key, Supplier getter, Consumer setter); + + /** + * Add a string array property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + void addStringArrayProperty(String key, Supplier getter, Consumer setter); + + /** + * Add a raw property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + void addRawProperty(String key, Supplier getter, Consumer setter); + + /** + * Add a NetworkTableValue property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + void addValueProperty(String key, Supplier getter, + Consumer setter); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java new file mode 100644 index 0000000000..5bb2a8fc38 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java @@ -0,0 +1,346 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.smartdashboard; + +import edu.wpi.first.networktables.EntryListenerFlags; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableValue; +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Function; +import java.util.function.Supplier; +import java.util.ArrayList; +import java.util.List; + +public class SendableBuilderImpl implements SendableBuilder { + private static class Property { + Property(NetworkTable table, String key) { + m_entry = table.getEntry(key); + } + + @Override + @SuppressWarnings("NoFinalizer") + public synchronized void finalize() { + stopListener(); + } + + void startListener() { + if (m_entry.isValid() && m_listener == 0 && m_createListener != null) { + m_listener = m_createListener.apply(m_entry); + } + } + + void stopListener() { + if (m_entry.isValid() && m_listener != 0) { + m_entry.removeListener(m_listener); + m_listener = 0; + } + } + + final NetworkTableEntry m_entry; + int m_listener = 0; + Consumer m_update; + Function m_createListener; + } + + private final List m_properties = new ArrayList<>(); + private Runnable m_safeState; + private Runnable m_updateTable; + private NetworkTable m_table; + + /** + * Set the network table. Must be called prior to any Add* functions being + * called. + * @param table Network table + */ + public void setTable(NetworkTable table) { + m_table = table; + } + + /** + * Get the network table. + * @return The network table + */ + public NetworkTable getTable() { + return m_table; + } + + /** + * Update the network table values by calling the getters for all properties. + */ + public void updateTable() { + for (Property property : m_properties) { + if (property.m_update != null) { + property.m_update.accept(property.m_entry); + } + } + if (m_updateTable != null) { + m_updateTable.run(); + } + } + + /** + * Start LiveWindow mode by hooking the setters for all properties. + */ + public void startLiveWindowMode() { + if (m_safeState != null) { + m_safeState.run(); + } + for (Property property : m_properties) { + property.startListener(); + } + } + + /** + * Stop LiveWindow mode by unhooking the setters for all properties. + */ + public void stopLiveWindowMode() { + if (m_safeState != null) { + m_safeState.run(); + } + for (Property property : m_properties) { + property.stopListener(); + } + } + + /** + * Set the string representation of the named data type that will be used + * by the smart dashboard for this sendable. + * + * @param type data type + */ + @Override + public void setSmartDashboardType(String type) { + m_table.getEntry(".type").setString(type); + } + + /** + * Set the function that should be called to set the Sendable into a safe + * state. This is called when entering and exiting Live Window mode. + * + * @param func function + */ + @Override + public void setSafeState(Runnable func) { + m_safeState = func; + } + + /** + * Set the function that should be called to update the network table + * for things other than properties. Note this function is not passed + * the network table object; instead it should use the entry handles + * returned by getEntry(). + * + * @param func function + */ + @Override + public void setUpdateTable(Runnable func) { + m_updateTable = func; + } + + /** + * Add a property without getters or setters. This can be used to get + * entry handles for the function called by setUpdateTable(). + * + * @param key property name + * @return Network table entry + */ + @Override + public NetworkTableEntry getEntry(String key) { + return m_table.getEntry(key); + } + + /** + * Add a boolean property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + @Override + public void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter) { + Property property = new Property(m_table, key); + if (getter != null) { + property.m_update = (entry) -> entry.setBoolean(getter.getAsBoolean()); + } + if (setter != null) { + property.m_createListener = (entry) -> entry.addListener((event) -> { + if (event.value.isBoolean()) { + setter.accept(event.value.getBoolean()); + } + }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + } + m_properties.add(property); + } + + /** + * Add a double property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + @Override + public void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter) { + Property property = new Property(m_table, key); + if (getter != null) { + property.m_update = (entry) -> entry.setDouble(getter.getAsDouble()); + } + if (setter != null) { + property.m_createListener = (entry) -> entry.addListener((event) -> { + if (event.value.isDouble()) { + setter.accept(event.value.getDouble()); + } + }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + } + m_properties.add(property); + } + + /** + * Add a string property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + @Override + public void addStringProperty(String key, Supplier getter, Consumer setter) { + Property property = new Property(m_table, key); + if (getter != null) { + property.m_update = (entry) -> entry.setString(getter.get()); + } + if (setter != null) { + property.m_createListener = (entry) -> entry.addListener((event) -> { + if (event.value.isString()) { + setter.accept(event.value.getString()); + } + }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + } + m_properties.add(property); + } + + /** + * Add a boolean array property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + @Override + public void addBooleanArrayProperty(String key, Supplier getter, + Consumer setter) { + Property property = new Property(m_table, key); + if (getter != null) { + property.m_update = (entry) -> entry.setBooleanArray(getter.get()); + } + if (setter != null) { + property.m_createListener = (entry) -> entry.addListener((event) -> { + if (event.value.isBooleanArray()) { + setter.accept(event.value.getBooleanArray()); + } + }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + } + m_properties.add(property); + } + + /** + * Add a double array property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + @Override + public void addDoubleArrayProperty(String key, Supplier getter, + Consumer setter) { + Property property = new Property(m_table, key); + if (getter != null) { + property.m_update = (entry) -> entry.setDoubleArray(getter.get()); + } + if (setter != null) { + property.m_createListener = (entry) -> entry.addListener((event) -> { + if (event.value.isDoubleArray()) { + setter.accept(event.value.getDoubleArray()); + } + }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + } + m_properties.add(property); + } + + /** + * Add a string array property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + @Override + public void addStringArrayProperty(String key, Supplier getter, + Consumer setter) { + Property property = new Property(m_table, key); + if (getter != null) { + property.m_update = (entry) -> entry.setStringArray(getter.get()); + } + if (setter != null) { + property.m_createListener = (entry) -> entry.addListener((event) -> { + if (event.value.isStringArray()) { + setter.accept(event.value.getStringArray()); + } + }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + } + m_properties.add(property); + } + + /** + * Add a raw property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + @Override + public void addRawProperty(String key, Supplier getter, Consumer setter) { + Property property = new Property(m_table, key); + if (getter != null) { + property.m_update = (entry) -> entry.setRaw(getter.get()); + } + if (setter != null) { + property.m_createListener = (entry) -> entry.addListener((event) -> { + if (event.value.isRaw()) { + setter.accept(event.value.getRaw()); + } + }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + } + m_properties.add(property); + } + + /** + * Add a NetworkTableValue property. + * + * @param key property name + * @param getter getter function (returns current value) + * @param setter setter function (sets new value) + */ + @Override + public void addValueProperty(String key, Supplier getter, + Consumer setter) { + Property property = new Property(m_table, key); + if (getter != null) { + property.m_update = (entry) -> entry.setValue(getter.get()); + } + if (setter != null) { + property.m_createListener = (entry) -> entry.addListener((event) -> { + setter.accept(event.value); + }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate); + } + m_properties.add(property); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java index 1661122899..752845f998 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java @@ -9,9 +9,9 @@ package edu.wpi.first.wpilibj.smartdashboard; import java.util.LinkedHashMap; -import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.SendableBase; import edu.wpi.first.wpilibj.command.Command; import static java.util.Objects.requireNonNull; @@ -28,7 +28,7 @@ import static java.util.Objects.requireNonNull; * * @param The type of the values to be stored */ -public class SendableChooser implements Sendable { +public class SendableChooser extends SendableBase implements Sendable { /** * The key for the default value. @@ -46,7 +46,7 @@ public class SendableChooser implements Sendable { * A map linking strings to the objects the represent. */ private final LinkedHashMap m_map = new LinkedHashMap<>(); - private String m_defaultChoice = null; + private String m_defaultChoice = ""; /** * Instantiates a {@link SendableChooser}. @@ -63,10 +63,6 @@ public class SendableChooser implements Sendable { */ public void addObject(String name, V object) { m_map.put(name, object); - - if (m_tableOptions != null) { - m_tableOptions.setStringArray(m_map.keySet().toArray(new String[0])); - } } /** @@ -81,9 +77,6 @@ public class SendableChooser implements Sendable { requireNonNull(name, "Provided name was null"); m_defaultChoice = name; - if (m_tableDefault != null) { - m_tableDefault.setString(m_defaultChoice); - } addObject(name, object); } @@ -94,29 +87,24 @@ public class SendableChooser implements Sendable { * @return the option selected */ public V getSelected() { - String selected = m_tableSelected.getString(null); - return m_map.getOrDefault(selected, m_map.get(m_defaultChoice)); - } - - @Override - public String getSmartDashboardType() { - return "String Chooser"; - } - - private NetworkTableEntry m_tableDefault; - private NetworkTableEntry m_tableSelected; - private NetworkTableEntry m_tableOptions; - - @Override - public void initTable(NetworkTable table) { - if (table != null) { - m_tableDefault = table.getEntry(DEFAULT); - m_tableSelected = table.getEntry(SELECTED); - m_tableOptions = table.getEntry(OPTIONS); - m_tableOptions.setStringArray(m_map.keySet().toArray(new String[0])); - if (m_defaultChoice != null) { - m_tableDefault.setString(m_defaultChoice); - } + String selected = m_defaultChoice; + if (m_tableSelected != null) { + selected = m_tableSelected.getString(m_defaultChoice); } + return m_map.get(selected); + } + + private NetworkTableEntry m_tableSelected; + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("String Chooser"); + builder.addStringProperty(DEFAULT, () -> { + return m_defaultChoice; + }, null); + builder.addStringArrayProperty(OPTIONS, () -> { + return m_map.keySet().toArray(new String[0]); + }, null); + m_tableSelected = builder.getEntry(SELECTED); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java index df7744020e..db76bcac4b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java @@ -8,14 +8,14 @@ package edu.wpi.first.wpilibj.smartdashboard; import java.nio.ByteBuffer; -import java.util.Hashtable; +import java.util.HashMap; +import java.util.Map; import java.util.Set; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.HLUsageReporting; -import edu.wpi.first.wpilibj.NamedSendable; import edu.wpi.first.wpilibj.Sendable; /** @@ -31,11 +31,17 @@ public class SmartDashboard { */ private static final NetworkTable table = NetworkTableInstance.getDefault().getTable("SmartDashboard"); + + private static class Data { + Sendable m_sendable; + final SendableBuilderImpl m_builder = new SendableBuilderImpl(); + } + /** * A table linking tables in the SmartDashboard to the {@link Sendable} objects they * came from. */ - private static final Hashtable tablesToData = new Hashtable<>(); + private static final Map tablesToData = new HashMap<>(); static { HLUsageReporting.reportSmartDashboard(); @@ -49,25 +55,29 @@ public class SmartDashboard { * @param data the value * @throws IllegalArgumentException If key is null */ - public static void putData(String key, Sendable data) { - NetworkTable dataTable = table.getSubTable(key); - dataTable.getEntry(".type").setString(data.getSmartDashboardType()); - data.initTable(dataTable); - tablesToData.put(dataTable, data); + public static synchronized void putData(String key, Sendable data) { + Data sddata = tablesToData.get(key); + if (sddata == null) { + sddata = new Data(); + tablesToData.put(key, sddata); + } + if (sddata.m_sendable == null || sddata.m_sendable != data) { + sddata.m_sendable = data; + sddata.m_builder.setTable(table.getSubTable(key)); + data.initSendable(sddata.m_builder); + } + sddata.m_builder.updateTable(); } - - // TODO should we reimplement NamedSendable? - /** * Maps the specified key (where the key is the name of the {@link NamedSendable} - * SmartDashboardNamedData to the specified value in this table. The value can be retrieved by + * to the specified value in this table. The value can be retrieved by * calling the get method with a key that is equal to the original key. * * @param value the value * @throws IllegalArgumentException If key is null */ - public static void putData(NamedSendable value) { + public static void putData(Sendable value) { putData(value.getName(), value); } @@ -78,13 +88,12 @@ public class SmartDashboard { * @return the value * @throws IllegalArgumentException if the key is null */ - public static Sendable getData(String key) { - NetworkTable subtable = table.getSubTable(key); - Sendable data = tablesToData.get(subtable); + public static synchronized Sendable getData(String key) { + Data data = tablesToData.get(key); if (data == null) { throw new IllegalArgumentException("SmartDashboard data does not exist: " + key); } else { - return data; + return data.m_sendable; } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java index e74fc1e0a3..75e7ceb2df 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java @@ -10,7 +10,6 @@ package edu.wpi.first.wpilibj.examples.gearsbot; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous; @@ -96,7 +95,6 @@ public class Robot extends IterativeRobot { */ @Override public void testPeriodic() { - LiveWindow.run(); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java index 4aa72f4791..3e4d3e4ca6 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java @@ -8,10 +8,8 @@ package edu.wpi.first.wpilibj.examples.gearsbot.subsystems; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * The claw subsystem is a simple system with a motor for opening and closing. @@ -20,15 +18,15 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; */ public class Claw extends Subsystem { - private SpeedController m_motor = new Victor(7); + private Victor m_motor = new Victor(7); private DigitalInput m_contact = new DigitalInput(5); public Claw() { super(); - // Let's show everything on the LiveWindow - LiveWindow.addActuator("Claw", "Motor", (Victor) m_motor); - LiveWindow.addActuator("Claw", "Limit Switch", m_contact); + // Let's name everything on the LiveWindow + addChild("Motor", m_motor); + addChild("Limit Switch", m_contact); } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java index 18c60b8e15..ee679ed097 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java @@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.examples.gearsbot.Robot; import edu.wpi.first.wpilibj.examples.gearsbot.commands.TankDriveWithJoystick; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -58,11 +57,12 @@ public class DriveTrain extends Subsystem { m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0); } - // Let's show the sensors on the LiveWindow - LiveWindow.addSensor("Drive Train", "Left Encoder", m_leftEncoder); - LiveWindow.addSensor("Drive Train", "Right Encoder", m_rightEncoder); - LiveWindow.addSensor("Drive Train", "Rangefinder", m_rangefinder); - LiveWindow.addSensor("Drive Train", "Gyro", m_gyro); + // Let's name the sensors on the LiveWindow + addChild("Drive", m_drive); + addChild("Left Encoder", m_leftEncoder); + addChild("Right Encoder", m_rightEncoder); + addChild("Rangefinder", m_rangefinder); + addChild("Gyro", m_gyro); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java index 79d0cd51e3..583ed83d82 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java @@ -8,12 +8,9 @@ package edu.wpi.first.wpilibj.examples.gearsbot.subsystems; import edu.wpi.first.wpilibj.AnalogPotentiometer; -import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.command.PIDSubsystem; import edu.wpi.first.wpilibj.examples.gearsbot.Robot; -import edu.wpi.first.wpilibj.interfaces.Potentiometer; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -23,8 +20,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; */ public class Elevator extends PIDSubsystem { - private SpeedController m_motor; - private Potentiometer m_pot; + private Victor m_motor; + private AnalogPotentiometer m_pot; private static final double kP_real = 4; private static final double kI_real = 0.07; @@ -48,10 +45,9 @@ public class Elevator extends PIDSubsystem { m_pot = new AnalogPotentiometer(2); // Defaults to meters } - // Let's show everything on the LiveWindow - LiveWindow.addActuator("Elevator", "Motor", (Victor) m_motor); - LiveWindow.addSensor("Elevator", "Pot", (AnalogPotentiometer) m_pot); - LiveWindow.addActuator("Elevator", "PID", getPIDController()); + // Let's name everything on the LiveWindow + addChild("Motor", m_motor); + addChild("Pot", m_pot); } @Override @@ -62,7 +58,7 @@ public class Elevator extends PIDSubsystem { * The log method puts interesting information to the SmartDashboard. */ public void log() { - SmartDashboard.putData("Wrist Pot", (AnalogPotentiometer) m_pot); + SmartDashboard.putData("Elevator Pot", (AnalogPotentiometer) m_pot); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java index 9414f9ecbd..3fa6bce39e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java @@ -8,12 +8,9 @@ package edu.wpi.first.wpilibj.examples.gearsbot.subsystems; import edu.wpi.first.wpilibj.AnalogPotentiometer; -import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.command.PIDSubsystem; import edu.wpi.first.wpilibj.examples.gearsbot.Robot; -import edu.wpi.first.wpilibj.interfaces.Potentiometer; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -22,8 +19,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; */ public class Wrist extends PIDSubsystem { - private SpeedController m_motor; - private Potentiometer m_pot; + private Victor m_motor; + private AnalogPotentiometer m_pot; private static final double kP_real = 1; private static final double kP_simulation = 0.05; @@ -45,10 +42,9 @@ public class Wrist extends PIDSubsystem { m_pot = new AnalogPotentiometer(3); // Defaults to degrees } - // Let's show everything on the LiveWindow - LiveWindow.addActuator("Wrist", "Motor", (Victor) m_motor); - LiveWindow.addSensor("Wrist", "Pot", (AnalogPotentiometer) m_pot); - LiveWindow.addActuator("Wrist", "PID", getPIDController()); + // Let's name everything on the LiveWindow + addChild("Motor", m_motor); + addChild("Pot", m_pot); } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java index f2b2e5f1e5..39cd0de590 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * The VM is configured to automatically run this class, and to call the @@ -78,6 +77,5 @@ public class Robot extends IterativeRobot { */ @Override public void testPeriodic() { - LiveWindow.run(); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java index 202adfdb9b..01eb26c0e1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java @@ -10,7 +10,6 @@ package edu.wpi.first.wpilibj.examples.pacgoat; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -103,7 +102,6 @@ public class Robot extends IterativeRobot { // This function called periodically during test mode @Override public void testPeriodic() { - LiveWindow.run(); } @Override diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java index 704c904aaf..7b48896839 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * The Collector subsystem has one motor for the rollers, a limit switch for @@ -33,10 +32,10 @@ public class Collector extends Subsystem { public Collector() { // Put everything to the LiveWindow for testing. - LiveWindow.addActuator("Collector", "Roller Motor", (Victor) m_rollerMotor); - LiveWindow.addSensor("Collector", "Ball Detector", m_ballDetector); - LiveWindow.addSensor("Collector", "Claw Open Detector", m_openDetector); - LiveWindow.addActuator("Collector", "Piston", m_piston); + addChild("Roller Motor", (Victor) m_rollerMotor); + addChild("Ball Detector", m_ballDetector); + addChild("Claw Open Detector", m_openDetector); + addChild("Piston", m_piston); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java index fe10fcdbed..95f2ef2c94 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java @@ -17,7 +17,6 @@ import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.SpeedControllerGroup; import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.examples.pacgoat.Robot; import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveWithJoystick; @@ -43,14 +42,10 @@ public class DriveTrain extends Subsystem { public DriveTrain() { // Configure drive motors - LiveWindow.addActuator("DriveTrain", "Front Left CIM", - (Victor) m_frontLeftCIM); - LiveWindow.addActuator("DriveTrain", "Front Right CIM", - (Victor) m_frontRightCIM); - LiveWindow.addActuator("DriveTrain", "Back Left CIM", - (Victor) m_rearLeftCIM); - LiveWindow.addActuator("DriveTrain", "Back Right CIM", - (Victor) m_rearRightCIM); + addChild("Front Left CIM", (Victor) m_frontLeftCIM); + addChild("Front Right CIM", (Victor) m_frontRightCIM); + addChild("Back Left CIM", (Victor) m_rearLeftCIM); + addChild("Back Right CIM", (Victor) m_rearRightCIM); // Configure the DifferentialDrive to reflect the fact that all motors // are wired backwards (right is inverted in DifferentialDrive). @@ -75,14 +70,14 @@ public class DriveTrain extends Subsystem { (4.0/* in */ * Math.PI) / (360.0 * 12.0/* in/ft */)); } - LiveWindow.addSensor("DriveTrain", "Right Encoder", m_rightEncoder); - LiveWindow.addSensor("DriveTrain", "Left Encoder", m_leftEncoder); + addChild("Right Encoder", m_rightEncoder); + addChild("Left Encoder", m_leftEncoder); // Configure gyro if (Robot.isReal()) { m_gyro.setSensitivity(0.007); // TODO: Handle more gracefully? } - LiveWindow.addSensor("DriveTrain", "Gyro", m_gyro); + addChild("Gyro", m_gyro); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java index ddaa2db753..fcf678f977 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj.SpeedController; import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.command.PIDSubsystem; import edu.wpi.first.wpilibj.interfaces.Potentiometer; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.examples.pacgoat.Robot; @@ -49,12 +48,11 @@ public class Pivot extends PIDSubsystem { } // Put everything to the LiveWindow for testing. - LiveWindow.addSensor("Pivot", "Upper Limit Switch", m_upperLimitSwitch); - LiveWindow.addSensor("Pivot", "Lower Limit Switch", m_lowerLimitSwitch); - LiveWindow.addSensor("Pivot", "Pot", (AnalogPotentiometer) m_pot); - LiveWindow.addActuator("Pivot", "Motor", (Victor) m_motor); - LiveWindow.addActuator("Pivot", "PIDSubsystem Controller", - getPIDController()); + addChild("Upper Limit Switch", m_upperLimitSwitch); + addChild("Lower Limit Switch", m_lowerLimitSwitch); + addChild("Pot", (AnalogPotentiometer) m_pot); + addChild("Motor", (Victor) m_motor); + addChild("PIDSubsystem Controller", getPIDController()); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java index 6ee1046b83..86d19c71b9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java @@ -9,7 +9,6 @@ package edu.wpi.first.wpilibj.examples.pacgoat.subsystems; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.examples.pacgoat.Robot; @@ -26,7 +25,7 @@ public class Pneumatics extends Subsystem { private static final double kMaxPressure = 2.55; public Pneumatics() { - LiveWindow.addSensor("Pneumatics", "Pressure Sensor", m_pressureSensor); + addChild("Pressure Sensor", m_pressureSensor); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java index 61d43fcea3..555541ae89 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java @@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * The Shooter subsystem handles shooting. The mechanism for shooting is @@ -35,12 +34,10 @@ public class Shooter extends Subsystem { public Shooter() { // Put everything to the LiveWindow for testing. - LiveWindow.addSensor("Shooter", "Hot Goal Sensor", m_hotGoalSensor); - LiveWindow.addSensor("Shooter", "Piston1 Reed Switch Front ", - m_piston1ReedSwitchFront); - LiveWindow.addSensor("Shooter", "Piston1 Reed Switch Back ", - m_piston1ReedSwitchBack); - LiveWindow.addActuator("Shooter", "Latch Piston", m_latchPiston); + addChild("Hot Goal Sensor", m_hotGoalSensor); + addChild("Piston1 Reed Switch Front ", m_piston1ReedSwitchFront); + addChild("Piston1 Reed Switch Back ", m_piston1ReedSwitchBack); + addChild("Latch Piston", m_latchPiston); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java index d953d91e7b..c5b3f4ca1e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java @@ -10,7 +10,6 @@ package edu.wpi.first.wpilibj.templates.commandbased; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.templates.commandbased.commands.ExampleCommand; @@ -119,6 +118,5 @@ public class Robot extends TimedRobot { */ @Override public void testPeriodic() { - LiveWindow.run(); } } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java index b7d741814b..d0bbca13c1 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java @@ -24,6 +24,7 @@ import java.util.logging.Logger; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; import edu.wpi.first.wpilibj.test.AbstractComsSetup; import edu.wpi.first.wpilibj.test.TestBench; @@ -43,6 +44,7 @@ import static org.junit.Assert.assertTrue; public class PIDTest extends AbstractComsSetup { private static final Logger logger = Logger.getLogger(PIDTest.class.getName()); private NetworkTable m_table; + private SendableBuilderImpl m_builder; private static final double absoluteTolerance = 50; private static final double outputRange = 0.25; @@ -104,8 +106,10 @@ public class PIDTest extends AbstractComsSetup { logger.fine("Setup: " + me.getType()); me.setup(); m_table = NetworkTableInstance.getDefault().getTable("TEST_PID"); + m_builder = new SendableBuilderImpl(); + m_builder.setTable(m_table); m_controller = new PIDController(k_p, k_i, k_d, me.getEncoder(), me.getMotor()); - m_controller.initTable(m_table); + m_controller.initSendable(m_builder); } @After @@ -134,6 +138,7 @@ public class PIDTest extends AbstractComsSetup { assertFalse("PID did not begin disabled", m_controller.isEnabled()); assertEquals("PID.getError() did not start at " + setpoint, setpoint, m_controller.getError(), 0); + m_builder.updateTable(); assertEquals(k_p, m_table.getEntry("p").getDouble(9999999), 0); assertEquals(k_i, m_table.getEntry("i").getDouble(9999999), 0); assertEquals(k_d, m_table.getEntry("d").getDouble(9999999), 0); @@ -148,11 +153,12 @@ public class PIDTest extends AbstractComsSetup { double setpoint = 2500.0; m_controller.setSetpoint(setpoint); m_controller.enable(); - Timer.delay(.5); + m_builder.updateTable(); assertTrue(m_table.getEntry("enabled").getBoolean(false)); assertTrue(m_controller.isEnabled()); assertThat(0.0, is(not(me.getMotor().get()))); m_controller.reset(); + m_builder.updateTable(); assertFalse(m_table.getEntry("enabled").getBoolean(true)); assertFalse(m_controller.isEnabled()); assertEquals(0, me.getMotor().get(), 0); diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/RelayCrossConnectTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/RelayCrossConnectTest.java index 2a3ed97684..588b16ec11 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/RelayCrossConnectTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/RelayCrossConnectTest.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.Relay.Direction; import edu.wpi.first.wpilibj.Relay.InvalidValueException; import edu.wpi.first.wpilibj.Relay.Value; import edu.wpi.first.wpilibj.fixtures.RelayCrossConnectFixture; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl; import edu.wpi.first.wpilibj.test.AbstractComsSetup; import edu.wpi.first.wpilibj.test.TestBench; @@ -34,13 +35,16 @@ public class RelayCrossConnectTest extends AbstractComsSetup { private static final NetworkTable table = NetworkTableInstance.getDefault().getTable("_RELAY_CROSS_CONNECT_TEST_"); private RelayCrossConnectFixture m_relayFixture; + private SendableBuilderImpl m_builder; @Before public void setUp() throws Exception { m_relayFixture = TestBench.getRelayCrossConnectFixture(); m_relayFixture.setup(); - m_relayFixture.getRelay().initTable(table); + m_builder = new SendableBuilderImpl(); + m_builder.setTable(table); + m_relayFixture.getRelay().initSendable(m_builder); } @After @@ -53,7 +57,7 @@ public class RelayCrossConnectTest extends AbstractComsSetup { public void testBothHigh() { m_relayFixture.getRelay().setDirection(Direction.kBoth); m_relayFixture.getRelay().set(Value.kOn); - m_relayFixture.getRelay().updateTable(); + m_builder.updateTable(); assertTrue("Input one was not high when relay set both high", m_relayFixture.getInputOne() .get()); assertTrue("Input two was not high when relay set both high", m_relayFixture.getInputTwo() @@ -66,7 +70,7 @@ public class RelayCrossConnectTest extends AbstractComsSetup { public void testFirstHigh() { m_relayFixture.getRelay().setDirection(Direction.kBoth); m_relayFixture.getRelay().set(Value.kForward); - m_relayFixture.getRelay().updateTable(); + m_builder.updateTable(); assertFalse("Input one was not low when relay set Value.kForward", m_relayFixture.getInputOne() .get()); assertTrue("Input two was not high when relay set Value.kForward", m_relayFixture @@ -80,7 +84,7 @@ public class RelayCrossConnectTest extends AbstractComsSetup { public void testSecondHigh() { m_relayFixture.getRelay().setDirection(Direction.kBoth); m_relayFixture.getRelay().set(Value.kReverse); - m_relayFixture.getRelay().updateTable(); + m_builder.updateTable(); assertTrue("Input one was not high when relay set Value.kReverse", m_relayFixture.getInputOne() .get()); assertFalse("Input two was not low when relay set Value.kReverse", m_relayFixture @@ -94,7 +98,7 @@ public class RelayCrossConnectTest extends AbstractComsSetup { public void testForwardDirection() { m_relayFixture.getRelay().setDirection(Direction.kForward); m_relayFixture.getRelay().set(Value.kOn); - m_relayFixture.getRelay().updateTable(); + m_builder.updateTable(); assertFalse("Input one was not low when relay set Value.kOn in kForward Direction", m_relayFixture.getInputOne().get()); assertTrue("Input two was not high when relay set Value.kOn in kForward Direction", @@ -107,7 +111,7 @@ public class RelayCrossConnectTest extends AbstractComsSetup { public void testReverseDirection() { m_relayFixture.getRelay().setDirection(Direction.kReverse); m_relayFixture.getRelay().set(Value.kOn); - m_relayFixture.getRelay().updateTable(); + m_builder.updateTable(); assertTrue("Input one was not high when relay set Value.kOn in kReverse Direction", m_relayFixture.getInputOne().get()); assertFalse("Input two was not low when relay set Value.kOn in kReverse Direction", @@ -130,6 +134,7 @@ public class RelayCrossConnectTest extends AbstractComsSetup { @Test public void testInitialSettings() { + m_builder.updateTable(); assertEquals(Value.kOff, m_relayFixture.getRelay().get()); // Initially both outputs should be off assertFalse(m_relayFixture.getInputOne().get());