001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj.livewindow;
006
007import edu.wpi.first.networktables.NetworkTable;
008import edu.wpi.first.networktables.NetworkTableEntry;
009import edu.wpi.first.networktables.NetworkTableInstance;
010import edu.wpi.first.util.sendable.Sendable;
011import edu.wpi.first.util.sendable.SendableRegistry;
012import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
013
014/**
015 * The LiveWindow class is the public interface for putting sensors and actuators on the LiveWindow.
016 */
017public class LiveWindow {
018  private static class Component {
019    boolean m_firstTime = true;
020    boolean m_telemetryEnabled = true;
021  }
022
023  private static final int dataHandle = SendableRegistry.getDataHandle();
024  private static final NetworkTable liveWindowTable =
025      NetworkTableInstance.getDefault().getTable("LiveWindow");
026  private static final NetworkTable statusTable = liveWindowTable.getSubTable(".status");
027  private static final NetworkTableEntry enabledEntry = statusTable.getEntry("LW Enabled");
028  private static boolean startLiveWindow;
029  private static boolean liveWindowEnabled;
030  private static boolean telemetryEnabled = true;
031
032  private static Runnable enabledListener;
033  private static Runnable disabledListener;
034
035  static {
036    SendableRegistry.setLiveWindowBuilderFactory(() -> new SendableBuilderImpl());
037  }
038
039  private static Component getOrAdd(Sendable sendable) {
040    Component data = (Component) SendableRegistry.getData(sendable, dataHandle);
041    if (data == null) {
042      data = new Component();
043      SendableRegistry.setData(sendable, dataHandle, data);
044    }
045    return data;
046  }
047
048  private LiveWindow() {
049    throw new UnsupportedOperationException("This is a utility class!");
050  }
051
052  public static synchronized void setEnabledListener(Runnable runnable) {
053    enabledListener = runnable;
054  }
055
056  public static synchronized void setDisabledListener(Runnable runnable) {
057    disabledListener = runnable;
058  }
059
060  public static synchronized boolean isEnabled() {
061    return liveWindowEnabled;
062  }
063
064  /**
065   * Set the enabled state of LiveWindow.
066   *
067   * <p>If it's being enabled, turn off the scheduler and remove all the commands from the queue and
068   * enable all the components registered for LiveWindow. If it's being disabled, stop all the
069   * registered components and reenable the scheduler.
070   *
071   * <p>TODO: add code to disable PID loops when enabling LiveWindow. The commands should reenable
072   * the PID loops themselves when they get rescheduled. This prevents arms from starting to move
073   * around, etc. after a period of adjusting them in LiveWindow mode.
074   *
075   * @param enabled True to enable LiveWindow.
076   */
077  public static synchronized void setEnabled(boolean enabled) {
078    if (liveWindowEnabled != enabled) {
079      startLiveWindow = enabled;
080      liveWindowEnabled = enabled;
081      updateValues(); // Force table generation now to make sure everything is defined
082      if (enabled) {
083        System.out.println("Starting live window mode.");
084        if (enabledListener != null) {
085          enabledListener.run();
086        }
087      } else {
088        System.out.println("stopping live window mode.");
089        SendableRegistry.foreachLiveWindow(
090            dataHandle,
091            cbdata -> {
092              ((SendableBuilderImpl) cbdata.builder).stopLiveWindowMode();
093            });
094        if (disabledListener != null) {
095          disabledListener.run();
096        }
097      }
098      enabledEntry.setBoolean(enabled);
099    }
100  }
101
102  /**
103   * Enable telemetry for a single component.
104   *
105   * @param sendable component
106   */
107  public static synchronized void enableTelemetry(Sendable sendable) {
108    // Re-enable global setting in case disableAllTelemetry() was called.
109    telemetryEnabled = true;
110    getOrAdd(sendable).m_telemetryEnabled = true;
111  }
112
113  /**
114   * Disable telemetry for a single component.
115   *
116   * @param sendable component
117   */
118  public static synchronized void disableTelemetry(Sendable sendable) {
119    getOrAdd(sendable).m_telemetryEnabled = false;
120  }
121
122  /** Disable ALL telemetry. */
123  public static synchronized void disableAllTelemetry() {
124    telemetryEnabled = false;
125    SendableRegistry.foreachLiveWindow(
126        dataHandle,
127        cbdata -> {
128          if (cbdata.data == null) {
129            cbdata.data = new Component();
130          }
131          ((Component) cbdata.data).m_telemetryEnabled = false;
132        });
133  }
134
135  /**
136   * Tell all the sensors to update (send) their values.
137   *
138   * <p>Actuators are handled through callbacks on their value changing from the SmartDashboard
139   * widgets.
140   */
141  public static synchronized void updateValues() {
142    // Only do this if either LiveWindow mode or telemetry is enabled.
143    if (!liveWindowEnabled && !telemetryEnabled) {
144      return;
145    }
146
147    SendableRegistry.foreachLiveWindow(
148        dataHandle,
149        cbdata -> {
150          if (cbdata.sendable == null || cbdata.parent != null) {
151            return;
152          }
153
154          if (cbdata.data == null) {
155            cbdata.data = new Component();
156          }
157
158          Component component = (Component) cbdata.data;
159
160          if (!liveWindowEnabled && !component.m_telemetryEnabled) {
161            return;
162          }
163
164          if (component.m_firstTime) {
165            // By holding off creating the NetworkTable entries, it allows the
166            // components to be redefined. This allows default sensor and actuator
167            // values to be created that are replaced with the custom names from
168            // users calling setName.
169            if (cbdata.name.isEmpty()) {
170              return;
171            }
172            NetworkTable ssTable = liveWindowTable.getSubTable(cbdata.subsystem);
173            NetworkTable table;
174            // Treat name==subsystem as top level of subsystem
175            if (cbdata.name.equals(cbdata.subsystem)) {
176              table = ssTable;
177            } else {
178              table = ssTable.getSubTable(cbdata.name);
179            }
180            table.getEntry(".name").setString(cbdata.name);
181            ((SendableBuilderImpl) cbdata.builder).setTable(table);
182            cbdata.sendable.initSendable(cbdata.builder);
183            ssTable.getEntry(".type").setString("LW Subsystem");
184
185            component.m_firstTime = false;
186          }
187
188          if (startLiveWindow) {
189            ((SendableBuilderImpl) cbdata.builder).startLiveWindowMode();
190          }
191          cbdata.builder.update();
192        });
193
194    startLiveWindow = false;
195  }
196}