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.smartdashboard;
006
007import edu.wpi.first.networktables.NTSendable;
008import edu.wpi.first.networktables.NTSendableBuilder;
009import edu.wpi.first.networktables.NetworkTable;
010import edu.wpi.first.wpilibj.util.Color8Bit;
011import java.util.HashMap;
012import java.util.Map;
013import java.util.Map.Entry;
014
015/**
016 * Visual 2D representation of arms, elevators, and general mechanisms through a node-based API.
017 *
018 * <p>A Mechanism2d object is published and contains at least one root node. A root is the anchor
019 * point of other nodes (such as ligaments). Other nodes are recursively appended based on other
020 * nodes.
021 *
022 * @see MechanismObject2d
023 * @see MechanismLigament2d
024 * @see MechanismRoot2d
025 */
026public final class Mechanism2d implements NTSendable {
027  private static final String kBackgroundColor = "backgroundColor";
028  private NetworkTable m_table;
029  private final Map<String, MechanismRoot2d> m_roots;
030  private final double[] m_dims = new double[2];
031  private String m_color;
032
033  /**
034   * Create a new Mechanism2d with the given dimensions and default color (dark blue).
035   *
036   * <p>The dimensions represent the canvas that all the nodes are drawn on.
037   *
038   * @param width the width
039   * @param height the height
040   */
041  public Mechanism2d(double width, double height) {
042    this(width, height, new Color8Bit(0, 0, 32));
043  }
044
045  /**
046   * Create a new Mechanism2d with the given dimensions.
047   *
048   * <p>The dimensions represent the canvas that all the nodes are drawn on.
049   *
050   * @param width the width
051   * @param height the height
052   * @param backgroundColor the background color. Defaults to dark blue.
053   */
054  public Mechanism2d(double width, double height, Color8Bit backgroundColor) {
055    m_roots = new HashMap<>();
056    m_dims[0] = width;
057    m_dims[1] = height;
058    setBackgroundColor(backgroundColor);
059  }
060
061  /**
062   * Get or create a root in this Mechanism2d with the given name and position.
063   *
064   * <p>If a root with the given name already exists, the given x and y coordinates are not used.
065   *
066   * @param name the root name
067   * @param x the root x coordinate
068   * @param y the root y coordinate
069   * @return a new root joint object, or the existing one with the given name.
070   */
071  public synchronized MechanismRoot2d getRoot(String name, double x, double y) {
072    var existing = m_roots.get(name);
073    if (existing != null) {
074      return existing;
075    }
076
077    var root = new MechanismRoot2d(name, x, y);
078    m_roots.put(name, root);
079    if (m_table != null) {
080      root.update(m_table.getSubTable(name));
081    }
082    return root;
083  }
084
085  /**
086   * Set the Mechanism2d background color.
087   *
088   * @param color the new color
089   */
090  public synchronized void setBackgroundColor(Color8Bit color) {
091    this.m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
092    if (m_table != null) {
093      m_table.getEntry(kBackgroundColor).setString(m_color);
094    }
095  }
096
097  @Override
098  public void initSendable(NTSendableBuilder builder) {
099    builder.setSmartDashboardType("Mechanism2d");
100    synchronized (this) {
101      m_table = builder.getTable();
102      m_table.getEntry("dims").setDoubleArray(m_dims);
103      m_table.getEntry(kBackgroundColor).setString(m_color);
104      for (Entry<String, MechanismRoot2d> entry : m_roots.entrySet()) {
105        String name = entry.getKey();
106        MechanismRoot2d root = entry.getValue();
107        synchronized (root) {
108          root.update(m_table.getSubTable(name));
109        }
110      }
111    }
112  }
113}