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.math.geometry.Rotation2d;
008import edu.wpi.first.networktables.DoubleEntry;
009import edu.wpi.first.networktables.NetworkTable;
010import edu.wpi.first.networktables.StringEntry;
011import edu.wpi.first.networktables.StringPublisher;
012import edu.wpi.first.wpilibj.util.Color8Bit;
013
014/**
015 * Ligament node on a Mechanism2d. A ligament can have its length changed (like an elevator) or
016 * angle changed, like an arm.
017 *
018 * @see Mechanism2d
019 */
020public class MechanismLigament2d extends MechanismObject2d {
021  private StringPublisher m_typePub;
022  private double m_angle;
023  private DoubleEntry m_angleEntry;
024  private String m_color;
025  private StringEntry m_colorEntry;
026  private double m_length;
027  private DoubleEntry m_lengthEntry;
028  private double m_weight;
029  private DoubleEntry m_weightEntry;
030
031  /**
032   * Create a new ligament.
033   *
034   * @param name The ligament name.
035   * @param length The ligament length.
036   * @param angle The ligament angle in degrees.
037   * @param lineWidth The ligament's line width.
038   * @param color The ligament's color.
039   */
040  public MechanismLigament2d(
041      String name, double length, double angle, double lineWidth, Color8Bit color) {
042    super(name);
043    setColor(color);
044    setLength(length);
045    setAngle(angle);
046    setLineWeight(lineWidth);
047  }
048
049  /**
050   * Create a new ligament with the default color (orange) and thickness (6).
051   *
052   * @param name The ligament's name.
053   * @param length The ligament's length.
054   * @param angle The ligament's angle relative to its parent in degrees.
055   */
056  public MechanismLigament2d(String name, double length, double angle) {
057    this(name, length, angle, 10, new Color8Bit(235, 137, 52));
058  }
059
060  @Override
061  public void close() {
062    super.close();
063    if (m_typePub != null) {
064      m_typePub.close();
065    }
066    if (m_angleEntry != null) {
067      m_angleEntry.close();
068    }
069    if (m_colorEntry != null) {
070      m_colorEntry.close();
071    }
072    if (m_lengthEntry != null) {
073      m_lengthEntry.close();
074    }
075    if (m_weightEntry != null) {
076      m_weightEntry.close();
077    }
078  }
079
080  /**
081   * Set the ligament's angle relative to its parent.
082   *
083   * @param degrees the angle in degrees
084   */
085  public synchronized void setAngle(double degrees) {
086    m_angle = degrees;
087    if (m_angleEntry != null) {
088      m_angleEntry.set(degrees);
089    }
090  }
091
092  /**
093   * Set the ligament's angle relative to its parent.
094   *
095   * @param angle the angle
096   */
097  public synchronized void setAngle(Rotation2d angle) {
098    setAngle(angle.getDegrees());
099  }
100
101  /**
102   * Get the ligament's angle relative to its parent.
103   *
104   * @return the angle in degrees
105   */
106  public synchronized double getAngle() {
107    if (m_angleEntry != null) {
108      m_angle = m_angleEntry.get();
109    }
110    return m_angle;
111  }
112
113  /**
114   * Set the ligament's length.
115   *
116   * @param length the line length
117   */
118  public synchronized void setLength(double length) {
119    m_length = length;
120    if (m_lengthEntry != null) {
121      m_lengthEntry.set(length);
122    }
123  }
124
125  /**
126   * Get the ligament length.
127   *
128   * @return the line length
129   */
130  public synchronized double getLength() {
131    if (m_lengthEntry != null) {
132      m_length = m_lengthEntry.get();
133    }
134    return m_length;
135  }
136
137  /**
138   * Set the ligament color.
139   *
140   * @param color the color of the line
141   */
142  public synchronized void setColor(Color8Bit color) {
143    m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
144    if (m_colorEntry != null) {
145      m_colorEntry.set(m_color);
146    }
147  }
148
149  /**
150   * Get the ligament color.
151   *
152   * @return the color of the line
153   */
154  public synchronized Color8Bit getColor() {
155    if (m_colorEntry != null) {
156      m_color = m_colorEntry.get();
157    }
158    int r = 0;
159    int g = 0;
160    int b = 0;
161    if (m_color.length() == 7 && m_color.charAt(0) == '#') {
162      try {
163        r = Integer.parseInt(m_color.substring(1, 3), 16);
164        g = Integer.parseInt(m_color.substring(3, 5), 16);
165        b = Integer.parseInt(m_color.substring(5, 7), 16);
166      } catch (NumberFormatException e) {
167        r = 0;
168        g = 0;
169        b = 0;
170      }
171    }
172    return new Color8Bit(r, g, b);
173  }
174
175  /**
176   * Set the line thickness.
177   *
178   * @param weight the line thickness
179   */
180  public synchronized void setLineWeight(double weight) {
181    m_weight = weight;
182    if (m_weightEntry != null) {
183      m_weightEntry.set(weight);
184    }
185  }
186
187  /**
188   * Get the line thickness.
189   *
190   * @return the line thickness
191   */
192  public synchronized double getLineWeight() {
193    if (m_weightEntry != null) {
194      m_weight = m_weightEntry.get();
195    }
196    return m_weight;
197  }
198
199  @Override
200  protected void updateEntries(NetworkTable table) {
201    if (m_typePub != null) {
202      m_typePub.close();
203    }
204    m_typePub = table.getStringTopic(".type").publish();
205    m_typePub.set("line");
206
207    if (m_angleEntry != null) {
208      m_angleEntry.close();
209    }
210    m_angleEntry = table.getDoubleTopic("angle").getEntry(0.0);
211    m_angleEntry.set(m_angle);
212
213    if (m_lengthEntry != null) {
214      m_lengthEntry.close();
215    }
216    m_lengthEntry = table.getDoubleTopic("length").getEntry(0.0);
217    m_lengthEntry.set(m_length);
218
219    if (m_colorEntry != null) {
220      m_colorEntry.close();
221    }
222    m_colorEntry = table.getStringTopic("color").getEntry("");
223    m_colorEntry.set(m_color);
224
225    if (m_weightEntry != null) {
226      m_weightEntry.close();
227    }
228    m_weightEntry = table.getDoubleTopic("weight").getEntry(0.0);
229    m_weightEntry.set(m_weight);
230  }
231}