mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
[glass, wpilib] Rewrite Mechanism2d (#3281)
Substantially improves Mechanism2d by moving it to NetworkTables and adding a robot API to create the mechanism elements, instead of requiring a JSON file. Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
This commit is contained in:
Starlight220
@@ -1,50 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.simulation;
|
||||
|
||||
import edu.wpi.first.hal.SimDevice;
|
||||
import edu.wpi.first.hal.SimDevice.Direction;
|
||||
import edu.wpi.first.hal.SimDouble;
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
|
||||
public class Mechanism2D {
|
||||
private static final SimDevice m_device = SimDevice.create("Mechanism2D");
|
||||
private static final Map<String, SimDouble> m_createdItems = new HashMap<String, SimDouble>();
|
||||
|
||||
/**
|
||||
* Set/Create the angle of a ligament.
|
||||
*
|
||||
* @param ligamentPath json path to the ligament
|
||||
* @param angle to set the ligament
|
||||
*/
|
||||
public void setLigamentAngle(String ligamentPath, float angle) {
|
||||
ligamentPath = ligamentPath + "/angle";
|
||||
if (m_device != null) {
|
||||
if (!m_createdItems.containsKey(ligamentPath)) {
|
||||
m_createdItems.put(
|
||||
ligamentPath, m_device.createDouble(ligamentPath, Direction.kInput, angle));
|
||||
}
|
||||
m_createdItems.get(ligamentPath).set(angle);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Set/Create the length of a ligament.
|
||||
*
|
||||
* @param ligamentPath json path to the ligament
|
||||
* @param length to set the ligament
|
||||
*/
|
||||
public void setLigamentLength(String ligamentPath, float length) {
|
||||
ligamentPath = ligamentPath + "/length";
|
||||
if (m_device != null) {
|
||||
if (!m_createdItems.containsKey(ligamentPath)) {
|
||||
m_createdItems.put(
|
||||
ligamentPath, m_device.createDouble(ligamentPath, Direction.kInput, length));
|
||||
}
|
||||
m_createdItems.get(ligamentPath).set(length);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,107 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.smartdashboard;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.wpilibj.Sendable;
|
||||
import edu.wpi.first.wpilibj.util.Color8Bit;
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
import java.util.Map.Entry;
|
||||
|
||||
/**
|
||||
* Visual 2D representation of arms, elevators, and general mechanisms; through a node-based API.
|
||||
*
|
||||
* <p>A Mechanism2d object is published and contains at least one root node. Other nodes (such as
|
||||
* ligaments) are recursively based on other nodes.
|
||||
*
|
||||
* @see MechanismObject2d
|
||||
* @see MechanismLigament2d
|
||||
* @see MechanismRoot2d
|
||||
*/
|
||||
public final class Mechanism2d implements Sendable {
|
||||
private static final String kBackgroundColor = "backgroundColor";
|
||||
private NetworkTable m_table;
|
||||
private final Map<String, MechanismRoot2d> m_roots;
|
||||
private final double[] m_dims = new double[2];
|
||||
private String m_color;
|
||||
|
||||
/**
|
||||
* Create a new Mechanism2d with the given dimensions and default color (dark blue).
|
||||
*
|
||||
* @param width the width
|
||||
* @param height the height
|
||||
*/
|
||||
public Mechanism2d(double width, double height) {
|
||||
this(width, height, new Color8Bit(0, 0, 32));
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Mechanism2d with the given dimensions.
|
||||
*
|
||||
* @param width the width
|
||||
* @param height the height
|
||||
* @param backgroundColor the background color. Defaults to dark blue.
|
||||
*/
|
||||
public Mechanism2d(double width, double height, Color8Bit backgroundColor) {
|
||||
m_roots = new HashMap<>();
|
||||
m_dims[0] = width;
|
||||
m_dims[1] = height;
|
||||
setBackgroundColor(backgroundColor);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get or create a root in this Mechanism2d with the given name and position.
|
||||
*
|
||||
* <p>If a root with the given name already exists, the given x and y coordinates are not used.
|
||||
*
|
||||
* @param name the root name
|
||||
* @param x the root x coordinate
|
||||
* @param y the root y coordinate
|
||||
* @return a new root joint object, or the existing one with the given name.
|
||||
*/
|
||||
public synchronized MechanismRoot2d getRoot(String name, double x, double y) {
|
||||
var existing = m_roots.get(name);
|
||||
if (existing != null) {
|
||||
return existing;
|
||||
}
|
||||
|
||||
var root = new MechanismRoot2d(name, x, y);
|
||||
m_roots.put(name, root);
|
||||
if (m_table != null) {
|
||||
root.update(m_table.getSubTable(name));
|
||||
}
|
||||
return root;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the Mechanism2d background color.
|
||||
*
|
||||
* @param color the new color
|
||||
*/
|
||||
public synchronized void setBackgroundColor(Color8Bit color) {
|
||||
this.m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
|
||||
if (m_table != null) {
|
||||
m_table.getEntry(kBackgroundColor).setString(m_color);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("Mechanism2d");
|
||||
m_table = builder.getTable();
|
||||
m_table.getEntry("dims").setDoubleArray(m_dims);
|
||||
m_table.getEntry(kBackgroundColor).setString(m_color);
|
||||
synchronized (this) {
|
||||
for (Entry<String, MechanismRoot2d> entry : m_roots.entrySet()) {
|
||||
String name = entry.getKey();
|
||||
MechanismRoot2d root = entry.getValue();
|
||||
synchronized (root) {
|
||||
root.update(m_table.getSubTable(name));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,140 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.smartdashboard;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.util.Color8Bit;
|
||||
|
||||
/**
|
||||
* Ligament node on a Mechanism2d.
|
||||
*
|
||||
* @see Mechanism2d
|
||||
*/
|
||||
public class MechanismLigament2d extends MechanismObject2d {
|
||||
private double m_angle;
|
||||
private NetworkTableEntry m_angleEntry;
|
||||
private String m_color;
|
||||
private NetworkTableEntry m_colorEntry;
|
||||
private double m_length;
|
||||
private NetworkTableEntry m_lengthEntry;
|
||||
private double m_weight;
|
||||
private NetworkTableEntry m_weightEntry;
|
||||
|
||||
/** Create a new ligament. */
|
||||
public MechanismLigament2d(
|
||||
String name, double length, double angle, double lineWidth, Color8Bit color) {
|
||||
super(name);
|
||||
setColor(color);
|
||||
setLength(length);
|
||||
setAngle(angle);
|
||||
setLineWeight(lineWidth);
|
||||
}
|
||||
|
||||
/** Create a new ligament with the default color (dark blue) and thickness (6). */
|
||||
public MechanismLigament2d(String name, double length, double angle) {
|
||||
this(name, length, angle, 10, new Color8Bit(235, 137, 52));
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the ligament's angle relative to its parent.
|
||||
*
|
||||
* @param degrees the angle, in degrees
|
||||
*/
|
||||
public synchronized void setAngle(double degrees) {
|
||||
m_angle = degrees;
|
||||
flush();
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the ligament's angle relative to its parent.
|
||||
*
|
||||
* @param angle the angle
|
||||
*/
|
||||
public synchronized void setAngle(Rotation2d angle) {
|
||||
setAngle(angle.getDegrees());
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the ligament's angle relative to its parent.
|
||||
*
|
||||
* @return the angle, in degrees
|
||||
*/
|
||||
public synchronized double getAngle() {
|
||||
if (m_angleEntry != null) {
|
||||
m_angle = m_angleEntry.getDouble(0.0);
|
||||
}
|
||||
return m_angle;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the ligament's length.
|
||||
*
|
||||
* @param length the line length
|
||||
*/
|
||||
public synchronized void setLength(double length) {
|
||||
m_length = length;
|
||||
flush();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the ligament length.
|
||||
*
|
||||
* @return the line length
|
||||
*/
|
||||
public synchronized double getLength() {
|
||||
if (m_lengthEntry != null) {
|
||||
m_length = m_lengthEntry.getDouble(0.0);
|
||||
}
|
||||
return m_length;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the ligament color.
|
||||
*
|
||||
* @param color the color of the line
|
||||
*/
|
||||
public synchronized void setColor(Color8Bit color) {
|
||||
m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
|
||||
flush();
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the line thickness.
|
||||
*
|
||||
* @param weight the line thickness
|
||||
*/
|
||||
public synchronized void setLineWeight(double weight) {
|
||||
m_weight = weight;
|
||||
flush();
|
||||
}
|
||||
|
||||
@Override
|
||||
protected void updateEntries(NetworkTable table) {
|
||||
table.getEntry(".type").setString("line");
|
||||
m_angleEntry = table.getEntry("angle");
|
||||
m_lengthEntry = table.getEntry("length");
|
||||
m_colorEntry = table.getEntry("color");
|
||||
m_weightEntry = table.getEntry("weight");
|
||||
flush();
|
||||
}
|
||||
|
||||
/** Flush latest data to NT. */
|
||||
private synchronized void flush() {
|
||||
if (m_angleEntry != null) {
|
||||
m_angleEntry.setDouble(m_angle);
|
||||
}
|
||||
if (m_lengthEntry != null) {
|
||||
m_lengthEntry.setDouble(m_length);
|
||||
}
|
||||
if (m_colorEntry != null) {
|
||||
m_colorEntry.setString(m_color);
|
||||
}
|
||||
if (m_weightEntry != null) {
|
||||
m_weightEntry.setDouble(m_weight);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,72 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.smartdashboard;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
|
||||
/**
|
||||
* Common base class for all Mechanism2d node types.
|
||||
*
|
||||
* <p>To append another node, call {@link #append(MechanismObject2d)}. Objects that aren't appended
|
||||
* to a published {@link Mechanism2d} container are nonfunctional.
|
||||
*
|
||||
* @see Mechanism2d
|
||||
*/
|
||||
public abstract class MechanismObject2d {
|
||||
/** Relative to parent. */
|
||||
private final String m_name;
|
||||
|
||||
private NetworkTable m_table;
|
||||
private final Map<String, MechanismObject2d> m_objects = new HashMap<>(1);
|
||||
|
||||
/**
|
||||
* Create a new Mechanism node object.
|
||||
*
|
||||
* @param name the node's name, must be unique.
|
||||
*/
|
||||
protected MechanismObject2d(String name) {
|
||||
m_name = name;
|
||||
}
|
||||
|
||||
/**
|
||||
* Append a Mechanism object that is based on this one.
|
||||
*
|
||||
* @param object the object to add.
|
||||
* @return the object given as a parameter, useful for variable assignments and call chaining.
|
||||
* @throws UnsupportedOperationException if the object's name is already used - object names must
|
||||
* be unique.
|
||||
*/
|
||||
public final synchronized <T extends MechanismObject2d> T append(T object) {
|
||||
if (m_objects.containsKey(object.getName())) {
|
||||
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 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 all entries with new ones from a new table.
|
||||
*
|
||||
* @param table the new table.
|
||||
*/
|
||||
protected abstract void updateEntries(NetworkTable table);
|
||||
|
||||
public final String getName() {
|
||||
return m_name;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.smartdashboard;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
|
||||
/**
|
||||
* Root Mechanism2d node.
|
||||
*
|
||||
* <p>Do not create objects of this class directly! Obtain instances from the {@link
|
||||
* Mechanism2d#getRoot(String, double, double)} factory method.
|
||||
*
|
||||
* <p>Append other nodes by using {@link #append(MechanismObject2d)}.
|
||||
*/
|
||||
public final class MechanismRoot2d {
|
||||
private final String m_name;
|
||||
private NetworkTable m_table;
|
||||
private final Map<String, MechanismObject2d> m_objects = new HashMap<>(1);
|
||||
private double m_x;
|
||||
private NetworkTableEntry m_xEntry;
|
||||
private double m_y;
|
||||
private NetworkTableEntry m_yEntry;
|
||||
|
||||
/**
|
||||
* Package-private constructor for roots.
|
||||
*
|
||||
* @param name name
|
||||
* @param x x coordinate of root (provide only when constructing a root node)
|
||||
* @param y y coordinate of root (provide only when constructing a root node)
|
||||
*/
|
||||
MechanismRoot2d(String name, double x, double y) {
|
||||
m_name = name;
|
||||
m_x = x;
|
||||
m_y = y;
|
||||
}
|
||||
|
||||
/**
|
||||
* Append a Mechanism object that is based on this one.
|
||||
*
|
||||
* @param object the object to add.
|
||||
* @return the object given as a parameter, useful for variable assignments and call chaining.
|
||||
* @throws UnsupportedOperationException if the object's name is already used - object names must
|
||||
* be unique.
|
||||
*/
|
||||
public synchronized <T extends MechanismObject2d> T append(T object) {
|
||||
if (m_objects.containsKey(object.getName())) {
|
||||
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;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the root's position.
|
||||
*
|
||||
* @param x new x coordinate
|
||||
* @param y new y coordinate
|
||||
*/
|
||||
public synchronized void setPosition(double x, double y) {
|
||||
m_x = x;
|
||||
m_y = y;
|
||||
}
|
||||
|
||||
void update(NetworkTable table) {
|
||||
m_table = table;
|
||||
m_xEntry = m_table.getEntry("x");
|
||||
m_yEntry = m_table.getEntry("y");
|
||||
flush();
|
||||
for (MechanismObject2d obj : m_objects.values()) {
|
||||
obj.update(m_table.getSubTable(obj.getName()));
|
||||
}
|
||||
}
|
||||
|
||||
public String getName() {
|
||||
return m_name;
|
||||
}
|
||||
|
||||
private void flush() {
|
||||
if (m_xEntry != null) {
|
||||
m_xEntry.setDouble(m_x);
|
||||
}
|
||||
if (m_yEntry != null) {
|
||||
m_yEntry.setDouble(m_y);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user