mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[wpilib] Remove LiveWindow (#7733)
This will be replaced by a different mechanism, but removing it eases the initial implementation burden of a new Telemetry/Sendable framework.
This commit is contained in:
@@ -43,11 +43,6 @@ public class LogBackedSendableBuilder implements SendableBuilder {
|
|||||||
// ignore
|
// ignore
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
|
||||||
public void setSafeState(Runnable func) {
|
|
||||||
// ignore
|
|
||||||
}
|
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter) {
|
public void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter) {
|
||||||
m_updates.add(() -> m_backend.log(key, getter.getAsBoolean()));
|
m_updates.add(() -> m_backend.log(key, getter.getAsBoolean()));
|
||||||
|
|||||||
@@ -1,2 +1,2 @@
|
|||||||
org.gradle.jvmargs=-Xmx2g
|
org.gradle.jvmargs=-Xmx4g
|
||||||
org.ysb33r.gradle.doxygen.download.url=https://frcmaven.wpi.edu/artifactory/generic-release-mirror/doxygen
|
org.ysb33r.gradle.doxygen.download.url=https://frcmaven.wpi.edu/artifactory/generic-release-mirror/doxygen
|
||||||
|
|||||||
@@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj.RobotState;
|
|||||||
import edu.wpi.first.wpilibj.TimedRobot;
|
import edu.wpi.first.wpilibj.TimedRobot;
|
||||||
import edu.wpi.first.wpilibj.Watchdog;
|
import edu.wpi.first.wpilibj.Watchdog;
|
||||||
import edu.wpi.first.wpilibj.event.EventLoop;
|
import edu.wpi.first.wpilibj.event.EventLoop;
|
||||||
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
|
|
||||||
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
|
import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
|
||||||
import java.io.PrintWriter;
|
import java.io.PrintWriter;
|
||||||
import java.io.StringWriter;
|
import java.io.StringWriter;
|
||||||
@@ -101,13 +100,7 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
|
|||||||
|
|
||||||
CommandScheduler() {
|
CommandScheduler() {
|
||||||
HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler);
|
HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler);
|
||||||
SendableRegistry.addLW(this, "Scheduler");
|
SendableRegistry.add(this, "Scheduler");
|
||||||
LiveWindow.setEnabledListener(
|
|
||||||
() -> {
|
|
||||||
disable();
|
|
||||||
cancelAll();
|
|
||||||
});
|
|
||||||
LiveWindow.setDisabledListener(this::enable);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -123,8 +116,6 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
|
|||||||
@Override
|
@Override
|
||||||
public void close() {
|
public void close() {
|
||||||
SendableRegistry.remove(this);
|
SendableRegistry.remove(this);
|
||||||
LiveWindow.setEnabledListener(null);
|
|
||||||
LiveWindow.setDisabledListener(null);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -20,7 +20,7 @@ public abstract class SubsystemBase implements Subsystem, Sendable {
|
|||||||
public SubsystemBase() {
|
public SubsystemBase() {
|
||||||
String name = this.getClass().getSimpleName();
|
String name = this.getClass().getSimpleName();
|
||||||
name = name.substring(name.lastIndexOf('.') + 1);
|
name = name.substring(name.lastIndexOf('.') + 1);
|
||||||
SendableRegistry.addLW(this, name, name);
|
SendableRegistry.add(this, name, name);
|
||||||
CommandScheduler.getInstance().registerSubsystem(this);
|
CommandScheduler.getInstance().registerSubsystem(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -31,7 +31,7 @@ public abstract class SubsystemBase implements Subsystem, Sendable {
|
|||||||
*/
|
*/
|
||||||
@SuppressWarnings("this-escape")
|
@SuppressWarnings("this-escape")
|
||||||
public SubsystemBase(String name) {
|
public SubsystemBase(String name) {
|
||||||
SendableRegistry.addLW(this, name, name);
|
SendableRegistry.add(this, name, name);
|
||||||
CommandScheduler.getInstance().registerSubsystem(this);
|
CommandScheduler.getInstance().registerSubsystem(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -79,7 +79,7 @@ public abstract class SubsystemBase implements Subsystem, Sendable {
|
|||||||
* @param child sendable
|
* @param child sendable
|
||||||
*/
|
*/
|
||||||
public void addChild(String name, Sendable child) {
|
public void addChild(String name, Sendable child) {
|
||||||
SendableRegistry.addLW(child, getSubsystem(), name);
|
SendableRegistry.add(child, getSubsystem(), name);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|||||||
@@ -13,7 +13,6 @@
|
|||||||
#include <frc/RobotBase.h>
|
#include <frc/RobotBase.h>
|
||||||
#include <frc/RobotState.h>
|
#include <frc/RobotState.h>
|
||||||
#include <frc/TimedRobot.h>
|
#include <frc/TimedRobot.h>
|
||||||
#include <frc/livewindow/LiveWindow.h>
|
|
||||||
#include <hal/FRCUsageReporting.h>
|
#include <hal/FRCUsageReporting.h>
|
||||||
#include <hal/HALBase.h>
|
#include <hal/HALBase.h>
|
||||||
#include <networktables/IntegerArrayTopic.h>
|
#include <networktables/IntegerArrayTopic.h>
|
||||||
@@ -72,19 +71,11 @@ CommandScheduler::CommandScheduler()
|
|||||||
}) {
|
}) {
|
||||||
HAL_Report(HALUsageReporting::kResourceType_Command,
|
HAL_Report(HALUsageReporting::kResourceType_Command,
|
||||||
HALUsageReporting::kCommand2_Scheduler);
|
HALUsageReporting::kCommand2_Scheduler);
|
||||||
wpi::SendableRegistry::AddLW(this, "Scheduler");
|
wpi::SendableRegistry::Add(this, "Scheduler");
|
||||||
frc::LiveWindow::SetEnabledCallback([this] {
|
|
||||||
this->Disable();
|
|
||||||
this->CancelAll();
|
|
||||||
});
|
|
||||||
frc::LiveWindow::SetDisabledCallback([this] { this->Enable(); });
|
|
||||||
}
|
}
|
||||||
|
|
||||||
CommandScheduler::~CommandScheduler() {
|
CommandScheduler::~CommandScheduler() {
|
||||||
wpi::SendableRegistry::Remove(this);
|
wpi::SendableRegistry::Remove(this);
|
||||||
frc::LiveWindow::SetEnabledCallback(nullptr);
|
|
||||||
frc::LiveWindow::SetDisabledCallback(nullptr);
|
|
||||||
|
|
||||||
std::unique_ptr<Impl>().swap(m_impl);
|
std::unique_ptr<Impl>().swap(m_impl);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -15,12 +15,12 @@
|
|||||||
using namespace frc2;
|
using namespace frc2;
|
||||||
|
|
||||||
SubsystemBase::SubsystemBase() {
|
SubsystemBase::SubsystemBase() {
|
||||||
wpi::SendableRegistry::AddLW(this, GetTypeName(*this));
|
wpi::SendableRegistry::Add(this, GetTypeName(*this));
|
||||||
CommandScheduler::GetInstance().RegisterSubsystem({this});
|
CommandScheduler::GetInstance().RegisterSubsystem({this});
|
||||||
}
|
}
|
||||||
|
|
||||||
SubsystemBase::SubsystemBase(std::string_view name) {
|
SubsystemBase::SubsystemBase(std::string_view name) {
|
||||||
wpi::SendableRegistry::AddLW(this, name);
|
wpi::SendableRegistry::Add(this, name);
|
||||||
CommandScheduler::GetInstance().RegisterSubsystem({this});
|
CommandScheduler::GetInstance().RegisterSubsystem({this});
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -71,5 +71,5 @@ void SubsystemBase::SetSubsystem(std::string_view name) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void SubsystemBase::AddChild(std::string name, wpi::Sendable* child) {
|
void SubsystemBase::AddChild(std::string name, wpi::Sendable* child) {
|
||||||
wpi::SendableRegistry::AddLW(child, GetSubsystem(), name);
|
wpi::SendableRegistry::Add(child, GetSubsystem(), name);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -30,7 +30,7 @@ ADXL345_I2C::ADXL345_I2C(I2C::Port port, Range range, int deviceAddress)
|
|||||||
HAL_Report(HALUsageReporting::kResourceType_ADXL345,
|
HAL_Report(HALUsageReporting::kResourceType_ADXL345,
|
||||||
HALUsageReporting::kADXL345_I2C, 0);
|
HALUsageReporting::kADXL345_I2C, 0);
|
||||||
|
|
||||||
wpi::SendableRegistry::AddLW(this, "ADXL345_I2C", port);
|
wpi::SendableRegistry::Add(this, "ADXL345_I2C", port);
|
||||||
}
|
}
|
||||||
|
|
||||||
I2C::Port ADXL345_I2C::GetI2CPort() const {
|
I2C::Port ADXL345_I2C::GetI2CPort() const {
|
||||||
|
|||||||
@@ -56,6 +56,6 @@ void AnalogAccelerometer::InitAccelerometer() {
|
|||||||
HAL_Report(HALUsageReporting::kResourceType_Accelerometer,
|
HAL_Report(HALUsageReporting::kResourceType_Accelerometer,
|
||||||
m_analogInput->GetChannel() + 1);
|
m_analogInput->GetChannel() + 1);
|
||||||
|
|
||||||
wpi::SendableRegistry::AddLW(this, "Accelerometer",
|
wpi::SendableRegistry::Add(this, "Accelerometer",
|
||||||
m_analogInput->GetChannel());
|
m_analogInput->GetChannel());
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -67,8 +67,8 @@ void AnalogEncoder::Init(double fullRange, double expectedZero) {
|
|||||||
m_fullRange = fullRange;
|
m_fullRange = fullRange;
|
||||||
m_expectedZero = expectedZero;
|
m_expectedZero = expectedZero;
|
||||||
|
|
||||||
wpi::SendableRegistry::AddLW(this, "Analog Encoder",
|
wpi::SendableRegistry::Add(this, "Analog Encoder",
|
||||||
m_analogInput->GetChannel());
|
m_analogInput->GetChannel());
|
||||||
}
|
}
|
||||||
|
|
||||||
double AnalogEncoder::Get() const {
|
double AnalogEncoder::Get() const {
|
||||||
|
|||||||
@@ -35,7 +35,7 @@ AnalogInput::AnalogInput(int channel) {
|
|||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel + 1);
|
HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel + 1);
|
||||||
|
|
||||||
wpi::SendableRegistry::AddLW(this, "AnalogInput", channel);
|
wpi::SendableRegistry::Add(this, "AnalogInput", channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
int AnalogInput::GetValue() const {
|
int AnalogInput::GetValue() const {
|
||||||
|
|||||||
@@ -33,8 +33,8 @@ AnalogPotentiometer::AnalogPotentiometer(std::shared_ptr<AnalogInput> input,
|
|||||||
: m_analog_input(std::move(input)),
|
: m_analog_input(std::move(input)),
|
||||||
m_fullRange(fullRange),
|
m_fullRange(fullRange),
|
||||||
m_offset(offset) {
|
m_offset(offset) {
|
||||||
wpi::SendableRegistry::AddLW(this, "AnalogPotentiometer",
|
wpi::SendableRegistry::Add(this, "AnalogPotentiometer",
|
||||||
m_analog_input->GetChannel());
|
m_analog_input->GetChannel());
|
||||||
}
|
}
|
||||||
|
|
||||||
double AnalogPotentiometer::Get() const {
|
double AnalogPotentiometer::Get() const {
|
||||||
|
|||||||
@@ -38,7 +38,7 @@ AnalogTrigger::AnalogTrigger(std::shared_ptr<AnalogInput> input)
|
|||||||
int index = GetIndex();
|
int index = GetIndex();
|
||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
|
HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "AnalogTrigger", index);
|
wpi::SendableRegistry::Add(this, "AnalogTrigger", index);
|
||||||
}
|
}
|
||||||
|
|
||||||
AnalogTrigger::AnalogTrigger(DutyCycle& input)
|
AnalogTrigger::AnalogTrigger(DutyCycle& input)
|
||||||
@@ -55,7 +55,7 @@ AnalogTrigger::AnalogTrigger(std::shared_ptr<DutyCycle> input)
|
|||||||
int index = GetIndex();
|
int index = GetIndex();
|
||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
|
HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "AnalogTrigger", index);
|
wpi::SendableRegistry::Add(this, "AnalogTrigger", index);
|
||||||
}
|
}
|
||||||
|
|
||||||
void AnalogTrigger::SetLimitsVoltage(double lower, double upper) {
|
void AnalogTrigger::SetLimitsVoltage(double lower, double upper) {
|
||||||
|
|||||||
@@ -25,7 +25,7 @@ Compressor::Compressor(int module, PneumaticsModuleType moduleType)
|
|||||||
m_module->EnableCompressorDigital();
|
m_module->EnableCompressorDigital();
|
||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_Compressor, module + 1);
|
HAL_Report(HALUsageReporting::kResourceType_Compressor, module + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "Compressor", module);
|
wpi::SendableRegistry::Add(this, "Compressor", module);
|
||||||
}
|
}
|
||||||
|
|
||||||
Compressor::Compressor(PneumaticsModuleType moduleType)
|
Compressor::Compressor(PneumaticsModuleType moduleType)
|
||||||
|
|||||||
@@ -27,7 +27,7 @@ Counter::Counter(Mode mode) {
|
|||||||
SetMaxPeriod(0.5_s);
|
SetMaxPeriod(0.5_s);
|
||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1, mode + 1);
|
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1, mode + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "Counter", m_index);
|
wpi::SendableRegistry::Add(this, "Counter", m_index);
|
||||||
}
|
}
|
||||||
|
|
||||||
Counter::Counter(int channel) : Counter(kTwoPulse) {
|
Counter::Counter(int channel) : Counter(kTwoPulse) {
|
||||||
|
|||||||
@@ -32,7 +32,7 @@ DigitalInput::DigitalInput(int channel) {
|
|||||||
FRC_CheckErrorStatus(status, "Channel {}", channel);
|
FRC_CheckErrorStatus(status, "Channel {}", channel);
|
||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel + 1);
|
HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "DigitalInput", channel);
|
wpi::SendableRegistry::Add(this, "DigitalInput", channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool DigitalInput::Get() const {
|
bool DigitalInput::Get() const {
|
||||||
|
|||||||
@@ -33,7 +33,7 @@ DigitalOutput::DigitalOutput(int channel) {
|
|||||||
FRC_CheckErrorStatus(status, "Channel {}", channel);
|
FRC_CheckErrorStatus(status, "Channel {}", channel);
|
||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel + 1);
|
HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "DigitalOutput", channel);
|
wpi::SendableRegistry::Add(this, "DigitalOutput", channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
DigitalOutput::~DigitalOutput() {
|
DigitalOutput::~DigitalOutput() {
|
||||||
|
|||||||
@@ -55,8 +55,8 @@ DoubleSolenoid::DoubleSolenoid(int module, PneumaticsModuleType moduleType,
|
|||||||
HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel + 1,
|
HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel + 1,
|
||||||
m_module->GetModuleNumber() + 1);
|
m_module->GetModuleNumber() + 1);
|
||||||
|
|
||||||
wpi::SendableRegistry::AddLW(this, "DoubleSolenoid",
|
wpi::SendableRegistry::Add(this, "DoubleSolenoid",
|
||||||
m_module->GetModuleNumber(), m_forwardChannel);
|
m_module->GetModuleNumber(), m_forwardChannel);
|
||||||
}
|
}
|
||||||
|
|
||||||
DoubleSolenoid::DoubleSolenoid(PneumaticsModuleType moduleType,
|
DoubleSolenoid::DoubleSolenoid(PneumaticsModuleType moduleType,
|
||||||
@@ -129,7 +129,6 @@ bool DoubleSolenoid::IsRevSolenoidDisabled() const {
|
|||||||
void DoubleSolenoid::InitSendable(wpi::SendableBuilder& builder) {
|
void DoubleSolenoid::InitSendable(wpi::SendableBuilder& builder) {
|
||||||
builder.SetSmartDashboardType("Double Solenoid");
|
builder.SetSmartDashboardType("Double Solenoid");
|
||||||
builder.SetActuator(true);
|
builder.SetActuator(true);
|
||||||
builder.SetSafeState([=, this] { Set(kOff); });
|
|
||||||
builder.AddSmallStringProperty(
|
builder.AddSmallStringProperty(
|
||||||
"Value",
|
"Value",
|
||||||
[=, this](wpi::SmallVectorImpl<char>& buf) -> std::string_view {
|
[=, this](wpi::SmallVectorImpl<char>& buf) -> std::string_view {
|
||||||
|
|||||||
@@ -34,7 +34,7 @@ void DutyCycle::InitDutyCycle() {
|
|||||||
&status);
|
&status);
|
||||||
FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
|
FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
|
||||||
HAL_Report(HALUsageReporting::kResourceType_DutyCycle, m_channel + 1);
|
HAL_Report(HALUsageReporting::kResourceType_DutyCycle, m_channel + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "Duty Cycle", m_channel);
|
wpi::SendableRegistry::Add(this, "Duty Cycle", m_channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
int DutyCycle::GetFPGAIndex() const {
|
int DutyCycle::GetFPGAIndex() const {
|
||||||
|
|||||||
@@ -74,8 +74,8 @@ void DutyCycleEncoder::Init(double fullRange, double expectedZero) {
|
|||||||
m_fullRange = fullRange;
|
m_fullRange = fullRange;
|
||||||
m_expectedZero = expectedZero;
|
m_expectedZero = expectedZero;
|
||||||
|
|
||||||
wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder",
|
wpi::SendableRegistry::Add(this, "DutyCycle Encoder",
|
||||||
m_dutyCycle->GetSourceChannel());
|
m_dutyCycle->GetSourceChannel());
|
||||||
}
|
}
|
||||||
|
|
||||||
double DutyCycleEncoder::Get() const {
|
double DutyCycleEncoder::Get() const {
|
||||||
|
|||||||
@@ -234,7 +234,7 @@ void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) {
|
|||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex() + 1,
|
HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex() + 1,
|
||||||
encodingType);
|
encodingType);
|
||||||
wpi::SendableRegistry::AddLW(this, "Encoder", m_aSource->GetChannel());
|
wpi::SendableRegistry::Add(this, "Encoder", m_aSource->GetChannel());
|
||||||
}
|
}
|
||||||
|
|
||||||
double Encoder::DecodingScaleFactor() const {
|
double Encoder::DecodingScaleFactor() const {
|
||||||
|
|||||||
@@ -13,7 +13,6 @@
|
|||||||
|
|
||||||
#include "frc/DSControlWord.h"
|
#include "frc/DSControlWord.h"
|
||||||
#include "frc/Errors.h"
|
#include "frc/Errors.h"
|
||||||
#include "frc/livewindow/LiveWindow.h"
|
|
||||||
#include "frc/smartdashboard/SmartDashboard.h"
|
#include "frc/smartdashboard/SmartDashboard.h"
|
||||||
|
|
||||||
using namespace frc;
|
using namespace frc;
|
||||||
@@ -96,24 +95,6 @@ void IterativeRobotBase::SetNetworkTablesFlushEnabled(bool enabled) {
|
|||||||
m_ntFlushEnabled = enabled;
|
m_ntFlushEnabled = enabled;
|
||||||
}
|
}
|
||||||
|
|
||||||
void IterativeRobotBase::EnableLiveWindowInTest(bool testLW) {
|
|
||||||
static bool hasReported;
|
|
||||||
if (IsTestEnabled()) {
|
|
||||||
throw FRC_MakeError(err::IncompatibleMode,
|
|
||||||
"Can't configure test mode while in test mode!");
|
|
||||||
}
|
|
||||||
if (!hasReported && testLW) {
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_SmartDashboard,
|
|
||||||
HALUsageReporting::kSmartDashboard_LiveWindow);
|
|
||||||
hasReported = true;
|
|
||||||
}
|
|
||||||
m_lwEnabledInTest = testLW;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool IterativeRobotBase::IsLiveWindowEnabledInTest() {
|
|
||||||
return m_lwEnabledInTest;
|
|
||||||
}
|
|
||||||
|
|
||||||
units::second_t IterativeRobotBase::GetPeriod() const {
|
units::second_t IterativeRobotBase::GetPeriod() const {
|
||||||
return m_period;
|
return m_period;
|
||||||
}
|
}
|
||||||
@@ -150,9 +131,6 @@ void IterativeRobotBase::LoopFunc() {
|
|||||||
} else if (m_lastMode == Mode::kTeleop) {
|
} else if (m_lastMode == Mode::kTeleop) {
|
||||||
TeleopExit();
|
TeleopExit();
|
||||||
} else if (m_lastMode == Mode::kTest) {
|
} else if (m_lastMode == Mode::kTest) {
|
||||||
if (m_lwEnabledInTest) {
|
|
||||||
LiveWindow::SetEnabled(false);
|
|
||||||
}
|
|
||||||
TestExit();
|
TestExit();
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -167,9 +145,6 @@ void IterativeRobotBase::LoopFunc() {
|
|||||||
TeleopInit();
|
TeleopInit();
|
||||||
m_watchdog.AddEpoch("TeleopInit()");
|
m_watchdog.AddEpoch("TeleopInit()");
|
||||||
} else if (mode == Mode::kTest) {
|
} else if (mode == Mode::kTest) {
|
||||||
if (m_lwEnabledInTest) {
|
|
||||||
LiveWindow::SetEnabled(true);
|
|
||||||
}
|
|
||||||
TestInit();
|
TestInit();
|
||||||
m_watchdog.AddEpoch("TestInit()");
|
m_watchdog.AddEpoch("TestInit()");
|
||||||
}
|
}
|
||||||
@@ -201,8 +176,6 @@ void IterativeRobotBase::LoopFunc() {
|
|||||||
|
|
||||||
SmartDashboard::UpdateValues();
|
SmartDashboard::UpdateValues();
|
||||||
m_watchdog.AddEpoch("SmartDashboard::UpdateValues()");
|
m_watchdog.AddEpoch("SmartDashboard::UpdateValues()");
|
||||||
LiveWindow::UpdateValues();
|
|
||||||
m_watchdog.AddEpoch("LiveWindow::UpdateValues()");
|
|
||||||
|
|
||||||
if constexpr (IsSimulation()) {
|
if constexpr (IsSimulation()) {
|
||||||
HAL_SimPeriodicBefore();
|
HAL_SimPeriodicBefore();
|
||||||
|
|||||||
@@ -40,7 +40,7 @@ PWM::PWM(int channel, bool registerSendable) {
|
|||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_PWM, channel + 1);
|
HAL_Report(HALUsageReporting::kResourceType_PWM, channel + 1);
|
||||||
if (registerSendable) {
|
if (registerSendable) {
|
||||||
wpi::SendableRegistry::AddLW(this, "PWM", channel);
|
wpi::SendableRegistry::Add(this, "PWM", channel);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -174,7 +174,6 @@ int PWM::GetChannel() const {
|
|||||||
void PWM::InitSendable(wpi::SendableBuilder& builder) {
|
void PWM::InitSendable(wpi::SendableBuilder& builder) {
|
||||||
builder.SetSmartDashboardType("PWM");
|
builder.SetSmartDashboardType("PWM");
|
||||||
builder.SetActuator(true);
|
builder.SetActuator(true);
|
||||||
builder.SetSafeState([=, this] { SetDisabled(); });
|
|
||||||
builder.AddDoubleProperty(
|
builder.AddDoubleProperty(
|
||||||
"Value", [=, this] { return GetPulseTime().value(); },
|
"Value", [=, this] { return GetPulseTime().value(); },
|
||||||
[=, this](double value) { SetPulseTime(units::millisecond_t{value}); });
|
[=, this](double value) { SetPulseTime(units::millisecond_t{value}); });
|
||||||
|
|||||||
@@ -47,7 +47,7 @@ PowerDistribution::PowerDistribution() {
|
|||||||
HAL_Report(HALUsageReporting::kResourceType_PDP,
|
HAL_Report(HALUsageReporting::kResourceType_PDP,
|
||||||
HALUsageReporting::kPDP_REV);
|
HALUsageReporting::kPDP_REV);
|
||||||
}
|
}
|
||||||
wpi::SendableRegistry::AddLW(this, "PowerDistribution", m_module);
|
wpi::SendableRegistry::Add(this, "PowerDistribution", m_module);
|
||||||
}
|
}
|
||||||
|
|
||||||
PowerDistribution::PowerDistribution(int module, ModuleType moduleType) {
|
PowerDistribution::PowerDistribution(int module, ModuleType moduleType) {
|
||||||
@@ -68,7 +68,7 @@ PowerDistribution::PowerDistribution(int module, ModuleType moduleType) {
|
|||||||
HAL_Report(HALUsageReporting::kResourceType_PDP,
|
HAL_Report(HALUsageReporting::kResourceType_PDP,
|
||||||
HALUsageReporting::kPDP_REV);
|
HALUsageReporting::kPDP_REV);
|
||||||
}
|
}
|
||||||
wpi::SendableRegistry::AddLW(this, "PowerDistribution", m_module);
|
wpi::SendableRegistry::Add(this, "PowerDistribution", m_module);
|
||||||
}
|
}
|
||||||
|
|
||||||
int PowerDistribution::GetNumChannels() const {
|
int PowerDistribution::GetNumChannels() const {
|
||||||
|
|||||||
@@ -31,7 +31,7 @@ SharpIR SharpIR::GP2Y0A51SK0F(int channel) {
|
|||||||
|
|
||||||
SharpIR::SharpIR(int channel, double a, double b, double minCM, double maxCM)
|
SharpIR::SharpIR(int channel, double a, double b, double minCM, double maxCM)
|
||||||
: m_sensor(channel), m_A(a), m_B(b), m_minCM(minCM), m_maxCM(maxCM) {
|
: m_sensor(channel), m_A(a), m_B(b), m_minCM(minCM), m_maxCM(maxCM) {
|
||||||
wpi::SendableRegistry::AddLW(this, "SharpIR", channel);
|
wpi::SendableRegistry::Add(this, "SharpIR", channel);
|
||||||
|
|
||||||
m_simDevice = hal::SimDevice("SharpIR", m_sensor.GetChannel());
|
m_simDevice = hal::SimDevice("SharpIR", m_sensor.GetChannel());
|
||||||
if (m_simDevice) {
|
if (m_simDevice) {
|
||||||
|
|||||||
@@ -30,8 +30,8 @@ Solenoid::Solenoid(int module, PneumaticsModuleType moduleType, int channel)
|
|||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel + 1,
|
HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel + 1,
|
||||||
m_module->GetModuleNumber() + 1);
|
m_module->GetModuleNumber() + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "Solenoid", m_module->GetModuleNumber(),
|
wpi::SendableRegistry::Add(this, "Solenoid", m_module->GetModuleNumber(),
|
||||||
m_channel);
|
m_channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
Solenoid::Solenoid(PneumaticsModuleType moduleType, int channel)
|
Solenoid::Solenoid(PneumaticsModuleType moduleType, int channel)
|
||||||
@@ -77,7 +77,6 @@ void Solenoid::StartPulse() {
|
|||||||
void Solenoid::InitSendable(wpi::SendableBuilder& builder) {
|
void Solenoid::InitSendable(wpi::SendableBuilder& builder) {
|
||||||
builder.SetSmartDashboardType("Solenoid");
|
builder.SetSmartDashboardType("Solenoid");
|
||||||
builder.SetActuator(true);
|
builder.SetActuator(true);
|
||||||
builder.SetSafeState([=, this] { Set(false); });
|
|
||||||
builder.AddBooleanProperty(
|
builder.AddBooleanProperty(
|
||||||
"Value", [=, this] { return Get(); },
|
"Value", [=, this] { return Get(); },
|
||||||
[=, this](bool value) { Set(value); });
|
[=, this](bool value) { Set(value); });
|
||||||
|
|||||||
@@ -183,7 +183,7 @@ void Ultrasonic::Initialize() {
|
|||||||
static int instances = 0;
|
static int instances = 0;
|
||||||
instances++;
|
instances++;
|
||||||
HAL_Report(HALUsageReporting::kResourceType_Ultrasonic, instances);
|
HAL_Report(HALUsageReporting::kResourceType_Ultrasonic, instances);
|
||||||
wpi::SendableRegistry::AddLW(this, "Ultrasonic", m_echoChannel->GetChannel());
|
wpi::SendableRegistry::Add(this, "Ultrasonic", m_echoChannel->GetChannel());
|
||||||
}
|
}
|
||||||
|
|
||||||
void Ultrasonic::UltrasonicChecker() {
|
void Ultrasonic::UltrasonicChecker() {
|
||||||
|
|||||||
@@ -60,7 +60,7 @@ ExternalDirectionCounter::ExternalDirectionCounter(
|
|||||||
Reset();
|
Reset();
|
||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
|
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "External Direction Counter", m_index);
|
wpi::SendableRegistry::Add(this, "External Direction Counter", m_index);
|
||||||
}
|
}
|
||||||
|
|
||||||
int ExternalDirectionCounter::GetCount() const {
|
int ExternalDirectionCounter::GetCount() const {
|
||||||
|
|||||||
@@ -34,7 +34,7 @@ Tachometer::Tachometer(std::shared_ptr<DigitalSource> source) {
|
|||||||
FRC_CheckErrorStatus(status, "{}", m_index);
|
FRC_CheckErrorStatus(status, "{}", m_index);
|
||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
|
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "Tachometer", m_index);
|
wpi::SendableRegistry::Add(this, "Tachometer", m_index);
|
||||||
}
|
}
|
||||||
|
|
||||||
units::hertz_t Tachometer::GetFrequency() const {
|
units::hertz_t Tachometer::GetFrequency() const {
|
||||||
|
|||||||
@@ -54,7 +54,7 @@ UpDownCounter::UpDownCounter(std::shared_ptr<DigitalSource> upSource,
|
|||||||
Reset();
|
Reset();
|
||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
|
HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "UpDown Counter", m_index);
|
wpi::SendableRegistry::Add(this, "UpDown Counter", m_index);
|
||||||
}
|
}
|
||||||
|
|
||||||
int UpDownCounter::GetCount() const {
|
int UpDownCounter::GetCount() const {
|
||||||
|
|||||||
@@ -35,7 +35,7 @@ DifferentialDrive::DifferentialDrive(std::function<void(double)> leftMotor,
|
|||||||
: m_leftMotor{std::move(leftMotor)}, m_rightMotor{std::move(rightMotor)} {
|
: m_leftMotor{std::move(leftMotor)}, m_rightMotor{std::move(rightMotor)} {
|
||||||
static int instances = 0;
|
static int instances = 0;
|
||||||
++instances;
|
++instances;
|
||||||
wpi::SendableRegistry::AddLW(this, "DifferentialDrive", instances);
|
wpi::SendableRegistry::Add(this, "DifferentialDrive", instances);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
|
void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
|
||||||
@@ -194,7 +194,6 @@ std::string DifferentialDrive::GetDescription() const {
|
|||||||
void DifferentialDrive::InitSendable(wpi::SendableBuilder& builder) {
|
void DifferentialDrive::InitSendable(wpi::SendableBuilder& builder) {
|
||||||
builder.SetSmartDashboardType("DifferentialDrive");
|
builder.SetSmartDashboardType("DifferentialDrive");
|
||||||
builder.SetActuator(true);
|
builder.SetActuator(true);
|
||||||
builder.SetSafeState([=, this] { StopMotor(); });
|
|
||||||
builder.AddDoubleProperty(
|
builder.AddDoubleProperty(
|
||||||
"Left Motor Speed", [&] { return m_leftOutput; }, m_leftMotor);
|
"Left Motor Speed", [&] { return m_leftOutput; }, m_leftMotor);
|
||||||
builder.AddDoubleProperty(
|
builder.AddDoubleProperty(
|
||||||
|
|||||||
@@ -46,7 +46,7 @@ MecanumDrive::MecanumDrive(std::function<void(double)> frontLeftMotor,
|
|||||||
m_rearRightMotor{std::move(rearRightMotor)} {
|
m_rearRightMotor{std::move(rearRightMotor)} {
|
||||||
static int instances = 0;
|
static int instances = 0;
|
||||||
++instances;
|
++instances;
|
||||||
wpi::SendableRegistry::AddLW(this, "MecanumDrive", instances);
|
wpi::SendableRegistry::Add(this, "MecanumDrive", instances);
|
||||||
}
|
}
|
||||||
|
|
||||||
void MecanumDrive::DriveCartesian(double xSpeed, double ySpeed,
|
void MecanumDrive::DriveCartesian(double xSpeed, double ySpeed,
|
||||||
@@ -133,7 +133,6 @@ std::string MecanumDrive::GetDescription() const {
|
|||||||
void MecanumDrive::InitSendable(wpi::SendableBuilder& builder) {
|
void MecanumDrive::InitSendable(wpi::SendableBuilder& builder) {
|
||||||
builder.SetSmartDashboardType("MecanumDrive");
|
builder.SetSmartDashboardType("MecanumDrive");
|
||||||
builder.SetActuator(true);
|
builder.SetActuator(true);
|
||||||
builder.SetSafeState([=, this] { StopMotor(); });
|
|
||||||
builder.AddDoubleProperty(
|
builder.AddDoubleProperty(
|
||||||
"Front Left Motor Speed", [&] { return m_frontLeftOutput; },
|
"Front Left Motor Speed", [&] { return m_frontLeftOutput; },
|
||||||
m_frontLeftMotor);
|
m_frontLeftMotor);
|
||||||
|
|||||||
@@ -1,230 +0,0 @@
|
|||||||
// Copyright (c) FIRST and other WPILib contributors.
|
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
|
||||||
// the WPILib BSD license file in the root directory of this project.
|
|
||||||
|
|
||||||
#include "frc/livewindow/LiveWindow.h"
|
|
||||||
|
|
||||||
#include <memory>
|
|
||||||
|
|
||||||
#include <networktables/BooleanTopic.h>
|
|
||||||
#include <networktables/NetworkTable.h>
|
|
||||||
#include <networktables/NetworkTableInstance.h>
|
|
||||||
#include <networktables/StringTopic.h>
|
|
||||||
#include <wpi/json.h>
|
|
||||||
#include <wpi/mutex.h>
|
|
||||||
#include <wpi/sendable/Sendable.h>
|
|
||||||
#include <wpi/sendable/SendableRegistry.h>
|
|
||||||
|
|
||||||
#include "frc/smartdashboard/SendableBuilderImpl.h"
|
|
||||||
|
|
||||||
using namespace frc;
|
|
||||||
|
|
||||||
static constexpr std::string_view kSmartDashboardType = "LW Subsystem";
|
|
||||||
|
|
||||||
namespace {
|
|
||||||
struct Component {
|
|
||||||
bool firstTime = true;
|
|
||||||
bool telemetryEnabled = false;
|
|
||||||
nt::StringPublisher namePub;
|
|
||||||
nt::StringPublisher typePub;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct Instance {
|
|
||||||
Instance() {
|
|
||||||
wpi::SendableRegistry::SetLiveWindowBuilderFactory(
|
|
||||||
[] { return std::make_unique<SendableBuilderImpl>(); });
|
|
||||||
enabledPub.Set(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
wpi::mutex mutex;
|
|
||||||
|
|
||||||
int dataHandle = wpi::SendableRegistry::GetDataHandle();
|
|
||||||
|
|
||||||
std::shared_ptr<nt::NetworkTable> liveWindowTable =
|
|
||||||
nt::NetworkTableInstance::GetDefault().GetTable("LiveWindow");
|
|
||||||
std::shared_ptr<nt::NetworkTable> statusTable =
|
|
||||||
liveWindowTable->GetSubTable(".status");
|
|
||||||
nt::BooleanPublisher enabledPub =
|
|
||||||
statusTable->GetBooleanTopic("LW Enabled").Publish();
|
|
||||||
|
|
||||||
bool startLiveWindow = false;
|
|
||||||
bool liveWindowEnabled = false;
|
|
||||||
bool telemetryEnabled = false;
|
|
||||||
|
|
||||||
std::function<void()> enabled;
|
|
||||||
std::function<void()> disabled;
|
|
||||||
|
|
||||||
std::shared_ptr<Component> GetOrAdd(wpi::Sendable* sendable);
|
|
||||||
};
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
static std::unique_ptr<Instance>& GetInstanceHolder() {
|
|
||||||
static std::unique_ptr<Instance> instance = std::make_unique<Instance>();
|
|
||||||
return instance;
|
|
||||||
}
|
|
||||||
|
|
||||||
static Instance& GetInstance() {
|
|
||||||
return *GetInstanceHolder();
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifndef __FRC_ROBORIO__
|
|
||||||
namespace frc::impl {
|
|
||||||
void ResetLiveWindow() {
|
|
||||||
std::make_unique<Instance>().swap(GetInstanceHolder());
|
|
||||||
}
|
|
||||||
} // namespace frc::impl
|
|
||||||
#endif
|
|
||||||
|
|
||||||
std::shared_ptr<Component> Instance::GetOrAdd(wpi::Sendable* sendable) {
|
|
||||||
auto data = std::static_pointer_cast<Component>(
|
|
||||||
wpi::SendableRegistry::GetData(sendable, dataHandle));
|
|
||||||
if (!data) {
|
|
||||||
data = std::make_shared<Component>();
|
|
||||||
wpi::SendableRegistry::SetData(sendable, dataHandle, data);
|
|
||||||
}
|
|
||||||
return data;
|
|
||||||
}
|
|
||||||
|
|
||||||
void LiveWindow::SetEnabledCallback(std::function<void()> func) {
|
|
||||||
::GetInstance().enabled = func;
|
|
||||||
}
|
|
||||||
|
|
||||||
void LiveWindow::SetDisabledCallback(std::function<void()> func) {
|
|
||||||
::GetInstance().disabled = func;
|
|
||||||
}
|
|
||||||
|
|
||||||
void LiveWindow::EnableTelemetry(wpi::Sendable* sendable) {
|
|
||||||
auto& inst = ::GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
// Re-enable global setting in case DisableAllTelemetry() was called.
|
|
||||||
inst.telemetryEnabled = true;
|
|
||||||
inst.GetOrAdd(sendable)->telemetryEnabled = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void LiveWindow::DisableTelemetry(wpi::Sendable* sendable) {
|
|
||||||
auto& inst = ::GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
inst.GetOrAdd(sendable)->telemetryEnabled = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
void LiveWindow::DisableAllTelemetry() {
|
|
||||||
auto& inst = ::GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
inst.telemetryEnabled = false;
|
|
||||||
wpi::SendableRegistry::ForeachLiveWindow(inst.dataHandle, [&](auto& cbdata) {
|
|
||||||
if (!cbdata.data) {
|
|
||||||
cbdata.data = std::make_shared<Component>();
|
|
||||||
}
|
|
||||||
std::static_pointer_cast<Component>(cbdata.data)->telemetryEnabled = false;
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
void LiveWindow::EnableAllTelemetry() {
|
|
||||||
auto& inst = ::GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
inst.telemetryEnabled = true;
|
|
||||||
wpi::SendableRegistry::ForeachLiveWindow(inst.dataHandle, [&](auto& cbdata) {
|
|
||||||
if (!cbdata.data) {
|
|
||||||
cbdata.data = std::make_shared<Component>();
|
|
||||||
}
|
|
||||||
std::static_pointer_cast<Component>(cbdata.data)->telemetryEnabled = true;
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
bool LiveWindow::IsEnabled() {
|
|
||||||
auto& inst = ::GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
return inst.liveWindowEnabled;
|
|
||||||
}
|
|
||||||
|
|
||||||
void LiveWindow::SetEnabled(bool enabled) {
|
|
||||||
auto& inst = ::GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
if (inst.liveWindowEnabled == enabled) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
inst.startLiveWindow = enabled;
|
|
||||||
inst.liveWindowEnabled = enabled;
|
|
||||||
// Force table generation now to make sure everything is defined
|
|
||||||
UpdateValuesUnsafe();
|
|
||||||
if (enabled) {
|
|
||||||
if (inst.enabled) {
|
|
||||||
inst.enabled();
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
wpi::SendableRegistry::ForeachLiveWindow(
|
|
||||||
inst.dataHandle, [&](auto& cbdata) {
|
|
||||||
static_cast<SendableBuilderImpl&>(cbdata.builder)
|
|
||||||
.StopLiveWindowMode();
|
|
||||||
});
|
|
||||||
if (inst.disabled) {
|
|
||||||
inst.disabled();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
inst.enabledPub.Set(enabled);
|
|
||||||
}
|
|
||||||
|
|
||||||
void LiveWindow::UpdateValues() {
|
|
||||||
auto& inst = ::GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
UpdateValuesUnsafe();
|
|
||||||
}
|
|
||||||
|
|
||||||
void LiveWindow::UpdateValuesUnsafe() {
|
|
||||||
auto& inst = ::GetInstance();
|
|
||||||
// Only do this if either LiveWindow mode or telemetry is enabled.
|
|
||||||
if (!inst.liveWindowEnabled && !inst.telemetryEnabled) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
wpi::SendableRegistry::ForeachLiveWindow(inst.dataHandle, [&](auto& cbdata) {
|
|
||||||
if (!cbdata.sendable || cbdata.parent) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!cbdata.data) {
|
|
||||||
cbdata.data = std::make_shared<Component>();
|
|
||||||
}
|
|
||||||
|
|
||||||
auto& comp = *std::static_pointer_cast<Component>(cbdata.data);
|
|
||||||
|
|
||||||
if (!inst.liveWindowEnabled && !comp.telemetryEnabled) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
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.
|
|
||||||
if (cbdata.name.empty()) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
auto ssTable = inst.liveWindowTable->GetSubTable(cbdata.subsystem);
|
|
||||||
std::shared_ptr<nt::NetworkTable> table;
|
|
||||||
// Treat name==subsystem as top level of subsystem
|
|
||||||
if (cbdata.name == cbdata.subsystem) {
|
|
||||||
table = ssTable;
|
|
||||||
} else {
|
|
||||||
table = ssTable->GetSubTable(cbdata.name);
|
|
||||||
}
|
|
||||||
comp.namePub = nt::StringTopic{table->GetTopic(".name")}.Publish();
|
|
||||||
comp.namePub.Set(cbdata.name);
|
|
||||||
static_cast<SendableBuilderImpl&>(cbdata.builder).SetTable(table);
|
|
||||||
cbdata.sendable->InitSendable(cbdata.builder);
|
|
||||||
comp.typePub = nt::StringTopic{ssTable->GetTopic(".type")}.PublishEx(
|
|
||||||
nt::StringTopic::kTypeString,
|
|
||||||
{{"SmartDashboard", kSmartDashboardType}});
|
|
||||||
comp.typePub.Set(kSmartDashboardType);
|
|
||||||
|
|
||||||
comp.firstTime = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (inst.startLiveWindow) {
|
|
||||||
static_cast<SendableBuilderImpl&>(cbdata.builder).StartLiveWindowMode();
|
|
||||||
}
|
|
||||||
cbdata.builder.Update();
|
|
||||||
});
|
|
||||||
|
|
||||||
inst.startLiveWindow = false;
|
|
||||||
}
|
|
||||||
@@ -74,7 +74,6 @@ void MotorControllerGroup::StopMotor() {
|
|||||||
void MotorControllerGroup::InitSendable(wpi::SendableBuilder& builder) {
|
void MotorControllerGroup::InitSendable(wpi::SendableBuilder& builder) {
|
||||||
builder.SetSmartDashboardType("Motor Controller");
|
builder.SetSmartDashboardType("Motor Controller");
|
||||||
builder.SetActuator(true);
|
builder.SetActuator(true);
|
||||||
builder.SetSafeState([=, this] { StopMotor(); });
|
|
||||||
builder.AddDoubleProperty(
|
builder.AddDoubleProperty(
|
||||||
"Value", [=, this] { return Get(); },
|
"Value", [=, this] { return Get(); },
|
||||||
[=, this](double value) { Set(value); });
|
[=, this](double value) { Set(value); });
|
||||||
|
|||||||
@@ -27,7 +27,7 @@ NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
|
|||||||
m_dio.EnablePWM(0.5);
|
m_dio.EnablePWM(0.5);
|
||||||
|
|
||||||
HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel + 1);
|
HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel + 1);
|
||||||
wpi::SendableRegistry::AddLW(this, "Nidec Brushless", pwmChannel);
|
wpi::SendableRegistry::Add(this, "Nidec Brushless", pwmChannel);
|
||||||
}
|
}
|
||||||
|
|
||||||
WPI_UNIGNORE_DEPRECATED
|
WPI_UNIGNORE_DEPRECATED
|
||||||
@@ -79,7 +79,6 @@ int NidecBrushless::GetChannel() const {
|
|||||||
void NidecBrushless::InitSendable(wpi::SendableBuilder& builder) {
|
void NidecBrushless::InitSendable(wpi::SendableBuilder& builder) {
|
||||||
builder.SetSmartDashboardType("Nidec Brushless");
|
builder.SetSmartDashboardType("Nidec Brushless");
|
||||||
builder.SetActuator(true);
|
builder.SetActuator(true);
|
||||||
builder.SetSafeState([=, this] { StopMotor(); });
|
|
||||||
builder.AddDoubleProperty(
|
builder.AddDoubleProperty(
|
||||||
"Value", [=, this] { return Get(); },
|
"Value", [=, this] { return Get(); },
|
||||||
[=, this](double value) { Set(value); });
|
[=, this](double value) { Set(value); });
|
||||||
|
|||||||
@@ -94,7 +94,7 @@ WPI_IGNORE_DEPRECATED
|
|||||||
|
|
||||||
PWMMotorController::PWMMotorController(std::string_view name, int channel)
|
PWMMotorController::PWMMotorController(std::string_view name, int channel)
|
||||||
: m_pwm(channel, false) {
|
: m_pwm(channel, false) {
|
||||||
wpi::SendableRegistry::AddLW(this, name, channel);
|
wpi::SendableRegistry::Add(this, name, channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
WPI_UNIGNORE_DEPRECATED
|
WPI_UNIGNORE_DEPRECATED
|
||||||
@@ -102,7 +102,6 @@ WPI_UNIGNORE_DEPRECATED
|
|||||||
void PWMMotorController::InitSendable(wpi::SendableBuilder& builder) {
|
void PWMMotorController::InitSendable(wpi::SendableBuilder& builder) {
|
||||||
builder.SetSmartDashboardType("Motor Controller");
|
builder.SetSmartDashboardType("Motor Controller");
|
||||||
builder.SetActuator(true);
|
builder.SetActuator(true);
|
||||||
builder.SetSafeState([=, this] { Disable(); });
|
|
||||||
builder.AddDoubleProperty(
|
builder.AddDoubleProperty(
|
||||||
"Value", [=, this] { return Get(); },
|
"Value", [=, this] { return Get(); },
|
||||||
[=, this](double value) { Set(value); });
|
[=, this](double value) { Set(value); });
|
||||||
|
|||||||
@@ -77,20 +77,6 @@ void SendableBuilderImpl::StopListeners() {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void SendableBuilderImpl::StartLiveWindowMode() {
|
|
||||||
if (m_safeState) {
|
|
||||||
m_safeState();
|
|
||||||
}
|
|
||||||
StartListeners();
|
|
||||||
}
|
|
||||||
|
|
||||||
void SendableBuilderImpl::StopLiveWindowMode() {
|
|
||||||
StopListeners();
|
|
||||||
if (m_safeState) {
|
|
||||||
m_safeState();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void SendableBuilderImpl::ClearProperties() {
|
void SendableBuilderImpl::ClearProperties() {
|
||||||
m_properties.clear();
|
m_properties.clear();
|
||||||
}
|
}
|
||||||
@@ -111,10 +97,6 @@ void SendableBuilderImpl::SetActuator(bool value) {
|
|||||||
m_actuator = value;
|
m_actuator = value;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SendableBuilderImpl::SetSafeState(std::function<void()> func) {
|
|
||||||
m_safeState = func;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SendableBuilderImpl::SetUpdateTable(wpi::unique_function<void()> func) {
|
void SendableBuilderImpl::SetUpdateTable(wpi::unique_function<void()> func) {
|
||||||
m_updateTables.emplace_back(std::move(func));
|
m_updateTables.emplace_back(std::move(func));
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -26,7 +26,6 @@
|
|||||||
#include "frc/DriverStation.h"
|
#include "frc/DriverStation.h"
|
||||||
#include "frc/Errors.h"
|
#include "frc/Errors.h"
|
||||||
#include "frc/Notifier.h"
|
#include "frc/Notifier.h"
|
||||||
#include "frc/livewindow/LiveWindow.h"
|
|
||||||
#include "frc/smartdashboard/SmartDashboard.h"
|
#include "frc/smartdashboard/SmartDashboard.h"
|
||||||
|
|
||||||
static_assert(frc::RuntimeType::kRoboRIO ==
|
static_assert(frc::RuntimeType::kRoboRIO ==
|
||||||
@@ -333,7 +332,4 @@ RobotBase::RobotBase() {
|
|||||||
|
|
||||||
// Call DriverStation::RefreshData() to kick things off
|
// Call DriverStation::RefreshData() to kick things off
|
||||||
DriverStation::RefreshData();
|
DriverStation::RefreshData();
|
||||||
|
|
||||||
// First and one-time initialization
|
|
||||||
LiveWindow::SetEnabled(false);
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -212,19 +212,6 @@ class IterativeRobotBase : public RobotBase {
|
|||||||
[[deprecated("Deprecated without replacement.")]]
|
[[deprecated("Deprecated without replacement.")]]
|
||||||
void SetNetworkTablesFlushEnabled(bool enabled);
|
void SetNetworkTablesFlushEnabled(bool enabled);
|
||||||
|
|
||||||
/**
|
|
||||||
* Sets whether LiveWindow operation is enabled during test mode.
|
|
||||||
*
|
|
||||||
* @param testLW True to enable, false to disable. Defaults to false.
|
|
||||||
* @throws if called in test mode.
|
|
||||||
*/
|
|
||||||
void EnableLiveWindowInTest(bool testLW);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Whether LiveWindow operation is enabled during test mode.
|
|
||||||
*/
|
|
||||||
bool IsLiveWindowEnabledInTest();
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Gets time period between calls to Periodic() functions.
|
* Gets time period between calls to Periodic() functions.
|
||||||
*/
|
*/
|
||||||
@@ -260,7 +247,6 @@ class IterativeRobotBase : public RobotBase {
|
|||||||
units::second_t m_period;
|
units::second_t m_period;
|
||||||
Watchdog m_watchdog;
|
Watchdog m_watchdog;
|
||||||
bool m_ntFlushEnabled = true;
|
bool m_ntFlushEnabled = true;
|
||||||
bool m_lwEnabledInTest = false;
|
|
||||||
bool m_calledDsConnected = false;
|
bool m_calledDsConnected = false;
|
||||||
|
|
||||||
void PrintLoopOverrunMessage();
|
void PrintLoopOverrunMessage();
|
||||||
|
|||||||
@@ -54,7 +54,6 @@ class PWM : public wpi::Sendable, public wpi::SendableHelper<PWM> {
|
|||||||
* @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the
|
* @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the
|
||||||
* MXP port
|
* MXP port
|
||||||
* @param registerSendable If true, adds this instance to SendableRegistry
|
* @param registerSendable If true, adds this instance to SendableRegistry
|
||||||
* and LiveWindow
|
|
||||||
*/
|
*/
|
||||||
explicit PWM(int channel, bool registerSendable = true);
|
explicit PWM(int channel, bool registerSendable = true);
|
||||||
|
|
||||||
|
|||||||
@@ -1,90 +0,0 @@
|
|||||||
// Copyright (c) FIRST and other WPILib contributors.
|
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
|
||||||
// the WPILib BSD license file in the root directory of this project.
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <functional>
|
|
||||||
|
|
||||||
namespace wpi {
|
|
||||||
class Sendable;
|
|
||||||
} // namespace wpi
|
|
||||||
|
|
||||||
namespace frc {
|
|
||||||
|
|
||||||
/**
|
|
||||||
* The LiveWindow class is the public interface for putting sensors and
|
|
||||||
* actuators on the LiveWindow.
|
|
||||||
*/
|
|
||||||
class LiveWindow final {
|
|
||||||
public:
|
|
||||||
/**
|
|
||||||
* Sets function to be called when LiveWindow is enabled.
|
|
||||||
*
|
|
||||||
* @param func function (or nullptr for none)
|
|
||||||
*/
|
|
||||||
static void SetEnabledCallback(std::function<void()> func);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Sets function to be called when LiveWindow is disabled.
|
|
||||||
*
|
|
||||||
* @param func function (or nullptr for none)
|
|
||||||
*/
|
|
||||||
static void SetDisabledCallback(std::function<void()> func);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Enable telemetry for a single component.
|
|
||||||
*
|
|
||||||
* @param component sendable
|
|
||||||
*/
|
|
||||||
static void EnableTelemetry(wpi::Sendable* component);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Disable telemetry for a single component.
|
|
||||||
*
|
|
||||||
* @param component sendable
|
|
||||||
*/
|
|
||||||
static void DisableTelemetry(wpi::Sendable* component);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Disable ALL telemetry.
|
|
||||||
*/
|
|
||||||
static void DisableAllTelemetry();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Enable ALL telemetry.
|
|
||||||
*/
|
|
||||||
static void EnableAllTelemetry();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Returns true if LiveWindow is enabled.
|
|
||||||
*
|
|
||||||
* @return True if LiveWindow is enabled.
|
|
||||||
*/
|
|
||||||
static bool IsEnabled();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Change the enabled status of LiveWindow.
|
|
||||||
*
|
|
||||||
* If it changes to enabled, start livewindow running otherwise stop it
|
|
||||||
*/
|
|
||||||
static void SetEnabled(bool enabled);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Tell all the sensors to update (send) their values.
|
|
||||||
*
|
|
||||||
* Actuators are handled through callbacks on their value changing from the
|
|
||||||
* SmartDashboard widgets.
|
|
||||||
*/
|
|
||||||
static void UpdateValues();
|
|
||||||
|
|
||||||
private:
|
|
||||||
LiveWindow() = default;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Updates the entries, without using a mutex or lock.
|
|
||||||
*/
|
|
||||||
static void UpdateValuesUnsafe();
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace frc
|
|
||||||
@@ -73,18 +73,6 @@ class SendableBuilderImpl : public nt::NTSendableBuilder {
|
|||||||
*/
|
*/
|
||||||
void StopListeners();
|
void StopListeners();
|
||||||
|
|
||||||
/**
|
|
||||||
* Start LiveWindow mode by hooking the setters for all properties. Also
|
|
||||||
* calls the SafeState function if one was provided.
|
|
||||||
*/
|
|
||||||
void StartLiveWindowMode();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Stop LiveWindow mode by unhooking the setters for all properties. Also
|
|
||||||
* calls the SafeState function if one was provided.
|
|
||||||
*/
|
|
||||||
void StopLiveWindowMode();
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Clear properties.
|
* Clear properties.
|
||||||
*/
|
*/
|
||||||
@@ -92,7 +80,6 @@ class SendableBuilderImpl : public nt::NTSendableBuilder {
|
|||||||
|
|
||||||
void SetSmartDashboardType(std::string_view type) override;
|
void SetSmartDashboardType(std::string_view type) override;
|
||||||
void SetActuator(bool value) override;
|
void SetActuator(bool value) override;
|
||||||
void SetSafeState(std::function<void()> func) override;
|
|
||||||
void SetUpdateTable(wpi::unique_function<void()> func) override;
|
void SetUpdateTable(wpi::unique_function<void()> func) override;
|
||||||
nt::Topic GetTopic(std::string_view key) override;
|
nt::Topic GetTopic(std::string_view key) override;
|
||||||
|
|
||||||
@@ -238,7 +225,6 @@ class SendableBuilderImpl : public nt::NTSendableBuilder {
|
|||||||
void AddSmallPropertyImpl(Topic topic, Getter getter, Setter setter);
|
void AddSmallPropertyImpl(Topic topic, Getter getter, Setter setter);
|
||||||
|
|
||||||
std::vector<std::unique_ptr<Property>> m_properties;
|
std::vector<std::unique_ptr<Property>> m_properties;
|
||||||
std::function<void()> m_safeState;
|
|
||||||
std::vector<wpi::unique_function<void()>> m_updateTables;
|
std::vector<wpi::unique_function<void()>> m_updateTables;
|
||||||
std::shared_ptr<nt::NetworkTable> m_table;
|
std::shared_ptr<nt::NetworkTable> m_table;
|
||||||
bool m_controllable = false;
|
bool m_controllable = false;
|
||||||
|
|||||||
@@ -11,7 +11,6 @@
|
|||||||
|
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
#include "frc/livewindow/LiveWindow.h"
|
|
||||||
#include "frc/simulation/DriverStationSim.h"
|
#include "frc/simulation/DriverStationSim.h"
|
||||||
#include "frc/simulation/SimHooks.h"
|
#include "frc/simulation/SimHooks.h"
|
||||||
|
|
||||||
@@ -20,7 +19,7 @@ using namespace frc;
|
|||||||
inline constexpr auto kPeriod = 20_ms;
|
inline constexpr auto kPeriod = 20_ms;
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
class TimedRobotTest : public ::testing::TestWithParam<bool> {
|
class TimedRobotTest : public ::testing::Test {
|
||||||
protected:
|
protected:
|
||||||
void SetUp() override { frc::sim::PauseTiming(); }
|
void SetUp() override { frc::sim::PauseTiming(); }
|
||||||
|
|
||||||
@@ -308,12 +307,8 @@ TEST_F(TimedRobotTest, TeleopMode) {
|
|||||||
robotThread.join();
|
robotThread.join();
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_P(TimedRobotTest, TestMode) {
|
TEST_F(TimedRobotTest, TestMode) {
|
||||||
bool isTestLW = GetParam();
|
|
||||||
|
|
||||||
MockRobot robot;
|
MockRobot robot;
|
||||||
robot.EnableLiveWindowInTest(isTestLW);
|
|
||||||
|
|
||||||
std::thread robotThread{[&] { robot.StartCompetition(); }};
|
std::thread robotThread{[&] { robot.StartCompetition(); }};
|
||||||
|
|
||||||
frc::sim::DriverStationSim::SetEnabled(true);
|
frc::sim::DriverStationSim::SetEnabled(true);
|
||||||
@@ -328,7 +323,6 @@ TEST_P(TimedRobotTest, TestMode) {
|
|||||||
EXPECT_EQ(0u, robot.m_autonomousInitCount);
|
EXPECT_EQ(0u, robot.m_autonomousInitCount);
|
||||||
EXPECT_EQ(0u, robot.m_teleopInitCount);
|
EXPECT_EQ(0u, robot.m_teleopInitCount);
|
||||||
EXPECT_EQ(0u, robot.m_testInitCount);
|
EXPECT_EQ(0u, robot.m_testInitCount);
|
||||||
EXPECT_FALSE(frc::LiveWindow::IsEnabled());
|
|
||||||
|
|
||||||
EXPECT_EQ(0u, robot.m_robotPeriodicCount);
|
EXPECT_EQ(0u, robot.m_robotPeriodicCount);
|
||||||
EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
|
EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
|
||||||
@@ -350,9 +344,6 @@ TEST_P(TimedRobotTest, TestMode) {
|
|||||||
EXPECT_EQ(0u, robot.m_autonomousInitCount);
|
EXPECT_EQ(0u, robot.m_autonomousInitCount);
|
||||||
EXPECT_EQ(0u, robot.m_teleopInitCount);
|
EXPECT_EQ(0u, robot.m_teleopInitCount);
|
||||||
EXPECT_EQ(1u, robot.m_testInitCount);
|
EXPECT_EQ(1u, robot.m_testInitCount);
|
||||||
EXPECT_EQ(isTestLW, frc::LiveWindow::IsEnabled());
|
|
||||||
|
|
||||||
EXPECT_THROW(robot.EnableLiveWindowInTest(isTestLW), std::runtime_error);
|
|
||||||
|
|
||||||
EXPECT_EQ(1u, robot.m_robotPeriodicCount);
|
EXPECT_EQ(1u, robot.m_robotPeriodicCount);
|
||||||
EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
|
EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
|
||||||
@@ -399,7 +390,6 @@ TEST_P(TimedRobotTest, TestMode) {
|
|||||||
EXPECT_EQ(0u, robot.m_autonomousInitCount);
|
EXPECT_EQ(0u, robot.m_autonomousInitCount);
|
||||||
EXPECT_EQ(0u, robot.m_teleopInitCount);
|
EXPECT_EQ(0u, robot.m_teleopInitCount);
|
||||||
EXPECT_EQ(1u, robot.m_testInitCount);
|
EXPECT_EQ(1u, robot.m_testInitCount);
|
||||||
EXPECT_FALSE(frc::LiveWindow::IsEnabled());
|
|
||||||
|
|
||||||
EXPECT_EQ(3u, robot.m_robotPeriodicCount);
|
EXPECT_EQ(3u, robot.m_robotPeriodicCount);
|
||||||
EXPECT_EQ(3u, robot.m_simulationPeriodicCount);
|
EXPECT_EQ(3u, robot.m_simulationPeriodicCount);
|
||||||
@@ -609,5 +599,3 @@ TEST_F(TimedRobotTest, AddPeriodicWithOffset) {
|
|||||||
robot.EndCompetition();
|
robot.EndCompetition();
|
||||||
robotThread.join();
|
robotThread.join();
|
||||||
}
|
}
|
||||||
|
|
||||||
INSTANTIATE_TEST_SUITE_P(TimedRobotTests, TimedRobotTest, testing::Bool());
|
|
||||||
|
|||||||
@@ -6,7 +6,6 @@
|
|||||||
|
|
||||||
#include <frc/DriverStation.h>
|
#include <frc/DriverStation.h>
|
||||||
#include <frc/internal/DriverStationModeThread.h>
|
#include <frc/internal/DriverStationModeThread.h>
|
||||||
#include <frc/livewindow/LiveWindow.h>
|
|
||||||
#include <hal/DriverStation.h>
|
#include <hal/DriverStation.h>
|
||||||
#include <networktables/NetworkTable.h>
|
#include <networktables/NetworkTable.h>
|
||||||
|
|
||||||
@@ -45,14 +44,12 @@ void Robot::StartCompetition() {
|
|||||||
wpi::WaitForObject(event.GetHandle());
|
wpi::WaitForObject(event.GetHandle());
|
||||||
}
|
}
|
||||||
} else if (IsTest()) {
|
} else if (IsTest()) {
|
||||||
frc::LiveWindow::SetEnabled(true);
|
|
||||||
modeThread.InTest(true);
|
modeThread.InTest(true);
|
||||||
Test();
|
Test();
|
||||||
modeThread.InTest(false);
|
modeThread.InTest(false);
|
||||||
while (IsTest() && IsEnabled()) {
|
while (IsTest() && IsEnabled()) {
|
||||||
wpi::WaitForObject(event.GetHandle());
|
wpi::WaitForObject(event.GetHandle());
|
||||||
}
|
}
|
||||||
frc::LiveWindow::SetEnabled(false);
|
|
||||||
} else {
|
} else {
|
||||||
modeThread.InTeleop(true);
|
modeThread.InTeleop(true);
|
||||||
Teleop();
|
Teleop();
|
||||||
|
|||||||
@@ -4,16 +4,11 @@
|
|||||||
|
|
||||||
#include "Robot.h"
|
#include "Robot.h"
|
||||||
|
|
||||||
#include <frc/livewindow/LiveWindow.h>
|
|
||||||
#include <frc/smartdashboard/SmartDashboard.h>
|
#include <frc/smartdashboard/SmartDashboard.h>
|
||||||
#include <wpi/print.h>
|
#include <wpi/print.h>
|
||||||
|
|
||||||
// Run robot periodic() functions for 5 ms, and run controllers every 10 ms
|
// Run robot periodic() functions for 5 ms, and run controllers every 10 ms
|
||||||
Robot::Robot() : frc::TimesliceRobot{5_ms, 10_ms} {
|
Robot::Robot() : frc::TimesliceRobot{5_ms, 10_ms} {
|
||||||
// LiveWindow causes drastic overruns in robot periodic functions that will
|
|
||||||
// interfere with controllers
|
|
||||||
frc::LiveWindow::DisableAllTelemetry();
|
|
||||||
|
|
||||||
// Runs for 2 ms after robot periodic functions
|
// Runs for 2 ms after robot periodic functions
|
||||||
Schedule([=] {}, 2_ms);
|
Schedule([=] {}, 2_ms);
|
||||||
|
|
||||||
|
|||||||
@@ -4,14 +4,8 @@
|
|||||||
|
|
||||||
#include "Robot.h"
|
#include "Robot.h"
|
||||||
|
|
||||||
#include <frc/livewindow/LiveWindow.h>
|
|
||||||
|
|
||||||
// Run robot periodic() functions for 5 ms, and run controllers every 10 ms
|
// Run robot periodic() functions for 5 ms, and run controllers every 10 ms
|
||||||
Robot::Robot() : frc::TimesliceRobot{5_ms, 10_ms} {
|
Robot::Robot() : frc::TimesliceRobot{5_ms, 10_ms} {
|
||||||
// LiveWindow causes drastic overruns in robot periodic functions that will
|
|
||||||
// interfere with controllers
|
|
||||||
frc::LiveWindow::DisableAllTelemetry();
|
|
||||||
|
|
||||||
// Runs for 2 ms after robot periodic functions
|
// Runs for 2 ms after robot periodic functions
|
||||||
Schedule([=] {}, 2_ms);
|
Schedule([=] {}, 2_ms);
|
||||||
|
|
||||||
|
|||||||
@@ -135,7 +135,7 @@ public class ADXL345_I2C implements NTSendable, AutoCloseable {
|
|||||||
setRange(range);
|
setRange(range);
|
||||||
|
|
||||||
HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_I2C);
|
HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_I2C);
|
||||||
SendableRegistry.addLW(this, "ADXL345_I2C", port.value);
|
SendableRegistry.add(this, "ADXL345_I2C", port.value);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -26,7 +26,7 @@ public class AnalogAccelerometer implements Sendable, AutoCloseable {
|
|||||||
/** Common initialization. */
|
/** Common initialization. */
|
||||||
private void initAccelerometer() {
|
private void initAccelerometer() {
|
||||||
HAL.report(tResourceType.kResourceType_Accelerometer, m_analogChannel.getChannel() + 1);
|
HAL.report(tResourceType.kResourceType_Accelerometer, m_analogChannel.getChannel() + 1);
|
||||||
SendableRegistry.addLW(this, "Accelerometer", m_analogChannel.getChannel());
|
SendableRegistry.add(this, "Accelerometer", m_analogChannel.getChannel());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -83,7 +83,7 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
|
|||||||
m_fullRange = fullRange;
|
m_fullRange = fullRange;
|
||||||
m_expectedZero = expectedZero;
|
m_expectedZero = expectedZero;
|
||||||
|
|
||||||
SendableRegistry.addLW(this, "Analog Encoder", m_analogInput.getChannel());
|
SendableRegistry.add(this, "Analog Encoder", m_analogInput.getChannel());
|
||||||
}
|
}
|
||||||
|
|
||||||
private double mapSensorRange(double pos) {
|
private double mapSensorRange(double pos) {
|
||||||
|
|||||||
@@ -29,7 +29,7 @@ public class AnalogGyro implements Sendable, AutoCloseable {
|
|||||||
/** Initialize the gyro. Calibration is handled by calibrate(). */
|
/** Initialize the gyro. Calibration is handled by calibrate(). */
|
||||||
private void initGyro() {
|
private void initGyro() {
|
||||||
HAL.report(tResourceType.kResourceType_Gyro, m_analog.getChannel() + 1);
|
HAL.report(tResourceType.kResourceType_Gyro, m_analog.getChannel() + 1);
|
||||||
SendableRegistry.addLW(this, "AnalogGyro", m_analog.getChannel());
|
SendableRegistry.add(this, "AnalogGyro", m_analog.getChannel());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -42,7 +42,7 @@ public class AnalogInput implements Sendable, AutoCloseable {
|
|||||||
m_port = AnalogJNI.initializeAnalogInputPort(portHandle);
|
m_port = AnalogJNI.initializeAnalogInputPort(portHandle);
|
||||||
|
|
||||||
HAL.report(tResourceType.kResourceType_AnalogChannel, channel + 1);
|
HAL.report(tResourceType.kResourceType_AnalogChannel, channel + 1);
|
||||||
SendableRegistry.addLW(this, "AnalogInput", channel);
|
SendableRegistry.add(this, "AnalogInput", channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|||||||
@@ -56,7 +56,7 @@ public class AnalogPotentiometer implements Sendable, AutoCloseable {
|
|||||||
*/
|
*/
|
||||||
@SuppressWarnings("this-escape")
|
@SuppressWarnings("this-escape")
|
||||||
public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) {
|
public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) {
|
||||||
SendableRegistry.addLW(this, "AnalogPotentiometer", input.getChannel());
|
SendableRegistry.add(this, "AnalogPotentiometer", input.getChannel());
|
||||||
m_analogInput = input;
|
m_analogInput = input;
|
||||||
m_initAnalogInput = false;
|
m_initAnalogInput = false;
|
||||||
|
|
||||||
|
|||||||
@@ -52,7 +52,7 @@ public class AnalogTrigger implements Sendable, AutoCloseable {
|
|||||||
int index = getIndex();
|
int index = getIndex();
|
||||||
|
|
||||||
HAL.report(tResourceType.kResourceType_AnalogTrigger, index + 1);
|
HAL.report(tResourceType.kResourceType_AnalogTrigger, index + 1);
|
||||||
SendableRegistry.addLW(this, "AnalogTrigger", index);
|
SendableRegistry.add(this, "AnalogTrigger", index);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -69,7 +69,7 @@ public class AnalogTrigger implements Sendable, AutoCloseable {
|
|||||||
int index = getIndex();
|
int index = getIndex();
|
||||||
|
|
||||||
HAL.report(tResourceType.kResourceType_AnalogTrigger, index + 1);
|
HAL.report(tResourceType.kResourceType_AnalogTrigger, index + 1);
|
||||||
SendableRegistry.addLW(this, "AnalogTrigger", index);
|
SendableRegistry.add(this, "AnalogTrigger", index);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|||||||
@@ -45,7 +45,7 @@ public class Compressor implements Sendable, AutoCloseable {
|
|||||||
m_module.enableCompressorDigital();
|
m_module.enableCompressorDigital();
|
||||||
|
|
||||||
HAL.report(tResourceType.kResourceType_Compressor, module + 1);
|
HAL.report(tResourceType.kResourceType_Compressor, module + 1);
|
||||||
SendableRegistry.addLW(this, "Compressor", module);
|
SendableRegistry.add(this, "Compressor", module);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -85,7 +85,7 @@ public class Counter implements CounterBase, Sendable, AutoCloseable {
|
|||||||
setMaxPeriod(0.5);
|
setMaxPeriod(0.5);
|
||||||
|
|
||||||
HAL.report(tResourceType.kResourceType_Counter, m_index + 1, mode.value + 1);
|
HAL.report(tResourceType.kResourceType_Counter, m_index + 1, mode.value + 1);
|
||||||
SendableRegistry.addLW(this, "Counter", m_index);
|
SendableRegistry.add(this, "Counter", m_index);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -35,7 +35,7 @@ public class DigitalInput extends DigitalSource implements Sendable {
|
|||||||
m_handle = DIOJNI.initializeDIOPort(HAL.getPort((byte) channel), true);
|
m_handle = DIOJNI.initializeDIOPort(HAL.getPort((byte) channel), true);
|
||||||
|
|
||||||
HAL.report(tResourceType.kResourceType_DigitalInput, channel + 1);
|
HAL.report(tResourceType.kResourceType_DigitalInput, channel + 1);
|
||||||
SendableRegistry.addLW(this, "DigitalInput", channel);
|
SendableRegistry.add(this, "DigitalInput", channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|||||||
@@ -37,7 +37,7 @@ public class DigitalOutput extends DigitalSource implements Sendable {
|
|||||||
m_handle = DIOJNI.initializeDIOPort(HAL.getPort((byte) channel), false);
|
m_handle = DIOJNI.initializeDIOPort(HAL.getPort((byte) channel), false);
|
||||||
|
|
||||||
HAL.report(tResourceType.kResourceType_DigitalOutput, channel + 1);
|
HAL.report(tResourceType.kResourceType_DigitalOutput, channel + 1);
|
||||||
SendableRegistry.addLW(this, "DigitalOutput", channel);
|
SendableRegistry.add(this, "DigitalOutput", channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|||||||
@@ -99,7 +99,7 @@ public class DoubleSolenoid implements Sendable, AutoCloseable {
|
|||||||
tResourceType.kResourceType_Solenoid, forwardChannel + 1, m_module.getModuleNumber() + 1);
|
tResourceType.kResourceType_Solenoid, forwardChannel + 1, m_module.getModuleNumber() + 1);
|
||||||
HAL.report(
|
HAL.report(
|
||||||
tResourceType.kResourceType_Solenoid, reverseChannel + 1, m_module.getModuleNumber() + 1);
|
tResourceType.kResourceType_Solenoid, reverseChannel + 1, m_module.getModuleNumber() + 1);
|
||||||
SendableRegistry.addLW(this, "DoubleSolenoid", m_module.getModuleNumber(), forwardChannel);
|
SendableRegistry.add(this, "DoubleSolenoid", m_module.getModuleNumber(), forwardChannel);
|
||||||
successfulCompletion = true;
|
successfulCompletion = true;
|
||||||
} finally {
|
} finally {
|
||||||
if (!successfulCompletion) {
|
if (!successfulCompletion) {
|
||||||
@@ -210,7 +210,6 @@ public class DoubleSolenoid implements Sendable, AutoCloseable {
|
|||||||
public void initSendable(SendableBuilder builder) {
|
public void initSendable(SendableBuilder builder) {
|
||||||
builder.setSmartDashboardType("Double Solenoid");
|
builder.setSmartDashboardType("Double Solenoid");
|
||||||
builder.setActuator(true);
|
builder.setActuator(true);
|
||||||
builder.setSafeState(() -> set(Value.kOff));
|
|
||||||
builder.addStringProperty(
|
builder.addStringProperty(
|
||||||
"Value",
|
"Value",
|
||||||
() -> get().name().substring(1),
|
() -> get().name().substring(1),
|
||||||
|
|||||||
@@ -36,7 +36,7 @@ public class DutyCycle implements Sendable, AutoCloseable {
|
|||||||
|
|
||||||
m_channel = channel;
|
m_channel = channel;
|
||||||
HAL.report(tResourceType.kResourceType_DutyCycle, channel + 1);
|
HAL.report(tResourceType.kResourceType_DutyCycle, channel + 1);
|
||||||
SendableRegistry.addLW(this, "Duty Cycle", channel);
|
SendableRegistry.add(this, "Duty Cycle", channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Close the DutyCycle and free all resources. */
|
/** Close the DutyCycle and free all resources. */
|
||||||
|
|||||||
@@ -93,7 +93,7 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
|
|||||||
m_fullRange = fullRange;
|
m_fullRange = fullRange;
|
||||||
m_expectedZero = expectedZero;
|
m_expectedZero = expectedZero;
|
||||||
|
|
||||||
SendableRegistry.addLW(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel());
|
SendableRegistry.add(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel());
|
||||||
}
|
}
|
||||||
|
|
||||||
private double mapSensorRange(double pos) {
|
private double mapSensorRange(double pos) {
|
||||||
|
|||||||
@@ -84,7 +84,7 @@ public class Encoder implements CounterBase, Sendable, AutoCloseable {
|
|||||||
|
|
||||||
int fpgaIndex = getFPGAIndex();
|
int fpgaIndex = getFPGAIndex();
|
||||||
HAL.report(tResourceType.kResourceType_Encoder, fpgaIndex + 1, type.value + 1);
|
HAL.report(tResourceType.kResourceType_Encoder, fpgaIndex + 1, type.value + 1);
|
||||||
SendableRegistry.addLW(this, "Encoder", fpgaIndex);
|
SendableRegistry.add(this, "Encoder", fpgaIndex);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -5,13 +5,9 @@
|
|||||||
package edu.wpi.first.wpilibj;
|
package edu.wpi.first.wpilibj;
|
||||||
|
|
||||||
import edu.wpi.first.hal.DriverStationJNI;
|
import edu.wpi.first.hal.DriverStationJNI;
|
||||||
import edu.wpi.first.hal.FRCNetComm.tInstances;
|
|
||||||
import edu.wpi.first.hal.FRCNetComm.tResourceType;
|
|
||||||
import edu.wpi.first.hal.HAL;
|
import edu.wpi.first.hal.HAL;
|
||||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||||
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
|
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||||
import java.util.ConcurrentModificationException;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* IterativeRobotBase implements a specific type of robot program framework, extending the RobotBase
|
* IterativeRobotBase implements a specific type of robot program framework, extending the RobotBase
|
||||||
@@ -71,7 +67,6 @@ public abstract class IterativeRobotBase extends RobotBase {
|
|||||||
private final double m_period;
|
private final double m_period;
|
||||||
private final Watchdog m_watchdog;
|
private final Watchdog m_watchdog;
|
||||||
private boolean m_ntFlushEnabled = true;
|
private boolean m_ntFlushEnabled = true;
|
||||||
private boolean m_lwEnabledInTest;
|
|
||||||
private boolean m_calledDsConnected;
|
private boolean m_calledDsConnected;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -259,34 +254,6 @@ public abstract class IterativeRobotBase extends RobotBase {
|
|||||||
m_ntFlushEnabled = enabled;
|
m_ntFlushEnabled = enabled;
|
||||||
}
|
}
|
||||||
|
|
||||||
private boolean m_reportedLw;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Sets whether LiveWindow operation is enabled during test mode. Calling
|
|
||||||
*
|
|
||||||
* @param testLW True to enable, false to disable. Defaults to false.
|
|
||||||
* @throws ConcurrentModificationException if this is called during test mode.
|
|
||||||
*/
|
|
||||||
public void enableLiveWindowInTest(boolean testLW) {
|
|
||||||
if (isTestEnabled()) {
|
|
||||||
throw new ConcurrentModificationException("Can't configure test mode while in test mode!");
|
|
||||||
}
|
|
||||||
if (!m_reportedLw && testLW) {
|
|
||||||
HAL.report(tResourceType.kResourceType_SmartDashboard, tInstances.kSmartDashboard_LiveWindow);
|
|
||||||
m_reportedLw = true;
|
|
||||||
}
|
|
||||||
m_lwEnabledInTest = testLW;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Whether LiveWindow operation is enabled during test mode.
|
|
||||||
*
|
|
||||||
* @return whether LiveWindow should be enabled in test mode.
|
|
||||||
*/
|
|
||||||
public boolean isLiveWindowEnabledInTest() {
|
|
||||||
return m_lwEnabledInTest;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Gets time period between calls to Periodic() functions.
|
* Gets time period between calls to Periodic() functions.
|
||||||
*
|
*
|
||||||
@@ -327,12 +294,7 @@ public abstract class IterativeRobotBase extends RobotBase {
|
|||||||
case kDisabled -> disabledExit();
|
case kDisabled -> disabledExit();
|
||||||
case kAutonomous -> autonomousExit();
|
case kAutonomous -> autonomousExit();
|
||||||
case kTeleop -> teleopExit();
|
case kTeleop -> teleopExit();
|
||||||
case kTest -> {
|
case kTest -> testExit();
|
||||||
if (m_lwEnabledInTest) {
|
|
||||||
LiveWindow.setEnabled(false);
|
|
||||||
}
|
|
||||||
testExit();
|
|
||||||
}
|
|
||||||
default -> {
|
default -> {
|
||||||
// NOP
|
// NOP
|
||||||
}
|
}
|
||||||
@@ -353,9 +315,6 @@ public abstract class IterativeRobotBase extends RobotBase {
|
|||||||
m_watchdog.addEpoch("teleopInit()");
|
m_watchdog.addEpoch("teleopInit()");
|
||||||
}
|
}
|
||||||
case kTest -> {
|
case kTest -> {
|
||||||
if (m_lwEnabledInTest) {
|
|
||||||
LiveWindow.setEnabled(true);
|
|
||||||
}
|
|
||||||
testInit();
|
testInit();
|
||||||
m_watchdog.addEpoch("testInit()");
|
m_watchdog.addEpoch("testInit()");
|
||||||
}
|
}
|
||||||
@@ -399,8 +358,6 @@ public abstract class IterativeRobotBase extends RobotBase {
|
|||||||
|
|
||||||
SmartDashboard.updateValues();
|
SmartDashboard.updateValues();
|
||||||
m_watchdog.addEpoch("SmartDashboard.updateValues()");
|
m_watchdog.addEpoch("SmartDashboard.updateValues()");
|
||||||
LiveWindow.updateValues();
|
|
||||||
m_watchdog.addEpoch("LiveWindow.updateValues()");
|
|
||||||
|
|
||||||
if (isSimulation()) {
|
if (isSimulation()) {
|
||||||
HAL.simPeriodicBefore();
|
HAL.simPeriodicBefore();
|
||||||
|
|||||||
@@ -40,7 +40,7 @@ public class PWM implements Sendable, AutoCloseable {
|
|||||||
* <p>Checks channel value range and allocates the appropriate channel. The allocation is only
|
* <p>Checks channel value range and allocates the appropriate channel. The allocation is only
|
||||||
* done to help users ensure that they don't double assign channels.
|
* done to help users ensure that they don't double assign channels.
|
||||||
*
|
*
|
||||||
* <p>By default, adds itself to SendableRegistry and LiveWindow.
|
* <p>By default, adds itself to SendableRegistry.
|
||||||
*
|
*
|
||||||
* @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
|
* @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
|
||||||
*/
|
*/
|
||||||
@@ -52,7 +52,7 @@ public class PWM implements Sendable, AutoCloseable {
|
|||||||
* Allocate a PWM given a channel.
|
* Allocate a PWM given a channel.
|
||||||
*
|
*
|
||||||
* @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
|
* @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
|
||||||
* @param registerSendable If true, adds this instance to SendableRegistry and LiveWindow
|
* @param registerSendable If true, adds this instance to SendableRegistry
|
||||||
*/
|
*/
|
||||||
@SuppressWarnings("this-escape")
|
@SuppressWarnings("this-escape")
|
||||||
public PWM(final int channel, final boolean registerSendable) {
|
public PWM(final int channel, final boolean registerSendable) {
|
||||||
@@ -67,7 +67,7 @@ public class PWM implements Sendable, AutoCloseable {
|
|||||||
|
|
||||||
HAL.report(tResourceType.kResourceType_PWM, channel + 1);
|
HAL.report(tResourceType.kResourceType_PWM, channel + 1);
|
||||||
if (registerSendable) {
|
if (registerSendable) {
|
||||||
SendableRegistry.addLW(this, "PWM", channel);
|
SendableRegistry.add(this, "PWM", channel);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -244,7 +244,6 @@ public class PWM implements Sendable, AutoCloseable {
|
|||||||
public void initSendable(SendableBuilder builder) {
|
public void initSendable(SendableBuilder builder) {
|
||||||
builder.setSmartDashboardType("PWM");
|
builder.setSmartDashboardType("PWM");
|
||||||
builder.setActuator(true);
|
builder.setActuator(true);
|
||||||
builder.setSafeState(this::setDisabled);
|
|
||||||
builder.addDoubleProperty(
|
builder.addDoubleProperty(
|
||||||
"Value", this::getPulseTimeMicroseconds, value -> setPulseTimeMicroseconds((int) value));
|
"Value", this::getPulseTimeMicroseconds, value -> setPulseTimeMicroseconds((int) value));
|
||||||
builder.addDoubleProperty("Speed", this::getSpeed, this::setSpeed);
|
builder.addDoubleProperty("Speed", this::getSpeed, this::setSpeed);
|
||||||
|
|||||||
@@ -57,7 +57,7 @@ public class PowerDistribution implements Sendable, AutoCloseable {
|
|||||||
} else {
|
} else {
|
||||||
HAL.report(tResourceType.kResourceType_PDP, tInstances.kPDP_REV);
|
HAL.report(tResourceType.kResourceType_PDP, tInstances.kPDP_REV);
|
||||||
}
|
}
|
||||||
SendableRegistry.addLW(this, "PowerDistribution", m_module);
|
SendableRegistry.add(this, "PowerDistribution", m_module);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -76,7 +76,7 @@ public class PowerDistribution implements Sendable, AutoCloseable {
|
|||||||
HAL.report(tResourceType.kResourceType_PDP, tInstances.kPDP_REV);
|
HAL.report(tResourceType.kResourceType_PDP, tInstances.kPDP_REV);
|
||||||
}
|
}
|
||||||
|
|
||||||
SendableRegistry.addLW(this, "PowerDistribution", m_module);
|
SendableRegistry.add(this, "PowerDistribution", m_module);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|||||||
@@ -17,7 +17,6 @@ import edu.wpi.first.networktables.MultiSubscriber;
|
|||||||
import edu.wpi.first.networktables.NetworkTableEvent;
|
import edu.wpi.first.networktables.NetworkTableEvent;
|
||||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||||
import edu.wpi.first.util.WPIUtilJNI;
|
import edu.wpi.first.util.WPIUtilJNI;
|
||||||
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
|
|
||||||
import edu.wpi.first.wpilibj.util.WPILibVersion;
|
import edu.wpi.first.wpilibj.util.WPILibVersion;
|
||||||
import java.io.File;
|
import java.io.File;
|
||||||
import java.io.IOException;
|
import java.io.IOException;
|
||||||
@@ -224,8 +223,6 @@ public abstract class RobotBase implements AutoCloseable {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
});
|
});
|
||||||
|
|
||||||
LiveWindow.setEnabled(false);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -89,7 +89,7 @@ public class SharpIR implements Sendable, AutoCloseable {
|
|||||||
m_minCM = minCM;
|
m_minCM = minCM;
|
||||||
m_maxCM = maxCM;
|
m_maxCM = maxCM;
|
||||||
|
|
||||||
SendableRegistry.addLW(this, "SharpIR", channel);
|
SendableRegistry.add(this, "SharpIR", channel);
|
||||||
|
|
||||||
m_simDevice = SimDevice.create("SharpIR", m_sensor.getChannel());
|
m_simDevice = SimDevice.create("SharpIR", m_sensor.getChannel());
|
||||||
if (m_simDevice != null) {
|
if (m_simDevice != null) {
|
||||||
|
|||||||
@@ -57,7 +57,7 @@ public class Solenoid implements Sendable, AutoCloseable {
|
|||||||
}
|
}
|
||||||
|
|
||||||
HAL.report(tResourceType.kResourceType_Solenoid, channel + 1, m_module.getModuleNumber() + 1);
|
HAL.report(tResourceType.kResourceType_Solenoid, channel + 1, m_module.getModuleNumber() + 1);
|
||||||
SendableRegistry.addLW(this, "Solenoid", m_module.getModuleNumber(), channel);
|
SendableRegistry.add(this, "Solenoid", m_module.getModuleNumber(), channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
@@ -148,7 +148,6 @@ public class Solenoid implements Sendable, AutoCloseable {
|
|||||||
public void initSendable(SendableBuilder builder) {
|
public void initSendable(SendableBuilder builder) {
|
||||||
builder.setSmartDashboardType("Solenoid");
|
builder.setSmartDashboardType("Solenoid");
|
||||||
builder.setActuator(true);
|
builder.setActuator(true);
|
||||||
builder.setSafeState(() -> set(false));
|
|
||||||
builder.addBooleanProperty("Value", this::get, this::set);
|
builder.addBooleanProperty("Value", this::get, this::set);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -107,7 +107,7 @@ public class Ultrasonic implements Sendable, AutoCloseable {
|
|||||||
|
|
||||||
m_instances++;
|
m_instances++;
|
||||||
HAL.report(tResourceType.kResourceType_Ultrasonic, m_instances);
|
HAL.report(tResourceType.kResourceType_Ultrasonic, m_instances);
|
||||||
SendableRegistry.addLW(this, "Ultrasonic", m_echoChannel.getChannel());
|
SendableRegistry.add(this, "Ultrasonic", m_echoChannel.getChannel());
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -60,7 +60,7 @@ public class ExternalDirectionCounter implements Sendable, AutoCloseable {
|
|||||||
|
|
||||||
int intIndex = index.getInt();
|
int intIndex = index.getInt();
|
||||||
HAL.report(tResourceType.kResourceType_Counter, intIndex + 1);
|
HAL.report(tResourceType.kResourceType_Counter, intIndex + 1);
|
||||||
SendableRegistry.addLW(this, "External Direction Counter", intIndex);
|
SendableRegistry.add(this, "External Direction Counter", intIndex);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|||||||
@@ -49,7 +49,7 @@ public class Tachometer implements Sendable, AutoCloseable {
|
|||||||
|
|
||||||
int intIndex = index.getInt();
|
int intIndex = index.getInt();
|
||||||
HAL.report(tResourceType.kResourceType_Counter, intIndex + 1);
|
HAL.report(tResourceType.kResourceType_Counter, intIndex + 1);
|
||||||
SendableRegistry.addLW(this, "Tachometer", intIndex);
|
SendableRegistry.add(this, "Tachometer", intIndex);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|||||||
@@ -63,7 +63,7 @@ public class UpDownCounter implements Sendable, AutoCloseable {
|
|||||||
|
|
||||||
int intIndex = index.getInt();
|
int intIndex = index.getInt();
|
||||||
HAL.report(tResourceType.kResourceType_Counter, intIndex + 1);
|
HAL.report(tResourceType.kResourceType_Counter, intIndex + 1);
|
||||||
SendableRegistry.addLW(this, "UpDown Counter", intIndex);
|
SendableRegistry.add(this, "UpDown Counter", intIndex);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
|
|||||||
@@ -127,7 +127,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
|||||||
m_leftMotor = leftMotor;
|
m_leftMotor = leftMotor;
|
||||||
m_rightMotor = rightMotor;
|
m_rightMotor = rightMotor;
|
||||||
instances++;
|
instances++;
|
||||||
SendableRegistry.addLW(this, "DifferentialDrive", instances);
|
SendableRegistry.add(this, "DifferentialDrive", instances);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
@@ -367,7 +367,6 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
|
|||||||
public void initSendable(SendableBuilder builder) {
|
public void initSendable(SendableBuilder builder) {
|
||||||
builder.setSmartDashboardType("DifferentialDrive");
|
builder.setSmartDashboardType("DifferentialDrive");
|
||||||
builder.setActuator(true);
|
builder.setActuator(true);
|
||||||
builder.setSafeState(this::stopMotor);
|
|
||||||
builder.addDoubleProperty("Left Motor Speed", () -> m_leftOutput, m_leftMotor);
|
builder.addDoubleProperty("Left Motor Speed", () -> m_leftOutput, m_leftMotor);
|
||||||
builder.addDoubleProperty("Right Motor Speed", () -> m_rightOutput, m_rightMotor);
|
builder.addDoubleProperty("Right Motor Speed", () -> m_rightOutput, m_rightMotor);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -158,7 +158,7 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
|
|||||||
m_frontRightMotor = frontRightMotor;
|
m_frontRightMotor = frontRightMotor;
|
||||||
m_rearRightMotor = rearRightMotor;
|
m_rearRightMotor = rearRightMotor;
|
||||||
instances++;
|
instances++;
|
||||||
SendableRegistry.addLW(this, "MecanumDrive", instances);
|
SendableRegistry.add(this, "MecanumDrive", instances);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
@@ -317,7 +317,6 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
|
|||||||
public void initSendable(SendableBuilder builder) {
|
public void initSendable(SendableBuilder builder) {
|
||||||
builder.setSmartDashboardType("MecanumDrive");
|
builder.setSmartDashboardType("MecanumDrive");
|
||||||
builder.setActuator(true);
|
builder.setActuator(true);
|
||||||
builder.setSafeState(this::stopMotor);
|
|
||||||
builder.addDoubleProperty("Front Left Motor Speed", () -> m_frontLeftOutput, m_frontLeftMotor);
|
builder.addDoubleProperty("Front Left Motor Speed", () -> m_frontLeftOutput, m_frontLeftMotor);
|
||||||
builder.addDoubleProperty(
|
builder.addDoubleProperty(
|
||||||
"Front Right Motor Speed", () -> m_frontRightOutput, m_frontRightMotor);
|
"Front Right Motor Speed", () -> m_frontRightOutput, m_frontRightMotor);
|
||||||
|
|||||||
@@ -1,247 +0,0 @@
|
|||||||
// Copyright (c) FIRST and other WPILib contributors.
|
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
|
||||||
// the WPILib BSD license file in the root directory of this project.
|
|
||||||
|
|
||||||
package edu.wpi.first.wpilibj.livewindow;
|
|
||||||
|
|
||||||
import edu.wpi.first.networktables.BooleanPublisher;
|
|
||||||
import edu.wpi.first.networktables.NetworkTable;
|
|
||||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
|
||||||
import edu.wpi.first.networktables.StringPublisher;
|
|
||||||
import edu.wpi.first.networktables.StringTopic;
|
|
||||||
import edu.wpi.first.util.sendable.Sendable;
|
|
||||||
import edu.wpi.first.util.sendable.SendableRegistry;
|
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* The LiveWindow class is the public interface for putting sensors and actuators on the LiveWindow.
|
|
||||||
*/
|
|
||||||
public final class LiveWindow {
|
|
||||||
private static final class Component implements AutoCloseable {
|
|
||||||
@Override
|
|
||||||
public void close() {
|
|
||||||
if (m_namePub != null) {
|
|
||||||
m_namePub.close();
|
|
||||||
m_namePub = null;
|
|
||||||
}
|
|
||||||
if (m_typePub != null) {
|
|
||||||
m_typePub.close();
|
|
||||||
m_typePub = null;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
boolean m_firstTime = true;
|
|
||||||
boolean m_telemetryEnabled;
|
|
||||||
StringPublisher m_namePub;
|
|
||||||
StringPublisher m_typePub;
|
|
||||||
}
|
|
||||||
|
|
||||||
private static final String kSmartDashboardType = "LW Subsystem";
|
|
||||||
|
|
||||||
private static final int dataHandle = SendableRegistry.getDataHandle();
|
|
||||||
private static final NetworkTable liveWindowTable =
|
|
||||||
NetworkTableInstance.getDefault().getTable("LiveWindow");
|
|
||||||
private static final NetworkTable statusTable = liveWindowTable.getSubTable(".status");
|
|
||||||
private static final BooleanPublisher enabledPub =
|
|
||||||
statusTable.getBooleanTopic("LW Enabled").publish();
|
|
||||||
private static boolean startLiveWindow;
|
|
||||||
private static boolean liveWindowEnabled;
|
|
||||||
private static boolean telemetryEnabled;
|
|
||||||
|
|
||||||
private static Runnable enabledListener;
|
|
||||||
private static Runnable disabledListener;
|
|
||||||
|
|
||||||
static {
|
|
||||||
SendableRegistry.setLiveWindowBuilderFactory(SendableBuilderImpl::new);
|
|
||||||
enabledPub.set(false);
|
|
||||||
}
|
|
||||||
|
|
||||||
private static Component getOrAdd(Sendable sendable) {
|
|
||||||
Component data = (Component) SendableRegistry.getData(sendable, dataHandle);
|
|
||||||
if (data == null) {
|
|
||||||
data = new Component();
|
|
||||||
SendableRegistry.setData(sendable, dataHandle, data);
|
|
||||||
}
|
|
||||||
return data;
|
|
||||||
}
|
|
||||||
|
|
||||||
private LiveWindow() {
|
|
||||||
throw new UnsupportedOperationException("This is a utility class!");
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Sets function to be called when LiveWindow is enabled.
|
|
||||||
*
|
|
||||||
* @param runnable function (or null for none)
|
|
||||||
*/
|
|
||||||
public static synchronized void setEnabledListener(Runnable runnable) {
|
|
||||||
enabledListener = runnable;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Sets function to be called when LiveWindow is disabled.
|
|
||||||
*
|
|
||||||
* @param runnable function (or null for none)
|
|
||||||
*/
|
|
||||||
public static synchronized void setDisabledListener(Runnable runnable) {
|
|
||||||
disabledListener = runnable;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Returns true if LiveWindow is enabled.
|
|
||||||
*
|
|
||||||
* @return True if LiveWindow is enabled.
|
|
||||||
*/
|
|
||||||
public static synchronized boolean isEnabled() {
|
|
||||||
return liveWindowEnabled;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Set the enabled state of LiveWindow.
|
|
||||||
*
|
|
||||||
* <p>If it's being enabled, turn off the scheduler and remove all the commands from the queue and
|
|
||||||
* enable all the components registered for LiveWindow. If it's being disabled, stop all the
|
|
||||||
* registered components and re-enable the scheduler.
|
|
||||||
*
|
|
||||||
* <p>TODO: add code to disable PID loops when enabling LiveWindow. The commands should re-enable
|
|
||||||
* the PID loops themselves when they get rescheduled. This prevents arms from starting to move
|
|
||||||
* around, etc. after a period of adjusting them in LiveWindow mode.
|
|
||||||
*
|
|
||||||
* @param enabled True to enable LiveWindow.
|
|
||||||
*/
|
|
||||||
public static synchronized void setEnabled(boolean enabled) {
|
|
||||||
if (liveWindowEnabled != enabled) {
|
|
||||||
startLiveWindow = enabled;
|
|
||||||
liveWindowEnabled = enabled;
|
|
||||||
updateValues(); // Force table generation now to make sure everything is defined
|
|
||||||
if (enabled) {
|
|
||||||
System.out.println("Starting live window mode.");
|
|
||||||
if (enabledListener != null) {
|
|
||||||
enabledListener.run();
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
System.out.println("stopping live window mode.");
|
|
||||||
SendableRegistry.foreachLiveWindow(
|
|
||||||
dataHandle, cbdata -> ((SendableBuilderImpl) cbdata.builder).stopLiveWindowMode());
|
|
||||||
if (disabledListener != null) {
|
|
||||||
disabledListener.run();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
enabledPub.set(enabled);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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;
|
|
||||||
getOrAdd(sendable).m_telemetryEnabled = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Disable telemetry for a single component.
|
|
||||||
*
|
|
||||||
* @param sendable component
|
|
||||||
*/
|
|
||||||
public static synchronized void disableTelemetry(Sendable sendable) {
|
|
||||||
getOrAdd(sendable).m_telemetryEnabled = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Disable ALL telemetry. */
|
|
||||||
public static synchronized void disableAllTelemetry() {
|
|
||||||
telemetryEnabled = false;
|
|
||||||
SendableRegistry.foreachLiveWindow(
|
|
||||||
dataHandle,
|
|
||||||
cbdata -> {
|
|
||||||
if (cbdata.data == null) {
|
|
||||||
cbdata.data = new Component();
|
|
||||||
}
|
|
||||||
((Component) cbdata.data).m_telemetryEnabled = false;
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Enable ALL telemetry. */
|
|
||||||
public static synchronized void enableAllTelemetry() {
|
|
||||||
telemetryEnabled = true;
|
|
||||||
SendableRegistry.foreachLiveWindow(
|
|
||||||
dataHandle,
|
|
||||||
cbdata -> {
|
|
||||||
if (cbdata.data == null) {
|
|
||||||
cbdata.data = new Component();
|
|
||||||
}
|
|
||||||
((Component) cbdata.data).m_telemetryEnabled = true;
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Tell all the sensors to update (send) their values.
|
|
||||||
*
|
|
||||||
* <p>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;
|
|
||||||
}
|
|
||||||
|
|
||||||
SendableRegistry.foreachLiveWindow(
|
|
||||||
dataHandle,
|
|
||||||
cbdata -> {
|
|
||||||
if (cbdata.sendable == null || cbdata.parent != null) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (cbdata.data == null) {
|
|
||||||
cbdata.data = new Component();
|
|
||||||
}
|
|
||||||
|
|
||||||
Component component = (Component) cbdata.data;
|
|
||||||
|
|
||||||
if (!liveWindowEnabled && !component.m_telemetryEnabled) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
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.
|
|
||||||
if (cbdata.name.isEmpty()) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
NetworkTable ssTable = liveWindowTable.getSubTable(cbdata.subsystem);
|
|
||||||
NetworkTable table;
|
|
||||||
// Treat name==subsystem as top level of subsystem
|
|
||||||
if (cbdata.name.equals(cbdata.subsystem)) {
|
|
||||||
table = ssTable;
|
|
||||||
} else {
|
|
||||||
table = ssTable.getSubTable(cbdata.name);
|
|
||||||
}
|
|
||||||
component.m_namePub = new StringTopic(table.getTopic(".name")).publish();
|
|
||||||
component.m_namePub.set(cbdata.name);
|
|
||||||
((SendableBuilderImpl) cbdata.builder).setTable(table);
|
|
||||||
cbdata.sendable.initSendable(cbdata.builder);
|
|
||||||
component.m_typePub =
|
|
||||||
new StringTopic(ssTable.getTopic(".type"))
|
|
||||||
.publishEx(
|
|
||||||
StringTopic.kTypeString,
|
|
||||||
"{\"SmartDashboard\":\"" + kSmartDashboardType + "\"}");
|
|
||||||
component.m_typePub.set(kSmartDashboardType);
|
|
||||||
|
|
||||||
component.m_firstTime = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (startLiveWindow) {
|
|
||||||
((SendableBuilderImpl) cbdata.builder).startLiveWindowMode();
|
|
||||||
}
|
|
||||||
cbdata.builder.update();
|
|
||||||
});
|
|
||||||
|
|
||||||
startLiveWindow = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -53,7 +53,7 @@ public class MotorControllerGroup implements MotorController, Sendable, AutoClos
|
|||||||
SendableRegistry.addChild(this, controller);
|
SendableRegistry.addChild(this, controller);
|
||||||
}
|
}
|
||||||
instances++;
|
instances++;
|
||||||
SendableRegistry.addLW(this, "MotorControllerGroup", instances);
|
SendableRegistry.add(this, "MotorControllerGroup", instances);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
@@ -111,7 +111,6 @@ public class MotorControllerGroup implements MotorController, Sendable, AutoClos
|
|||||||
public void initSendable(SendableBuilder builder) {
|
public void initSendable(SendableBuilder builder) {
|
||||||
builder.setSmartDashboardType("Motor Controller");
|
builder.setSmartDashboardType("Motor Controller");
|
||||||
builder.setActuator(true);
|
builder.setActuator(true);
|
||||||
builder.setSafeState(this::stopMotor);
|
|
||||||
builder.addDoubleProperty("Value", this::get, this::set);
|
builder.addDoubleProperty("Value", this::get, this::set);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -46,7 +46,7 @@ public class NidecBrushless extends MotorSafety
|
|||||||
SendableRegistry.addChild(this, m_pwm);
|
SendableRegistry.addChild(this, m_pwm);
|
||||||
|
|
||||||
HAL.report(tResourceType.kResourceType_NidecBrushless, pwmChannel + 1);
|
HAL.report(tResourceType.kResourceType_NidecBrushless, pwmChannel + 1);
|
||||||
SendableRegistry.addLW(this, "Nidec Brushless", pwmChannel);
|
SendableRegistry.add(this, "Nidec Brushless", pwmChannel);
|
||||||
}
|
}
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
@@ -139,7 +139,6 @@ public class NidecBrushless extends MotorSafety
|
|||||||
public void initSendable(SendableBuilder builder) {
|
public void initSendable(SendableBuilder builder) {
|
||||||
builder.setSmartDashboardType("Nidec Brushless");
|
builder.setSmartDashboardType("Nidec Brushless");
|
||||||
builder.setActuator(true);
|
builder.setActuator(true);
|
||||||
builder.setSafeState(this::stopMotor);
|
|
||||||
builder.addDoubleProperty("Value", this::get, this::set);
|
builder.addDoubleProperty("Value", this::get, this::set);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -32,7 +32,7 @@ public abstract class PWMMotorController extends MotorSafety
|
|||||||
@SuppressWarnings("this-escape")
|
@SuppressWarnings("this-escape")
|
||||||
protected PWMMotorController(final String name, final int channel) {
|
protected PWMMotorController(final String name, final int channel) {
|
||||||
m_pwm = new PWM(channel, false);
|
m_pwm = new PWM(channel, false);
|
||||||
SendableRegistry.addLW(this, name, channel);
|
SendableRegistry.add(this, name, channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Free the resource associated with the PWM channel and set the value to 0. */
|
/** Free the resource associated with the PWM channel and set the value to 0. */
|
||||||
@@ -161,7 +161,6 @@ public abstract class PWMMotorController extends MotorSafety
|
|||||||
public void initSendable(SendableBuilder builder) {
|
public void initSendable(SendableBuilder builder) {
|
||||||
builder.setSmartDashboardType("Motor Controller");
|
builder.setSmartDashboardType("Motor Controller");
|
||||||
builder.setActuator(true);
|
builder.setActuator(true);
|
||||||
builder.setSafeState(this::disable);
|
|
||||||
builder.addDoubleProperty("Value", this::get, this::set);
|
builder.addDoubleProperty("Value", this::get, this::set);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -98,7 +98,6 @@ public class SendableBuilderImpl implements NTSendableBuilder {
|
|||||||
}
|
}
|
||||||
|
|
||||||
private final List<Property<?, ?>> m_properties = new ArrayList<>();
|
private final List<Property<?, ?>> m_properties = new ArrayList<>();
|
||||||
private Runnable m_safeState;
|
|
||||||
private final List<Runnable> m_updateTables = new ArrayList<>();
|
private final List<Runnable> m_updateTables = new ArrayList<>();
|
||||||
private NetworkTable m_table;
|
private NetworkTable m_table;
|
||||||
private boolean m_controllable;
|
private boolean m_controllable;
|
||||||
@@ -205,28 +204,6 @@ public class SendableBuilderImpl implements NTSendableBuilder {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* Start LiveWindow mode by hooking the setters for all properties. Also calls the safeState
|
|
||||||
* function if one was provided.
|
|
||||||
*/
|
|
||||||
public void startLiveWindowMode() {
|
|
||||||
if (m_safeState != null) {
|
|
||||||
m_safeState.run();
|
|
||||||
}
|
|
||||||
startListeners();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Stop LiveWindow mode by unhooking the setters for all properties. Also calls the safeState
|
|
||||||
* function if one was provided.
|
|
||||||
*/
|
|
||||||
public void stopLiveWindowMode() {
|
|
||||||
stopListeners();
|
|
||||||
if (m_safeState != null) {
|
|
||||||
m_safeState.run();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/** Clear properties. */
|
/** Clear properties. */
|
||||||
@Override
|
@Override
|
||||||
public void clearProperties() {
|
public void clearProperties() {
|
||||||
@@ -274,17 +251,6 @@ public class SendableBuilderImpl implements NTSendableBuilder {
|
|||||||
m_actuator = value;
|
m_actuator = value;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* 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
|
* 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
|
* properties. Note this function is not passed the network table object; instead it should use
|
||||||
|
|||||||
@@ -5,20 +5,14 @@
|
|||||||
package edu.wpi.first.wpilibj;
|
package edu.wpi.first.wpilibj;
|
||||||
|
|
||||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||||
import static org.junit.jupiter.api.Assertions.assertFalse;
|
|
||||||
import static org.junit.jupiter.api.Assertions.assertThrows;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
|
|
||||||
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
|
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
|
||||||
import edu.wpi.first.wpilibj.simulation.SimHooks;
|
import edu.wpi.first.wpilibj.simulation.SimHooks;
|
||||||
import java.util.ConcurrentModificationException;
|
|
||||||
import java.util.concurrent.atomic.AtomicInteger;
|
import java.util.concurrent.atomic.AtomicInteger;
|
||||||
import org.junit.jupiter.api.AfterEach;
|
import org.junit.jupiter.api.AfterEach;
|
||||||
import org.junit.jupiter.api.BeforeEach;
|
import org.junit.jupiter.api.BeforeEach;
|
||||||
import org.junit.jupiter.api.Test;
|
import org.junit.jupiter.api.Test;
|
||||||
import org.junit.jupiter.api.parallel.ResourceLock;
|
import org.junit.jupiter.api.parallel.ResourceLock;
|
||||||
import org.junit.jupiter.params.ParameterizedTest;
|
|
||||||
import org.junit.jupiter.params.provider.ValueSource;
|
|
||||||
|
|
||||||
class TimedRobotTest {
|
class TimedRobotTest {
|
||||||
static final double kPeriod = 0.02;
|
static final double kPeriod = 0.02;
|
||||||
@@ -389,12 +383,10 @@ class TimedRobotTest {
|
|||||||
robot.close();
|
robot.close();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ValueSource(booleans = {true, false})
|
@Test
|
||||||
@ParameterizedTest
|
|
||||||
@ResourceLock("timing")
|
@ResourceLock("timing")
|
||||||
void testModeTest(boolean isLW) {
|
void testModeTest() {
|
||||||
MockRobot robot = new MockRobot();
|
MockRobot robot = new MockRobot();
|
||||||
robot.enableLiveWindowInTest(isLW);
|
|
||||||
|
|
||||||
Thread robotThread = new Thread(robot::startCompetition);
|
Thread robotThread = new Thread(robot::startCompetition);
|
||||||
robotThread.start();
|
robotThread.start();
|
||||||
@@ -411,7 +403,6 @@ class TimedRobotTest {
|
|||||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||||
assertEquals(0, robot.m_teleopInitCount.get());
|
assertEquals(0, robot.m_teleopInitCount.get());
|
||||||
assertEquals(0, robot.m_testInitCount.get());
|
assertEquals(0, robot.m_testInitCount.get());
|
||||||
assertFalse(LiveWindow.isEnabled());
|
|
||||||
|
|
||||||
assertEquals(0, robot.m_robotPeriodicCount.get());
|
assertEquals(0, robot.m_robotPeriodicCount.get());
|
||||||
assertEquals(0, robot.m_simulationPeriodicCount.get());
|
assertEquals(0, robot.m_simulationPeriodicCount.get());
|
||||||
@@ -454,9 +445,6 @@ class TimedRobotTest {
|
|||||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||||
assertEquals(0, robot.m_teleopInitCount.get());
|
assertEquals(0, robot.m_teleopInitCount.get());
|
||||||
assertEquals(1, robot.m_testInitCount.get());
|
assertEquals(1, robot.m_testInitCount.get());
|
||||||
assertEquals(isLW, LiveWindow.isEnabled());
|
|
||||||
|
|
||||||
assertThrows(ConcurrentModificationException.class, () -> robot.enableLiveWindowInTest(isLW));
|
|
||||||
|
|
||||||
assertEquals(2, robot.m_robotPeriodicCount.get());
|
assertEquals(2, robot.m_robotPeriodicCount.get());
|
||||||
assertEquals(2, robot.m_simulationPeriodicCount.get());
|
assertEquals(2, robot.m_simulationPeriodicCount.get());
|
||||||
@@ -483,7 +471,6 @@ class TimedRobotTest {
|
|||||||
assertEquals(0, robot.m_autonomousInitCount.get());
|
assertEquals(0, robot.m_autonomousInitCount.get());
|
||||||
assertEquals(0, robot.m_teleopInitCount.get());
|
assertEquals(0, robot.m_teleopInitCount.get());
|
||||||
assertEquals(1, robot.m_testInitCount.get());
|
assertEquals(1, robot.m_testInitCount.get());
|
||||||
assertFalse(LiveWindow.isEnabled());
|
|
||||||
|
|
||||||
assertEquals(3, robot.m_robotPeriodicCount.get());
|
assertEquals(3, robot.m_robotPeriodicCount.get());
|
||||||
assertEquals(3, robot.m_simulationPeriodicCount.get());
|
assertEquals(3, robot.m_simulationPeriodicCount.get());
|
||||||
|
|||||||
@@ -1,14 +0,0 @@
|
|||||||
// Copyright (c) FIRST and other WPILib contributors.
|
|
||||||
// Open Source Software; you can modify and/or share it under the terms of
|
|
||||||
// the WPILib BSD license file in the root directory of this project.
|
|
||||||
|
|
||||||
package edu.wpi.first.wpilibj.livewindow;
|
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.UtilityClassTest;
|
|
||||||
|
|
||||||
@SuppressWarnings("PMD.TestClassWithoutTestCases")
|
|
||||||
class LiveWindowTest extends UtilityClassTest<LiveWindow> {
|
|
||||||
LiveWindowTest() {
|
|
||||||
super(LiveWindow.class);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -5,7 +5,6 @@
|
|||||||
package edu.wpi.first.wpilibj.templates.timeslice;
|
package edu.wpi.first.wpilibj.templates.timeslice;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.TimesliceRobot;
|
import edu.wpi.first.wpilibj.TimesliceRobot;
|
||||||
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
|
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||||
|
|
||||||
@@ -25,10 +24,6 @@ public class Robot extends TimesliceRobot {
|
|||||||
// Run robot periodic() functions for 5 ms, and run controllers every 10 ms
|
// Run robot periodic() functions for 5 ms, and run controllers every 10 ms
|
||||||
super(0.005, 0.01);
|
super(0.005, 0.01);
|
||||||
|
|
||||||
// LiveWindow causes drastic overruns in robot periodic functions that will
|
|
||||||
// interfere with controllers
|
|
||||||
LiveWindow.disableAllTelemetry();
|
|
||||||
|
|
||||||
// Runs for 2 ms after robot periodic functions
|
// Runs for 2 ms after robot periodic functions
|
||||||
schedule(() -> {}, 0.002);
|
schedule(() -> {}, 0.002);
|
||||||
|
|
||||||
|
|||||||
@@ -5,7 +5,6 @@
|
|||||||
package edu.wpi.first.wpilibj.templates.timesliceskeleton;
|
package edu.wpi.first.wpilibj.templates.timesliceskeleton;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.TimesliceRobot;
|
import edu.wpi.first.wpilibj.TimesliceRobot;
|
||||||
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The methods in this class are called automatically corresponding to each mode, as described in
|
* The methods in this class are called automatically corresponding to each mode, as described in
|
||||||
@@ -18,10 +17,6 @@ public class Robot extends TimesliceRobot {
|
|||||||
// Run robot periodic() functions for 5 ms, and run controllers every 10 ms
|
// Run robot periodic() functions for 5 ms, and run controllers every 10 ms
|
||||||
super(0.005, 0.01);
|
super(0.005, 0.01);
|
||||||
|
|
||||||
// LiveWindow causes drastic overruns in robot periodic functions that will
|
|
||||||
// interfere with controllers
|
|
||||||
LiveWindow.disableAllTelemetry();
|
|
||||||
|
|
||||||
// Runs for 2 ms after robot periodic functions
|
// Runs for 2 ms after robot periodic functions
|
||||||
schedule(() -> {}, 0.002);
|
schedule(() -> {}, 0.002);
|
||||||
|
|
||||||
|
|||||||
@@ -43,7 +43,7 @@ public class BangBangController implements Sendable {
|
|||||||
|
|
||||||
setTolerance(tolerance);
|
setTolerance(tolerance);
|
||||||
|
|
||||||
SendableRegistry.addLW(this, "BangBangController", instances);
|
SendableRegistry.add(this, "BangBangController", instances);
|
||||||
|
|
||||||
MathSharedStore.reportUsage(MathUsageId.kController_BangBangController, instances);
|
MathSharedStore.reportUsage(MathUsageId.kController_BangBangController, instances);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -109,7 +109,7 @@ public class PIDController implements Sendable, AutoCloseable {
|
|||||||
m_period = period;
|
m_period = period;
|
||||||
|
|
||||||
instances++;
|
instances++;
|
||||||
SendableRegistry.addLW(this, "PIDController", instances);
|
SendableRegistry.add(this, "PIDController", instances);
|
||||||
|
|
||||||
MathSharedStore.reportUsage(MathUsageId.kController_PIDController2, instances);
|
MathSharedStore.reportUsage(MathUsageId.kController_PIDController2, instances);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -42,14 +42,6 @@ public interface SendableBuilder extends AutoCloseable {
|
|||||||
*/
|
*/
|
||||||
void setActuator(boolean value);
|
void setActuator(boolean value);
|
||||||
|
|
||||||
/**
|
|
||||||
* 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);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Add a boolean property.
|
* Add a boolean property.
|
||||||
*
|
*
|
||||||
|
|||||||
@@ -5,17 +5,13 @@
|
|||||||
package edu.wpi.first.util.sendable;
|
package edu.wpi.first.util.sendable;
|
||||||
|
|
||||||
import java.lang.ref.WeakReference;
|
import java.lang.ref.WeakReference;
|
||||||
import java.util.ArrayList;
|
|
||||||
import java.util.Arrays;
|
import java.util.Arrays;
|
||||||
import java.util.List;
|
|
||||||
import java.util.Map;
|
import java.util.Map;
|
||||||
import java.util.WeakHashMap;
|
import java.util.WeakHashMap;
|
||||||
import java.util.function.Consumer;
|
|
||||||
import java.util.function.Supplier;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* The SendableRegistry class is the public interface for registering sensors and actuators for use
|
* The SendableRegistry class is the public interface for registering sensors and actuators for use
|
||||||
* on dashboards and LiveWindow.
|
* on dashboards.
|
||||||
*/
|
*/
|
||||||
@SuppressWarnings("PMD.AvoidCatchingGenericException")
|
@SuppressWarnings("PMD.AvoidCatchingGenericException")
|
||||||
public final class SendableRegistry {
|
public final class SendableRegistry {
|
||||||
@@ -40,8 +36,6 @@ public final class SendableRegistry {
|
|||||||
SendableBuilder m_builder;
|
SendableBuilder m_builder;
|
||||||
String m_name;
|
String m_name;
|
||||||
String m_subsystem = "Ungrouped";
|
String m_subsystem = "Ungrouped";
|
||||||
WeakReference<Sendable> m_parent;
|
|
||||||
boolean m_liveWindow;
|
|
||||||
AutoCloseable[] m_data;
|
AutoCloseable[] m_data;
|
||||||
|
|
||||||
void setName(String moduleType, int channel) {
|
void setName(String moduleType, int channel) {
|
||||||
@@ -53,7 +47,6 @@ public final class SendableRegistry {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private static Supplier<SendableBuilder> liveWindowFactory;
|
|
||||||
private static final Map<Object, Component> components = new WeakHashMap<>();
|
private static final Map<Object, Component> components = new WeakHashMap<>();
|
||||||
private static int nextDataHandle;
|
private static int nextDataHandle;
|
||||||
|
|
||||||
@@ -74,15 +67,6 @@ public final class SendableRegistry {
|
|||||||
throw new UnsupportedOperationException("This is a utility class!");
|
throw new UnsupportedOperationException("This is a utility class!");
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* Sets the factory for LiveWindow builders.
|
|
||||||
*
|
|
||||||
* @param factory factory function
|
|
||||||
*/
|
|
||||||
public static synchronized void setLiveWindowBuilderFactory(Supplier<SendableBuilder> factory) {
|
|
||||||
liveWindowFactory = factory;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Adds an object to the registry.
|
* Adds an object to the registry.
|
||||||
*
|
*
|
||||||
@@ -133,100 +117,6 @@ public final class SendableRegistry {
|
|||||||
comp.m_subsystem = subsystem;
|
comp.m_subsystem = subsystem;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* Adds an object to the registry and LiveWindow.
|
|
||||||
*
|
|
||||||
* @param sendable object to add
|
|
||||||
* @param name component name
|
|
||||||
*/
|
|
||||||
public static synchronized void addLW(Sendable sendable, String name) {
|
|
||||||
Component comp = getOrAdd(sendable);
|
|
||||||
if (liveWindowFactory != null) {
|
|
||||||
if (comp.m_builder != null) {
|
|
||||||
try {
|
|
||||||
comp.m_builder.close();
|
|
||||||
} catch (Exception e) {
|
|
||||||
// ignore
|
|
||||||
}
|
|
||||||
}
|
|
||||||
comp.m_builder = liveWindowFactory.get();
|
|
||||||
}
|
|
||||||
comp.m_liveWindow = true;
|
|
||||||
comp.m_name = name;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Adds an object to the registry and LiveWindow.
|
|
||||||
*
|
|
||||||
* @param sendable object to add
|
|
||||||
* @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
|
|
||||||
*/
|
|
||||||
public static synchronized void addLW(Sendable sendable, String moduleType, int channel) {
|
|
||||||
Component comp = getOrAdd(sendable);
|
|
||||||
if (liveWindowFactory != null) {
|
|
||||||
if (comp.m_builder != null) {
|
|
||||||
try {
|
|
||||||
comp.m_builder.close();
|
|
||||||
} catch (Exception e) {
|
|
||||||
// ignore
|
|
||||||
}
|
|
||||||
}
|
|
||||||
comp.m_builder = liveWindowFactory.get();
|
|
||||||
}
|
|
||||||
comp.m_liveWindow = true;
|
|
||||||
comp.setName(moduleType, channel);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Adds an object to the registry and LiveWindow.
|
|
||||||
*
|
|
||||||
* @param sendable object to add
|
|
||||||
* @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
|
|
||||||
*/
|
|
||||||
public static synchronized void addLW(
|
|
||||||
Sendable sendable, String moduleType, int moduleNumber, int channel) {
|
|
||||||
Component comp = getOrAdd(sendable);
|
|
||||||
if (liveWindowFactory != null) {
|
|
||||||
if (comp.m_builder != null) {
|
|
||||||
try {
|
|
||||||
comp.m_builder.close();
|
|
||||||
} catch (Exception e) {
|
|
||||||
// ignore
|
|
||||||
}
|
|
||||||
}
|
|
||||||
comp.m_builder = liveWindowFactory.get();
|
|
||||||
}
|
|
||||||
comp.m_liveWindow = true;
|
|
||||||
comp.setName(moduleType, moduleNumber, channel);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Adds an object to the registry and LiveWindow.
|
|
||||||
*
|
|
||||||
* @param sendable object to add
|
|
||||||
* @param subsystem subsystem name
|
|
||||||
* @param name component name
|
|
||||||
*/
|
|
||||||
public static synchronized void addLW(Sendable sendable, String subsystem, String name) {
|
|
||||||
Component comp = getOrAdd(sendable);
|
|
||||||
if (liveWindowFactory != null) {
|
|
||||||
if (comp.m_builder != null) {
|
|
||||||
try {
|
|
||||||
comp.m_builder.close();
|
|
||||||
} catch (Exception e) {
|
|
||||||
// ignore
|
|
||||||
}
|
|
||||||
}
|
|
||||||
comp.m_builder = liveWindowFactory.get();
|
|
||||||
}
|
|
||||||
comp.m_liveWindow = true;
|
|
||||||
comp.m_name = name;
|
|
||||||
comp.m_subsystem = subsystem;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Adds a child object to an object. Adds the child object to the registry if it's not already
|
* Adds a child object to an object. Adds the child object to the registry if it's not already
|
||||||
* present.
|
* present.
|
||||||
@@ -240,7 +130,7 @@ public final class SendableRegistry {
|
|||||||
comp = new Component();
|
comp = new Component();
|
||||||
components.put(child, comp);
|
components.put(child, comp);
|
||||||
}
|
}
|
||||||
comp.m_parent = new WeakReference<>(parent);
|
// comp.m_parent = new WeakReference<>(parent);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@@ -430,30 +320,6 @@ public final class SendableRegistry {
|
|||||||
return comp.m_data[handle];
|
return comp.m_data[handle];
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* Enables LiveWindow for an object.
|
|
||||||
*
|
|
||||||
* @param sendable object
|
|
||||||
*/
|
|
||||||
public static synchronized void enableLiveWindow(Sendable sendable) {
|
|
||||||
Component comp = components.get(sendable);
|
|
||||||
if (comp != null) {
|
|
||||||
comp.m_liveWindow = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Disables LiveWindow for an object.
|
|
||||||
*
|
|
||||||
* @param sendable object
|
|
||||||
*/
|
|
||||||
public static synchronized void disableLiveWindow(Sendable sendable) {
|
|
||||||
Component comp = components.get(sendable);
|
|
||||||
if (comp != null) {
|
|
||||||
comp.m_liveWindow = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Publishes an object in the registry to a builder.
|
* Publishes an object in the registry to a builder.
|
||||||
*
|
*
|
||||||
@@ -485,98 +351,4 @@ public final class SendableRegistry {
|
|||||||
comp.m_builder.update();
|
comp.m_builder.update();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Data passed to foreachLiveWindow() callback function. */
|
|
||||||
@SuppressWarnings("MemberName")
|
|
||||||
public static class CallbackData {
|
|
||||||
/** Sendable object. */
|
|
||||||
public Sendable sendable;
|
|
||||||
|
|
||||||
/** Name. */
|
|
||||||
public String name;
|
|
||||||
|
|
||||||
/** Subsystem. */
|
|
||||||
public String subsystem;
|
|
||||||
|
|
||||||
/** Parent sendable object. */
|
|
||||||
public Sendable parent;
|
|
||||||
|
|
||||||
/** Data stored in object with setData(). Update this to change the data. */
|
|
||||||
public AutoCloseable data;
|
|
||||||
|
|
||||||
/** Sendable builder for the sendable. */
|
|
||||||
public SendableBuilder builder;
|
|
||||||
|
|
||||||
/** Default constructor. */
|
|
||||||
public CallbackData() {}
|
|
||||||
}
|
|
||||||
|
|
||||||
// As foreachLiveWindow is single threaded, cache the components it
|
|
||||||
// iterates over to avoid risk of ConcurrentModificationException
|
|
||||||
private static List<Component> foreachComponents = new ArrayList<>();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Iterates over LiveWindow-enabled objects in the registry. It is *not* safe to call other
|
|
||||||
* SendableRegistry functions from the callback.
|
|
||||||
*
|
|
||||||
* @param dataHandle data handle to get data object passed to callback
|
|
||||||
* @param callback function to call for each object
|
|
||||||
*/
|
|
||||||
@SuppressWarnings("PMD.CompareObjectsWithEquals")
|
|
||||||
public static synchronized void foreachLiveWindow(
|
|
||||||
int dataHandle, Consumer<CallbackData> callback) {
|
|
||||||
CallbackData cbdata = new CallbackData();
|
|
||||||
foreachComponents.clear();
|
|
||||||
foreachComponents.addAll(components.values());
|
|
||||||
for (Component comp : foreachComponents) {
|
|
||||||
if (comp.m_builder == null || comp.m_sendable == null) {
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
cbdata.sendable = comp.m_sendable.get();
|
|
||||||
if (cbdata.sendable != null && comp.m_liveWindow) {
|
|
||||||
cbdata.name = comp.m_name;
|
|
||||||
cbdata.subsystem = comp.m_subsystem;
|
|
||||||
if (comp.m_parent != null) {
|
|
||||||
cbdata.parent = comp.m_parent.get();
|
|
||||||
} else {
|
|
||||||
cbdata.parent = null;
|
|
||||||
}
|
|
||||||
if (comp.m_data != null && dataHandle < comp.m_data.length) {
|
|
||||||
cbdata.data = comp.m_data[dataHandle];
|
|
||||||
} else {
|
|
||||||
cbdata.data = null;
|
|
||||||
}
|
|
||||||
cbdata.builder = comp.m_builder;
|
|
||||||
try {
|
|
||||||
callback.accept(cbdata);
|
|
||||||
} catch (Throwable throwable) {
|
|
||||||
Throwable cause = throwable.getCause();
|
|
||||||
if (cause != null) {
|
|
||||||
throwable = cause;
|
|
||||||
}
|
|
||||||
System.err.println("Unhandled exception calling LiveWindow for " + comp.m_name + ": ");
|
|
||||||
throwable.printStackTrace();
|
|
||||||
comp.m_liveWindow = false;
|
|
||||||
}
|
|
||||||
if (cbdata.data != null) {
|
|
||||||
if (comp.m_data == null) {
|
|
||||||
comp.m_data = new AutoCloseable[dataHandle + 1];
|
|
||||||
} else if (dataHandle >= comp.m_data.length) {
|
|
||||||
comp.m_data = Arrays.copyOf(comp.m_data, dataHandle + 1);
|
|
||||||
}
|
|
||||||
if (comp.m_data[dataHandle] != cbdata.data) {
|
|
||||||
if (comp.m_data[dataHandle] != null) {
|
|
||||||
try {
|
|
||||||
comp.m_data[dataHandle].close();
|
|
||||||
} catch (Exception e) {
|
|
||||||
// ignore
|
|
||||||
}
|
|
||||||
}
|
|
||||||
comp.m_data[dataHandle] = cbdata.data;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
foreachComponents.clear();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -26,7 +26,6 @@ struct Component {
|
|||||||
std::string name;
|
std::string name;
|
||||||
std::string subsystem = "Ungrouped";
|
std::string subsystem = "Ungrouped";
|
||||||
Sendable* parent = nullptr;
|
Sendable* parent = nullptr;
|
||||||
bool liveWindow = false;
|
|
||||||
wpi::SmallVector<std::shared_ptr<void>, 2> data;
|
wpi::SmallVector<std::shared_ptr<void>, 2> data;
|
||||||
|
|
||||||
void SetName(std::string_view moduleType, int channel) {
|
void SetName(std::string_view moduleType, int channel) {
|
||||||
@@ -41,7 +40,6 @@ struct Component {
|
|||||||
struct SendableRegistryInst {
|
struct SendableRegistryInst {
|
||||||
wpi::recursive_mutex mutex;
|
wpi::recursive_mutex mutex;
|
||||||
|
|
||||||
std::function<std::unique_ptr<SendableBuilder>()> liveWindowFactory;
|
|
||||||
wpi::UidVector<std::unique_ptr<Component>, 32> components;
|
wpi::UidVector<std::unique_ptr<Component>, 32> components;
|
||||||
wpi::DenseMap<void*, SendableRegistry::UID> componentMap;
|
wpi::DenseMap<void*, SendableRegistry::UID> componentMap;
|
||||||
int nextDataHandle = 0;
|
int nextDataHandle = 0;
|
||||||
@@ -85,11 +83,6 @@ void SendableRegistry::EnsureInitialized() {
|
|||||||
GetInstance();
|
GetInstance();
|
||||||
}
|
}
|
||||||
|
|
||||||
void SendableRegistry::SetLiveWindowBuilderFactory(
|
|
||||||
std::function<std::unique_ptr<SendableBuilder>()> factory) {
|
|
||||||
GetInstance().liveWindowFactory = std::move(factory);
|
|
||||||
}
|
|
||||||
|
|
||||||
void SendableRegistry::Add(Sendable* sendable, std::string_view name) {
|
void SendableRegistry::Add(Sendable* sendable, std::string_view name) {
|
||||||
auto& inst = GetInstance();
|
auto& inst = GetInstance();
|
||||||
std::scoped_lock lock(inst.mutex);
|
std::scoped_lock lock(inst.mutex);
|
||||||
@@ -126,58 +119,6 @@ void SendableRegistry::Add(Sendable* sendable, std::string_view subsystem,
|
|||||||
comp.subsystem = subsystem;
|
comp.subsystem = subsystem;
|
||||||
}
|
}
|
||||||
|
|
||||||
void SendableRegistry::AddLW(Sendable* sendable, std::string_view name) {
|
|
||||||
auto& inst = GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
auto& comp = inst.GetOrAdd(sendable);
|
|
||||||
comp.sendable = sendable;
|
|
||||||
if (inst.liveWindowFactory) {
|
|
||||||
comp.builder = inst.liveWindowFactory();
|
|
||||||
}
|
|
||||||
comp.liveWindow = true;
|
|
||||||
comp.name = name;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SendableRegistry::AddLW(Sendable* sendable, std::string_view moduleType,
|
|
||||||
int channel) {
|
|
||||||
auto& inst = GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
auto& comp = inst.GetOrAdd(sendable);
|
|
||||||
comp.sendable = sendable;
|
|
||||||
if (inst.liveWindowFactory) {
|
|
||||||
comp.builder = inst.liveWindowFactory();
|
|
||||||
}
|
|
||||||
comp.liveWindow = true;
|
|
||||||
comp.SetName(moduleType, channel);
|
|
||||||
}
|
|
||||||
|
|
||||||
void SendableRegistry::AddLW(Sendable* sendable, std::string_view moduleType,
|
|
||||||
int moduleNumber, int channel) {
|
|
||||||
auto& inst = GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
auto& comp = inst.GetOrAdd(sendable);
|
|
||||||
comp.sendable = sendable;
|
|
||||||
if (inst.liveWindowFactory) {
|
|
||||||
comp.builder = inst.liveWindowFactory();
|
|
||||||
}
|
|
||||||
comp.liveWindow = true;
|
|
||||||
comp.SetName(moduleType, moduleNumber, channel);
|
|
||||||
}
|
|
||||||
|
|
||||||
void SendableRegistry::AddLW(Sendable* sendable, std::string_view subsystem,
|
|
||||||
std::string_view name) {
|
|
||||||
auto& inst = GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
auto& comp = inst.GetOrAdd(sendable);
|
|
||||||
comp.sendable = sendable;
|
|
||||||
if (inst.liveWindowFactory) {
|
|
||||||
comp.builder = inst.liveWindowFactory();
|
|
||||||
}
|
|
||||||
comp.liveWindow = true;
|
|
||||||
comp.name = name;
|
|
||||||
comp.subsystem = subsystem;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SendableRegistry::AddChild(Sendable* parent, Sendable* child) {
|
void SendableRegistry::AddChild(Sendable* parent, Sendable* child) {
|
||||||
auto& inst = GetInstance();
|
auto& inst = GetInstance();
|
||||||
std::scoped_lock lock(inst.mutex);
|
std::scoped_lock lock(inst.mutex);
|
||||||
@@ -361,26 +302,6 @@ std::shared_ptr<void> SendableRegistry::GetData(Sendable* sendable,
|
|||||||
return comp.data[handle];
|
return comp.data[handle];
|
||||||
}
|
}
|
||||||
|
|
||||||
void SendableRegistry::EnableLiveWindow(Sendable* sendable) {
|
|
||||||
auto& inst = GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
auto it = inst.componentMap.find(sendable);
|
|
||||||
if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
inst.components[it->getSecond() - 1]->liveWindow = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SendableRegistry::DisableLiveWindow(Sendable* sendable) {
|
|
||||||
auto& inst = GetInstance();
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
auto it = inst.componentMap.find(sendable);
|
|
||||||
if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
inst.components[it->getSecond() - 1]->liveWindow = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
SendableRegistry::UID SendableRegistry::GetUniqueId(Sendable* sendable) {
|
SendableRegistry::UID SendableRegistry::GetUniqueId(Sendable* sendable) {
|
||||||
auto& inst = GetInstance();
|
auto& inst = GetInstance();
|
||||||
std::scoped_lock lock(inst.mutex);
|
std::scoped_lock lock(inst.mutex);
|
||||||
@@ -430,25 +351,3 @@ void SendableRegistry::Update(UID sendableUid) {
|
|||||||
inst.components[sendableUid - 1]->builder->Update();
|
inst.components[sendableUid - 1]->builder->Update();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void SendableRegistry::ForeachLiveWindow(
|
|
||||||
int dataHandle, wpi::function_ref<void(CallbackData& data)> callback) {
|
|
||||||
auto& inst = GetInstance();
|
|
||||||
assert(dataHandle >= 0);
|
|
||||||
std::scoped_lock lock(inst.mutex);
|
|
||||||
wpi::SmallVector<Component*, 128> components;
|
|
||||||
for (auto&& comp : inst.components) {
|
|
||||||
components.emplace_back(comp.get());
|
|
||||||
}
|
|
||||||
for (auto comp : components) {
|
|
||||||
if (comp && comp->builder && comp->sendable && comp->liveWindow) {
|
|
||||||
if (static_cast<size_t>(dataHandle) >= comp->data.size()) {
|
|
||||||
comp->data.resize(dataHandle + 1);
|
|
||||||
}
|
|
||||||
CallbackData cbdata{comp->sendable, comp->name,
|
|
||||||
comp->subsystem, comp->parent,
|
|
||||||
comp->data[dataHandle], *comp->builder};
|
|
||||||
callback(cbdata);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|||||||
@@ -48,14 +48,6 @@ class SendableBuilder {
|
|||||||
*/
|
*/
|
||||||
virtual void SetActuator(bool value) = 0;
|
virtual void SetActuator(bool value) = 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<void()> func) = 0;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Add a boolean property.
|
* Add a boolean property.
|
||||||
*
|
*
|
||||||
|
|||||||
@@ -4,13 +4,10 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <functional>
|
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <string_view>
|
#include <string_view>
|
||||||
|
|
||||||
#include "wpi/function_ref.h"
|
|
||||||
|
|
||||||
namespace wpi {
|
namespace wpi {
|
||||||
|
|
||||||
class Sendable;
|
class Sendable;
|
||||||
@@ -18,7 +15,7 @@ class SendableBuilder;
|
|||||||
|
|
||||||
/**
|
/**
|
||||||
* The SendableRegistry class is the public interface for registering sensors
|
* The SendableRegistry class is the public interface for registering sensors
|
||||||
* and actuators for use on dashboards and LiveWindow.
|
* and actuators for use on dashboards.
|
||||||
*/
|
*/
|
||||||
class SendableRegistry final {
|
class SendableRegistry final {
|
||||||
public:
|
public:
|
||||||
@@ -32,14 +29,6 @@ class SendableRegistry final {
|
|||||||
*/
|
*/
|
||||||
static void EnsureInitialized();
|
static void EnsureInitialized();
|
||||||
|
|
||||||
/**
|
|
||||||
* Sets the factory for LiveWindow builders.
|
|
||||||
*
|
|
||||||
* @param factory factory function
|
|
||||||
*/
|
|
||||||
static void SetLiveWindowBuilderFactory(
|
|
||||||
std::function<std::unique_ptr<SendableBuilder>()> factory);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Adds an object to the registry.
|
* Adds an object to the registry.
|
||||||
*
|
*
|
||||||
@@ -80,47 +69,6 @@ class SendableRegistry final {
|
|||||||
static void Add(Sendable* sendable, std::string_view subsystem,
|
static void Add(Sendable* sendable, std::string_view subsystem,
|
||||||
std::string_view name);
|
std::string_view name);
|
||||||
|
|
||||||
/**
|
|
||||||
* Adds an object to the registry and LiveWindow.
|
|
||||||
*
|
|
||||||
* @param sendable object to add
|
|
||||||
* @param name component name
|
|
||||||
*/
|
|
||||||
static void AddLW(Sendable* sendable, std::string_view name);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Adds an object to the registry and LiveWindow.
|
|
||||||
*
|
|
||||||
* @param sendable object to add
|
|
||||||
* @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
|
|
||||||
*/
|
|
||||||
static void AddLW(Sendable* sendable, std::string_view moduleType,
|
|
||||||
int channel);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Adds an object to the registry and LiveWindow.
|
|
||||||
*
|
|
||||||
* @param sendable object to add
|
|
||||||
* @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
|
|
||||||
*/
|
|
||||||
static void AddLW(Sendable* sendable, std::string_view moduleType,
|
|
||||||
int moduleNumber, int channel);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Adds an object to the registry and LiveWindow.
|
|
||||||
*
|
|
||||||
* @param sendable object to add
|
|
||||||
* @param subsystem subsystem name
|
|
||||||
* @param name component name
|
|
||||||
*/
|
|
||||||
static void AddLW(Sendable* sendable, std::string_view subsystem,
|
|
||||||
std::string_view name);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Adds a child object to an object. Adds the child object to the registry
|
* Adds a child object to an object. Adds the child object to the registry
|
||||||
* if it's not already present.
|
* if it's not already present.
|
||||||
@@ -255,20 +203,6 @@ class SendableRegistry final {
|
|||||||
*/
|
*/
|
||||||
static std::shared_ptr<void> GetData(Sendable* sendable, int handle);
|
static std::shared_ptr<void> GetData(Sendable* sendable, int handle);
|
||||||
|
|
||||||
/**
|
|
||||||
* Enables LiveWindow for an object.
|
|
||||||
*
|
|
||||||
* @param sendable object
|
|
||||||
*/
|
|
||||||
static void EnableLiveWindow(Sendable* sendable);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Disables LiveWindow for an object.
|
|
||||||
*
|
|
||||||
* @param sendable object
|
|
||||||
*/
|
|
||||||
static void DisableLiveWindow(Sendable* sendable);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Get unique id for an object. Since objects can move, use this instead
|
* Get unique id for an object. Since objects can move, use this instead
|
||||||
* of storing Sendable* directly if ownership is in question.
|
* of storing Sendable* directly if ownership is in question.
|
||||||
@@ -301,39 +235,6 @@ class SendableRegistry final {
|
|||||||
* @param sendableUid sendable unique id
|
* @param sendableUid sendable unique id
|
||||||
*/
|
*/
|
||||||
static void Update(UID sendableUid);
|
static void Update(UID sendableUid);
|
||||||
|
|
||||||
/**
|
|
||||||
* Data passed to ForeachLiveWindow() callback function
|
|
||||||
*/
|
|
||||||
struct CallbackData {
|
|
||||||
CallbackData(Sendable* sendable_, std::string_view name_,
|
|
||||||
std::string_view subsystem_, wpi::Sendable* parent_,
|
|
||||||
std::shared_ptr<void>& data_, SendableBuilder& builder_)
|
|
||||||
: sendable(sendable_),
|
|
||||||
name(name_),
|
|
||||||
subsystem(subsystem_),
|
|
||||||
parent(parent_),
|
|
||||||
data(data_),
|
|
||||||
builder(builder_) {}
|
|
||||||
|
|
||||||
Sendable* sendable;
|
|
||||||
std::string_view name;
|
|
||||||
std::string_view subsystem;
|
|
||||||
Sendable* parent;
|
|
||||||
std::shared_ptr<void>& data;
|
|
||||||
SendableBuilder& builder;
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Iterates over LiveWindow-enabled objects in the registry.
|
|
||||||
* It is *not* safe to call other SendableRegistry functions from the
|
|
||||||
* callback (this will likely deadlock).
|
|
||||||
*
|
|
||||||
* @param dataHandle data handle to get data pointer passed to callback
|
|
||||||
* @param callback function to call for each object
|
|
||||||
*/
|
|
||||||
static void ForeachLiveWindow(
|
|
||||||
int dataHandle, wpi::function_ref<void(CallbackData& cbdata)> callback);
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace wpi
|
} // namespace wpi
|
||||||
|
|||||||
Reference in New Issue
Block a user