Update Mechanism2d

Author: PeterJohnson

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Mechanism2d.java

@@ -4,11 +4,8 @@
 
 package edu.wpi.first.wpilibj.smartdashboard;
 
-import edu.wpi.first.networktables.DoubleArrayPublisher;
-import edu.wpi.first.networktables.NTSendable;
-import edu.wpi.first.networktables.NTSendableBuilder;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.telemetry.TelemetryLoggable;
+import edu.wpi.first.telemetry.TelemetryTable;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import java.util.HashMap;
 import java.util.Map;
@@ -25,13 +22,10 @@
  * @see MechanismLigament2d
  * @see MechanismRoot2d
  */
-public final class Mechanism2d implements NTSendable, AutoCloseable {
-  private NetworkTable m_table;
+public final class Mechanism2d implements TelemetryLoggable, AutoCloseable {
   private final Map<String, MechanismRoot2d> m_roots;
   private final double[] m_dims = new double[2];
   private String m_color;
-  private DoubleArrayPublisher m_dimsPub;
-  private StringPublisher m_colorPub;
 
   /**
    * Create a new Mechanism2d with the given dimensions and default color (dark blue).
@@ -63,12 +57,6 @@ public Mechanism2d(double width, double height, Color8Bit backgroundColor) {
 
   @Override
   public void close() {
-    if (m_dimsPub != null) {
-      m_dimsPub.close();
-    }
-    if (m_colorPub != null) {
-      m_colorPub.close();
-    }
     for (MechanismRoot2d root : m_roots.values()) {
       root.close();
     }
@@ -92,9 +80,6 @@ public synchronized MechanismRoot2d getRoot(String name, double x, double y) {
 
     var root = new MechanismRoot2d(name, x, y);
     m_roots.put(name, root);
-    if (m_table != null) {
-      root.update(m_table.getSubTable(name));
-    }
     return root;
   }
 
@@ -105,32 +90,18 @@ public synchronized MechanismRoot2d getRoot(String name, double x, double y) {
    */
   public synchronized void setBackgroundColor(Color8Bit color) {
     m_color = color.toHexString();
-    if (m_colorPub != null) {
-      m_colorPub.set(m_color);
-    }
   }
 
   @Override
-  public void initSendable(NTSendableBuilder builder) {
-    builder.setSmartDashboardType("Mechanism2d");
+  public void toTelemetry(TelemetryTable table, boolean first) {
+    if (first) {
+      table.log(".type", "Mechanism2d");
+    }
     synchronized (this) {
-      m_table = builder.getTable();
-      if (m_dimsPub != null) {
-        m_dimsPub.close();
-      }
-      m_dimsPub = m_table.getDoubleArrayTopic("dims").publish();
-      m_dimsPub.set(m_dims);
-      if (m_colorPub != null) {
-        m_colorPub.close();
-      }
-      m_colorPub = m_table.getStringTopic("backgroundColor").publish();
-      m_colorPub.set(m_color);
+      table.log("dims", m_dims);
+      table.log("backgroundColor", m_color);
       for (Entry<String, MechanismRoot2d> entry : m_roots.entrySet()) {
-        String name = entry.getKey();
-        MechanismRoot2d root = entry.getValue();
-        synchronized (root) {
-          root.update(m_table.getSubTable(name));
-        }
+        table.log(entry.getKey(), entry.getValue());
       }
     }
   }

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java

@@ -5,11 +5,7 @@
 package edu.wpi.first.wpilibj.smartdashboard;
 
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.networktables.DoubleEntry;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.StringEntry;
-import edu.wpi.first.networktables.StringPublisher;
-import edu.wpi.first.networktables.StringTopic;
+import edu.wpi.first.telemetry.TelemetryTable;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 
 /**
@@ -19,17 +15,10 @@
  * @see Mechanism2d
  */
 public class MechanismLigament2d extends MechanismObject2d {
-  private StringPublisher m_typePub;
-  private double m_angle;
-  private DoubleEntry m_angleEntry;
+  private Rotation2d m_angle;
   private String m_color;
-  private StringEntry m_colorEntry;
   private double m_length;
-  private DoubleEntry m_lengthEntry;
   private double m_weight;
-  private DoubleEntry m_weightEntry;
-
-  private static String kSmartDashboardType = "line";
 
   /**
    * Create a new ligament.
@@ -60,36 +49,13 @@ public MechanismLigament2d(String name, double length, double angle) {
     this(name, length, angle, 10, new Color8Bit(235, 137, 52));
   }
 
-  @Override
-  public void close() {
-    super.close();
-    if (m_typePub != null) {
-      m_typePub.close();
-    }
-    if (m_angleEntry != null) {
-      m_angleEntry.close();
-    }
-    if (m_colorEntry != null) {
-      m_colorEntry.close();
-    }
-    if (m_lengthEntry != null) {
-      m_lengthEntry.close();
-    }
-    if (m_weightEntry != null) {
-      m_weightEntry.close();
-    }
-  }
-
   /**
    * Set the ligament's angle relative to its parent.
    *
    * @param degrees the angle in degrees
    */
   public final synchronized void setAngle(double degrees) {
-    m_angle = degrees;
-    if (m_angleEntry != null) {
-      m_angleEntry.set(degrees);
-    }
+    m_angle = Rotation2d.fromDegrees(degrees);
   }
 
   /**
@@ -106,10 +72,7 @@ public synchronized void setAngle(Rotation2d angle) {
    *
    * @return the angle in degrees
    */
-  public synchronized double getAngle() {
-    if (m_angleEntry != null) {
-      m_angle = m_angleEntry.get();
-    }
+  public synchronized Rotation2d getAngle() {
     return m_angle;
   }
 
@@ -120,9 +83,6 @@ public synchronized double getAngle() {
    */
   public final synchronized void setLength(double length) {
     m_length = length;
-    if (m_lengthEntry != null) {
-      m_lengthEntry.set(length);
-    }
   }
 
   /**
@@ -131,9 +91,6 @@ public final synchronized void setLength(double length) {
    * @return the line length
    */
   public synchronized double getLength() {
-    if (m_lengthEntry != null) {
-      m_length = m_lengthEntry.get();
-    }
     return m_length;
   }
 
@@ -144,9 +101,6 @@ public synchronized double getLength() {
    */
   public final synchronized void setColor(Color8Bit color) {
     m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
-    if (m_colorEntry != null) {
-      m_colorEntry.set(m_color);
-    }
   }
 
   /**
@@ -155,9 +109,6 @@ public final synchronized void setColor(Color8Bit color) {
    * @return the color of the line
    */
   public synchronized Color8Bit getColor() {
-    if (m_colorEntry != null) {
-      m_color = m_colorEntry.get();
-    }
     int r = 0;
     int g = 0;
     int b = 0;
@@ -182,9 +133,6 @@ public synchronized Color8Bit getColor() {
    */
   public final synchronized void setLineWeight(double weight) {
     m_weight = weight;
-    if (m_weightEntry != null) {
-      m_weightEntry.set(weight);
-    }
   }
 
   /**
@@ -193,46 +141,17 @@ public final synchronized void setLineWeight(double weight) {
    * @return the line thickness
    */
   public synchronized double getLineWeight() {
-    if (m_weightEntry != null) {
-      m_weight = m_weightEntry.get();
-    }
     return m_weight;
   }
 
   @Override
-  protected void updateEntries(NetworkTable table) {
-    if (m_typePub != null) {
-      m_typePub.close();
-    }
-    m_typePub =
-        table
-            .getStringTopic(".type")
-            .publishEx(
-                StringTopic.kTypeString, "{\"SmartDashboard\":\"" + kSmartDashboardType + "\"}");
-    m_typePub.set(kSmartDashboardType);
-
-    if (m_angleEntry != null) {
-      m_angleEntry.close();
-    }
-    m_angleEntry = table.getDoubleTopic("angle").getEntry(0.0);
-    m_angleEntry.set(m_angle);
-
-    if (m_lengthEntry != null) {
-      m_lengthEntry.close();
-    }
-    m_lengthEntry = table.getDoubleTopic("length").getEntry(0.0);
-    m_lengthEntry.set(m_length);
-
-    if (m_colorEntry != null) {
-      m_colorEntry.close();
-    }
-    m_colorEntry = table.getStringTopic("color").getEntry("");
-    m_colorEntry.set(m_color);
-
-    if (m_weightEntry != null) {
-      m_weightEntry.close();
-    }
-    m_weightEntry = table.getDoubleTopic("weight").getEntry(0.0);
-    m_weightEntry.set(m_weight);
+  public void toTelemetry(TelemetryTable table, boolean first) {
+    if (first) {
+      table.log(".type", "line");
+    }
+    table.log("angle", m_angle);
+    table.log("length", m_length);
+    table.log("color", m_color);
+    table.log("weight", m_weight);
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java

@@ -4,9 +4,11 @@
 
 package edu.wpi.first.wpilibj.smartdashboard;
 
-import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.telemetry.TelemetryLoggable;
+import edu.wpi.first.telemetry.TelemetryTable;
 import java.util.HashMap;
 import java.util.Map;
+import java.util.Map.Entry;
 
 /**
  * Common base class for all Mechanism2d node types.
@@ -16,11 +18,10 @@
  *
  * @see Mechanism2d
  */
-public abstract class MechanismObject2d implements AutoCloseable {
+public abstract class MechanismObject2d implements TelemetryLoggable, AutoCloseable {
   /** Relative to parent. */
   private final String m_name;
 
-  private NetworkTable m_table;
   private final Map<String, MechanismObject2d> m_objects = new HashMap<>(1);
 
   /**
@@ -53,27 +54,9 @@ public final synchronized <T extends MechanismObject2d> T append(T object) {
       throw new UnsupportedOperationException("Mechanism object names must be unique!");
     }
     m_objects.put(object.getName(), object);
-    if (m_table != null) {
-      object.update(m_table.getSubTable(object.getName()));
-    }
     return object;
   }
 
-  final synchronized void update(NetworkTable table) {
-    m_table = table;
-    updateEntries(m_table);
-    for (MechanismObject2d obj : m_objects.values()) {
-      obj.update(m_table.getSubTable(obj.m_name));
-    }
-  }
-
-  /**
-   * Update this object's entries with new ones from a new table.
-   *
-   * @param table the new table.
-   */
-  protected abstract void updateEntries(NetworkTable table);
-
   /**
    * Retrieve the object's name.
    *
@@ -82,4 +65,11 @@ final synchronized void update(NetworkTable table) {
   public final String getName() {
     return m_name;
   }
+
+  @Override
+  public synchronized void toTelemetry(TelemetryTable table, boolean first) {
+    for (Entry<String, MechanismObject2d> entry : m_objects.entrySet()) {
+      table.log(entry.getKey(), entry.getValue());
+    }
+  }
 }