From 9cbf0ba05cc07e70c1aa64e1bdc8efdfcea6ceda Mon Sep 17 00:00:00 2001 From: Dan Royer Date: Wed, 20 Dec 2023 20:37:33 -0800 Subject: [PATCH] MarlinRobotArm can now receive GCode commands --- .../ro3/node/nodes/HingeJoint.java | 10 ++++- .../ro3/node/nodes/MarlinRobotArm.java | 45 ++++++++++++++++--- .../ro3/node/nodetreeview/NodeTreeView.java | 16 ++++--- 3 files changed, 56 insertions(+), 15 deletions(-) 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 49f0b4274..14c8e69e8 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java @@ -124,7 +124,9 @@ public JSONObject toJSON() { @Override public void fromJSON(JSONObject from) { super.fromJSON(from); - if(from.has("axle")) axle = Registry.findNodeByID(from.getString("axle"),Pose.class); + if(from.has("axle")) { + axle = Registry.findNodeByID(from.getString("axle"),Pose.class); + } if(from.has("angle")) angle = from.getDouble("angle"); if(from.has("minAngle")) minAngle = from.getDouble("minAngle"); if(from.has("maxAngle")) maxAngle = from.getDouble("maxAngle"); @@ -136,6 +138,10 @@ public double getAngle() { return angle; } + public void setAngle(double degrees) { + angle = degrees; + } + public double getMinAngle() { return minAngle; } @@ -159,4 +165,4 @@ public double getAcceleration() { public void setAcceleration(double acceleration) { this.acceleration = acceleration; } -} +} \ No newline at end of file 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 b1fa26642..d59f6ba1c 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/MarlinRobotArm.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/MarlinRobotArm.java @@ -76,19 +76,29 @@ public void getComponents(List list) { // Add a text field to send a position to the robot arm. JTextField output = new JTextField(); output.setEditable(false); - addLabelAndComponent(pane, "Output", output); + pane.add(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()) ); + JButton getFKButton = new JButton("Get"); + getFKButton.addActionListener(e-> output.setText(getFKAsGCode()) ); + pane.add(getFKButton); - // TODO add a text field that will be sent to the robot arm. + // Add a text field that will be sent to the robot arm. + JTextField input = new JTextField(); + pane.add(input); + // Add a button to send the text field to the robot arm. + JButton sendButton = new JButton("Send"); + sendButton.addActionListener(e-> output.setText(sendGCode(input.getText())) ); + pane.add(sendButton); super.getComponents(list); } - // take the current angle of each motor hinge and writes as a GCode command. + /** + * Build a string from the current angle of each motor hinge, aka the + * Forward Kinematics of the robot arm. + * @return GCode command + */ public String getFKAsGCode() { StringBuilder sb = new StringBuilder("G0"); for(Motor motor : motors) { @@ -100,4 +110,27 @@ public String getFKAsGCode() { } return sb.toString(); } + + /** + * Send gcode to robot arm. + * @param gcode GCode command + * @return response from robot arm + */ + public String sendGCode(String gcode) { + if(gcode.startsWith("G0")) { + // parse gcode and set motor angles + String [] parts = gcode.split("\\s+"); + for(Motor motor : motors) { + if(motor!=null) { + for(String p : parts) { + if(p.startsWith(motor.getName())) { + motor.getAxle().setAngle(Double.parseDouble(p.substring(motor.getName().length()))); + } + } + } + } + return "Ok"; + } + return "Error: unknown command"; + } } diff --git a/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeView.java b/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeView.java index 81c7bf151..b9e4115e4 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeView.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeView.java @@ -90,6 +90,12 @@ private void stopListeningTo(Node node) { node.removeAttachListener(this); node.removeDetachListener(this); node.removeRenameListener(this); + + // stop listening to all the children of this node, a reverse of scanTree() + List toRemove = new ArrayList<>(node.getChildren()); + for(Node progeny : toRemove) { + stopListeningTo(progeny); + } } /** @@ -167,10 +173,7 @@ public void nodeAttached(Node child) { int index = parent.getChildren().indexOf(child); branchParent.insert(branchChild,index); - child.addAttachListener(this); - child.addDetachListener(this); - child.addRenameListener(this); - + listenTo(child); scanTree(child); ((DefaultTreeModel) tree.getModel()).nodeStructureChanged(branchParent); @@ -179,9 +182,8 @@ public void nodeAttached(Node child) { @Override public void nodeDetached(Node child) { logger.debug("Detached "+child.getAbsolutePath()); - child.removeAttachListener(this); - child.removeDetachListener(this); - child.removeRenameListener(this); + + stopListeningTo(child); Node parent = child.getParent(); if(parent==null) throw new RuntimeException("Source node has no parent");