MarlinRobotArm can now read FK

MarginallyClever · Dec 21, 2023 · 53ba58f · 53ba58f
1 parent d267ae7
commit 53ba58f
Show file tree

Hide file tree

Showing 6 changed files with 77 additions and 29 deletions.
diff --git 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 extends Node> T findNodeByID(String nodeID, Class<T> 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
@@ -25,7 +25,7 @@ public class Node {
     private final List<Node> 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
@@ -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
@@ -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<JComponent> 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<MAX_JOINTS;++i) {
-            motorSelector[i] = new NodeSelector<>(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<JComponent> 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<MAX_JOINTS;++i) {
+            motorSelector[i] = new NodeSelector<>(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
@@ -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
@@ -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