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.NetworkTable;
009import edu.wpi.first.networktables.NetworkTableEntry;
010import edu.wpi.first.wpilibj.util.Color8Bit;
011
012/**
013 * Ligament node on a Mechanism2d. A ligament can have its length changed (like an elevator) or
014 * angle changed, like an arm.
015 *
016 * @see Mechanism2d
017 */
018public class MechanismLigament2d extends MechanismObject2d {
019  private double m_angle;
020  private NetworkTableEntry m_angleEntry;
021  private String m_color;
022  private NetworkTableEntry m_colorEntry;
023  private double m_length;
024  private NetworkTableEntry m_lengthEntry;
025  private double m_weight;
026  private NetworkTableEntry m_weightEntry;
027
028  /**
029   * Create a new ligament.
030   *
031   * @param name The ligament name.
032   * @param length The ligament length.
033   * @param angle The ligament angle.
034   * @param lineWidth The ligament's line width.
035   * @param color The ligament's color.
036   */
037  public MechanismLigament2d(
038      String name, double length, double angle, double lineWidth, Color8Bit color) {
039    super(name);
040    setColor(color);
041    setLength(length);
042    setAngle(angle);
043    setLineWeight(lineWidth);
044  }
045
046  /**
047   * Create a new ligament with the default color (orange) and thickness (6).
048   *
049   * @param name The ligament's name.
050   * @param length The ligament's length.
051   * @param angle The ligament's angle relative to its parent.
052   */
053  public MechanismLigament2d(String name, double length, double angle) {
054    this(name, length, angle, 10, new Color8Bit(235, 137, 52));
055  }
056
057  /**
058   * Set the ligament's angle relative to its parent.
059   *
060   * @param degrees the angle, in degrees
061   */
062  public synchronized void setAngle(double degrees) {
063    m_angle = degrees;
064    flush();
065  }
066
067  /**
068   * Set the ligament's angle relative to its parent.
069   *
070   * @param angle the angle
071   */
072  public synchronized void setAngle(Rotation2d angle) {
073    setAngle(angle.getDegrees());
074  }
075
076  /**
077   * Get the ligament's angle relative to its parent.
078   *
079   * @return the angle, in degrees
080   */
081  public synchronized double getAngle() {
082    if (m_angleEntry != null) {
083      m_angle = m_angleEntry.getDouble(0.0);
084    }
085    return m_angle;
086  }
087
088  /**
089   * Set the ligament's length.
090   *
091   * @param length the line length
092   */
093  public synchronized void setLength(double length) {
094    m_length = length;
095    flush();
096  }
097
098  /**
099   * Get the ligament length.
100   *
101   * @return the line length
102   */
103  public synchronized double getLength() {
104    if (m_lengthEntry != null) {
105      m_length = m_lengthEntry.getDouble(0.0);
106    }
107    return m_length;
108  }
109
110  /**
111   * Set the ligament color.
112   *
113   * @param color the color of the line
114   */
115  public synchronized void setColor(Color8Bit color) {
116    m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
117    flush();
118  }
119
120  /**
121   * Set the line thickness.
122   *
123   * @param weight the line thickness
124   */
125  public synchronized void setLineWeight(double weight) {
126    m_weight = weight;
127    flush();
128  }
129
130  @Override
131  protected void updateEntries(NetworkTable table) {
132    table.getEntry(".type").setString("line");
133    m_angleEntry = table.getEntry("angle");
134    m_lengthEntry = table.getEntry("length");
135    m_colorEntry = table.getEntry("color");
136    m_weightEntry = table.getEntry("weight");
137    flush();
138  }
139
140  /** Flush latest data to NT. */
141  private void flush() {
142    if (m_angleEntry != null) {
143      m_angleEntry.setDouble(m_angle);
144    }
145    if (m_lengthEntry != null) {
146      m_lengthEntry.setDouble(m_length);
147    }
148    if (m_colorEntry != null) {
149      m_colorEntry.setString(m_color);
150    }
151    if (m_weightEntry != null) {
152      m_weightEntry.setDouble(m_weight);
153    }
154  }
155}