From 97733caa9d446f0678a01200890a08eb4e76c0a1 Mon Sep 17 00:00:00 2001 From: Dan Royer Date: Sun, 24 Dec 2023 20:34:07 -0800 Subject: [PATCH] added gripper motor to MarlinRobotArm --- .../ro3/apps/render/Viewport.java | 18 +++--- .../render/renderpasses/DrawHingeJoints.java | 1 - .../ro3/node/nodes/HingeJoint.java | 2 +- .../ro3/node/nodes/Material.java | 5 +- .../ro3/node/nodes/Motor.java | 2 +- .../nodes/marlinrobotarm/MarlinRobotArm.java | 63 ++++++++++++++----- 6 files changed, 64 insertions(+), 27 deletions(-) diff --git a/src/main/java/com/marginallyclever/ro3/apps/render/Viewport.java b/src/main/java/com/marginallyclever/ro3/apps/render/Viewport.java index 10444b1d7..da46edb0d 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/render/Viewport.java +++ b/src/main/java/com/marginallyclever/ro3/apps/render/Viewport.java @@ -57,8 +57,8 @@ private void addRenderPasses() { renderPasses.add(new DrawBoundingBoxes()); renderPasses.add(new DrawCameras()); renderPasses.add(new DrawDHParameters()); - renderPasses.add(new DrawHingeJoints()); renderPasses.add(new DrawPoses()); + renderPasses.add(new DrawHingeJoints()); } private void allocateButtonMemory() { @@ -257,13 +257,17 @@ public void mouseReleased(MouseEvent e) { @Override public void mouseDragged(MouseEvent e) { int px = e.getX(); - int dx = px - mx; + double dx = px - mx; mx = px; int py = e.getY(); - int dy = py - my; + double dy = py - my; my = py; + // scale based on orbit distance - smaller orbits need smaller movements + dx *= orbitRadius / 50d; + dy *= orbitRadius / 50d; + boolean shift = (e.getModifiersEx() & MouseEvent.SHIFT_DOWN_MASK) != 0; //if(buttonPressed.get(MouseEvent.BUTTON1)) {} @@ -309,7 +313,7 @@ private void changeOrbitRadius(int dz) { local.setTranslation(orbitVector); } - private void panTiltCamera(int dx, int dy) { + private void panTiltCamera(double dx, double dy) { Matrix4d local = camera.getLocal(); Vector3d t = new Vector3d(); local.get(t); @@ -324,10 +328,10 @@ private void panTiltCamera(int dx, int dy) { /** * Orbit the camera around a point orbitRadius ahead of the camera. - * @param dx mouse movement in x - * @param dy mouse movement in y + * @param dx change in x + * @param dy change in y */ - void orbitCamera(int dx,int dy) { + void orbitCamera(double dx,double dy) { Matrix4d local = camera.getLocal(); Vector3d orbitPoint = getOrbitPoint(); //logger.debug("before {}",orbitPoint); diff --git a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawHingeJoints.java b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawHingeJoints.java index 1ee0cd389..dc0194bc5 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawHingeJoints.java +++ b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawHingeJoints.java @@ -25,7 +25,6 @@ public class DrawHingeJoints extends AbstractRenderPass { private final Mesh mesh = new Mesh(); private final Mesh circleFan = new Mesh(); private ShaderProgram shader; - private int canvasWidth,canvasHeight; private final float ringScale = 3; public DrawHingeJoints() { 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 de466bc51..a4b6e9656 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java @@ -121,7 +121,7 @@ public JSONObject toJSON() { public void fromJSON(JSONObject from) { super.fromJSON(from); if(from.has("axle")) { - axle = Registry.findNodeByID(from.getString("axle"),Pose.class); + axle = this.getRootNode().findNodeByID(from.getString("axle"),Pose.class); } if(from.has("angle")) angle = from.getDouble("angle"); if(from.has("minAngle")) minAngle = from.getDouble("minAngle"); diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/Material.java b/src/main/java/com/marginallyclever/ro3/node/nodes/Material.java index 4796c420f..8ed605c9d 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/Material.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/Material.java @@ -109,7 +109,7 @@ public TextureWithMetadata getTexture() { @Override public JSONObject toJSON() { JSONObject json = super.toJSON(); - json.put("texture",texture.getSource()); + if(texture!=null) json.put("texture",texture.getSource()); json.put("diffuseColor", diffuseColor.getRGB()); json.put("specularColor", specularColor.getRGB()); json.put("ambientColor", ambientColor.getRGB()); @@ -119,8 +119,7 @@ public JSONObject toJSON() { @Override public void fromJSON(JSONObject from) { super.fromJSON(from); - String textureSource = from.getString("texture"); - texture = Registry.textureFactory.load(textureSource); + if(from.has("texture")) texture = Registry.textureFactory.load(from.getString("texture")); if(from.has("diffuseColor")) diffuseColor = new Color(from.getInt("diffuseColor"),true); if(from.has("specularColor")) specularColor = new Color(from.getInt("specularColor"),true); if(from.has("ambientColor")) ambientColor = new Color(from.getInt("ambientColor"),true); 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 38e906051..dc7d66b24 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java @@ -70,7 +70,7 @@ public JSONObject toJSON() { @Override public void fromJSON(JSONObject from) { super.fromJSON(from); - if(from.has("hinge")) hinge = Registry.findNodeByID(from.getString("hinge"),HingeJoint.class); + if(from.has("hinge")) hinge = this.getRootNode().findNodeByID(from.getString("hinge"),HingeJoint.class); } public HingeJoint getAxle() { diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArm.java b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArm.java index 1a2e75700..78812b82b 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArm.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArm.java @@ -41,6 +41,7 @@ public class MarlinRobotArm extends Node { private Pose endEffector; private Pose target; private double linearVelocity=0; + private Motor gripperMotor; public MarlinRobotArm() { this("MarlinRobotArm"); @@ -61,6 +62,7 @@ public JSONObject toJSON() { json.put("motors",jointArray); if(endEffector!=null) json.put("endEffector",endEffector.getNodeID()); if(target!=null) json.put("target",target.getNodeID()); + if(gripperMotor!=null) json.put("gripperMotor",gripperMotor.getNodeID()); json.put("linearVelocity",linearVelocity); return json; } @@ -74,12 +76,14 @@ public void fromJSON(JSONObject from) { if(motorArray.isNull(i)) { motors.set(i,null); } else { - motors.set(i,Registry.findNodeByID(motorArray.getString(i),Motor.class)); + motors.set(i,this.getRootNode().findNodeByID(motorArray.getString(i),Motor.class)); } } } - if(from.has("endEffector")) endEffector = Registry.findNodeByID(from.getString("endEffector"),Pose.class); - if(from.has("target")) target = Registry.findNodeByID(from.getString("target"),Pose.class); + Node root = this.getRootNode(); + if(from.has("endEffector")) endEffector = root.findNodeByID(from.getString("endEffector"),Pose.class); + if(from.has("target")) target = root.findNodeByID(from.getString("target"),Pose.class); + if(from.has("gripperMotor")) gripperMotor = root.findNodeByID(from.getString("gripperMotor"),Motor.class); if(from.has("linearVelocity")) linearVelocity = from.getDouble("linearVelocity"); } @@ -117,6 +121,11 @@ public void getComponents(List list) { targetSelector.addPropertyChangeListener("subject",(e)-> target = (Pose)e.getNewValue()); addLabelAndComponent(pane, "Target", targetSelector,gbc); + // add a selector for the target + NodeSelector gripperMotorSelector = new NodeSelector<>(Motor.class, gripperMotor); + gripperMotorSelector.addPropertyChangeListener("subject",(e)-> gripperMotor = (Motor)e.getNewValue()); + addLabelAndComponent(pane, "Gripper motor", gripperMotorSelector,gbc); + // add a slider to control linear velocity JSlider slider = new JSlider(0,20,(int)linearVelocity); slider.addChangeListener(e-> linearVelocity = slider.getValue()); @@ -183,14 +192,26 @@ protected void addLabelAndComponent(JPanel pane, String labelText, JComponent co * @return GCode command */ private String getM114() { - StringBuilder sb = new StringBuilder("M114"); + return "M114"+getMotorsAndFeedrateAsString(); + } + + private String getMotorsAndFeedrateAsString() { + StringBuilder sb = new StringBuilder(); for(Motor motor : motors) { if(motor!=null && motor.hasAxle()) { sb.append(" ") - .append(motor.getName()) - .append(StringHelper.formatDouble(motor.getAxle().getAngle())); + .append(motor.getName()) + .append(StringHelper.formatDouble(motor.getAxle().getAngle())); } } + // gripper motor + if(gripperMotor!=null && gripperMotor.hasAxle()) { + sb.append(" ") + .append(gripperMotor.getName()) + .append(StringHelper.formatDouble(gripperMotor.getAxle().getAngle())); + } + // feedrate + sb.append(" F").append(StringHelper.formatDouble(linearVelocity)); return sb.toString(); } @@ -230,21 +251,35 @@ public void sendGCode(String gcode) { */ private String parseG0(String gcode) { String [] parts = gcode.split("\\s+"); - for(Motor motor : motors) { - if(motor!=null && motor.hasAxle()) { - for(String p : parts) { - if(p.startsWith(motor.getName())) { - // TODO check for NaN, Infinity, etc + try { + for (Motor motor : motors) { + if (motor != null && motor.hasAxle()) { + for (String p : parts) { + if (p.startsWith(motor.getName())) { + // TODO check new value is in range. + motor.getAxle().setAngle(Double.parseDouble(p.substring(motor.getName().length()))); + break; + } + } + } + } + // gripper motor + if (gripperMotor != null && gripperMotor.hasAxle()) { + for (String p : parts) { + if (p.startsWith(gripperMotor.getName())) { // TODO check new value is in range. - motor.getAxle().setAngle(Double.parseDouble(p.substring(motor.getName().length()))); + gripperMotor.getAxle().setAngle(Double.parseDouble(p.substring(gripperMotor.getName().length()))); break; } } } + // else ignore unused parts + } catch( NumberFormatException e ) { + logger.error("Number format exception: "+e.getMessage()); + return "Error: "+e.getMessage(); } - // else ignore unused parts - return "Ok"; + return "Ok: G0"+getMotorsAndFeedrateAsString(); } /**