From 53ba58f79591828ad345454a3d00f29fd20a1c24 Mon Sep 17 00:00:00 2001 From: Dan Royer Date: Wed, 20 Dec 2023 19:53:42 -0800 Subject: [PATCH] MarlinRobotArm can now read FK --- .../com/marginallyclever/ro3/Registry.java | 4 +- .../com/marginallyclever/ro3/node/Node.java | 15 ++++- .../ro3/node/nodes/HingeJoint.java | 9 +-- .../ro3/node/nodes/MarlinRobotArm.java | 64 +++++++++++++------ .../ro3/node/nodes/Motor.java | 8 +++ .../nodetreeview/NodeTreeTransferHandler.java | 6 +- 6 files changed, 77 insertions(+), 29 deletions(-) diff --git a/src/main/java/com/marginallyclever/ro3/Registry.java b/src/main/java/com/marginallyclever/ro3/Registry.java index 3f77bf8c7..37593882b 100644 --- a/src/main/java/com/marginallyclever/ro3/Registry.java +++ b/src/main/java/com/marginallyclever/ro3/Registry.java @@ -106,8 +106,8 @@ public static T findNodeByID(String nodeID, Class type) { toScan.add(scene); while(!toScan.isEmpty()) { Node node = toScan.remove(0); - if(node.getNodeID().toString().equals(nodeID)) { - if(type.equals(node.getClass())) { + if(type.equals(node.getClass())) { + if(node.getNodeID().toString().equals(nodeID)) { return type.cast(node); } } diff --git a/src/main/java/com/marginallyclever/ro3/node/Node.java b/src/main/java/com/marginallyclever/ro3/node/Node.java index e7fb9e4dd..16437281e 100644 --- a/src/main/java/com/marginallyclever/ro3/node/Node.java +++ b/src/main/java/com/marginallyclever/ro3/node/Node.java @@ -25,7 +25,7 @@ public class Node { private final List children = new ArrayList<>(); private Node parent; private UUID nodeID; - private final EventListenerList listeners = new EventListenerList(); + protected final EventListenerList listeners = new EventListenerList(); public Node() { this("Node"); @@ -391,8 +391,19 @@ public void fromJSON(JSONObject from) { logger.error("{}: Could not create type {}.",getAbsolutePath(),child.getString("type")); n = new Node(); } - n.fromJSON(child); addChild(n); + n.fromJSON(child); + } + } + + public boolean hasParent(Node beingMoved) { + Node p = parent; + while(p != null) { + if(p == beingMoved) { + return true; + } + p = p.getParent(); } + return false; } } diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java b/src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java index 43f3d99bc..49f0b4274 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java @@ -18,10 +18,11 @@ * {@link HingeJoint} is a joint that can rotate around the local Z axis. */ public class HingeJoint extends Node { - private double angle = 0; - private double minAngle = 0; - private double maxAngle = 360; - private double velocity=0, acceleration=0; + private double angle = 0; // degrees + private double minAngle = 0; // degrees + private double maxAngle = 360; // degrees + private double velocity = 0; // degrees/s + private double acceleration = 0; // degrees/s/s private Pose axle; public HingeJoint() { diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/MarlinRobotArm.java b/src/main/java/com/marginallyclever/ro3/node/nodes/MarlinRobotArm.java index b6f61ba35..b1fa26642 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/MarlinRobotArm.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/MarlinRobotArm.java @@ -8,6 +8,7 @@ import org.json.JSONObject; import javax.swing.*; +import javax.swing.event.EventListenerList; import java.awt.*; import java.util.ArrayList; import java.util.List; @@ -30,25 +31,6 @@ public MarlinRobotArm(String name) { } } - @Override - public void getComponents(List list) { - CollapsiblePanel panel = new CollapsiblePanel(MarlinRobotArm.class.getSimpleName()); - list.add(panel); - JPanel pane = panel.getContentPane(); - - pane.setLayout(new GridLayout(0, 2)); - - var motorSelector = new NodeSelector[MAX_JOINTS]; - for(int i=0;i(Motor.class, motors.get(i)); - int j = i; - motorSelector[i].addPropertyChangeListener("subject",(e)-> motors.set(j,(Motor)e.getNewValue())); - addLabelAndComponent(pane, "Joint "+i, motorSelector[i]); - } - - super.getComponents(list); - } - @Override public JSONObject toJSON() { JSONObject json = super.toJSON(); @@ -74,4 +56,48 @@ public void fromJSON(JSONObject from) { } } } + + @Override + public void getComponents(List list) { + CollapsiblePanel panel = new CollapsiblePanel(MarlinRobotArm.class.getSimpleName()); + list.add(panel); + JPanel pane = panel.getContentPane(); + + pane.setLayout(new GridLayout(0, 2)); + + var motorSelector = new NodeSelector[MAX_JOINTS]; + for(int i=0;i(Motor.class, motors.get(i)); + int j = i; + motorSelector[i].addPropertyChangeListener("subject",(e)-> motors.set(j,(Motor)e.getNewValue())); + addLabelAndComponent(pane, "Motor "+i, motorSelector[i]); + } + + // Add a text field to send a position to the robot arm. + JTextField output = new JTextField(); + output.setEditable(false); + addLabelAndComponent(pane, "Output", output); + + // Add a button that displays gcode to the output. + JButton gcodeButton = new JButton("Get"); + addLabelAndComponent(pane, "FK as GCode", gcodeButton); + gcodeButton.addActionListener(e-> output.setText(getFKAsGCode()) ); + + // TODO add a text field that will be sent to the robot arm. + + super.getComponents(list); + } + + // take the current angle of each motor hinge and writes as a GCode command. + public String getFKAsGCode() { + StringBuilder sb = new StringBuilder("G0"); + for(Motor motor : motors) { + if(motor!=null) { + sb.append(" ") + .append(motor.getName()) + .append(motor.getAxle().getAngle()); + } + } + return sb.toString(); + } } diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java b/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java index de891be3f..c20ca17d6 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java @@ -68,4 +68,12 @@ public void fromJSON(JSONObject from) { super.fromJSON(from); if(from.has("hinge")) hinge = Registry.findNodeByID(from.getString("hinge"),HingeJoint.class); } + + public HingeJoint getAxle() { + return hinge; + } + + public void setAxle(HingeJoint hinge) { + this.hinge = hinge; + } } diff --git a/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeTransferHandler.java b/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeTransferHandler.java index 5fb7999ea..18fc75408 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeTransferHandler.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeTransferHandler.java @@ -57,9 +57,11 @@ public boolean importData(TransferHandler.TransferSupport support) { Transferable transferable = support.getTransferable(); Node beingMoved = (Node) transferable.getTransferData(NodeTransferable.nodeFlavor); - // Prevent a node from being dragged to itself if (beingMoved == newParent) { - return false; + return false; // Prevent a node from being dragged to itself + } + if( newParent.hasParent(beingMoved) ) { + return false; // i can't become my own grandpa } // Remove node from its current parent