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