Skip to content

Commit

Permalink
added gripper motor to MarlinRobotArm
Browse files Browse the repository at this point in the history
  • Loading branch information
@i-make-robots
i-make-robots committed Dec 25, 2023
1 parent 7ed8338 commit 97733ca
Show file tree
Hide file tree
Showing 6 changed files with 64 additions and 27 deletions.
18 changes: 11 additions & 7 deletions src/main/java/com/marginallyclever/ro3/apps/render/Viewport.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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() {
Expand Down Expand Up @@ -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)) {}
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down
1 change: 0 additions & 1 deletion src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawHingeJoints.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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() {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/com/marginallyclever/ro3/node/nodes/HingeJoint.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
5 changes: 2 additions & 3 deletions src/main/java/com/marginallyclever/ro3/node/nodes/Material.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand All @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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() {
Expand Down
63 changes: 49 additions & 14 deletions src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand All @@ -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;
}
Expand All @@ -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");
}

Expand Down Expand Up @@ -117,6 +121,11 @@ public void getComponents(List<JComponent> list) {
targetSelector.addPropertyChangeListener("subject",(e)-> target = (Pose)e.getNewValue());
addLabelAndComponent(pane, "Target", targetSelector,gbc);

// add a selector for the target
NodeSelector<Motor> 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());
Expand Down Expand Up @@ -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();
}

Expand Down Expand Up @@ -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();
}

/**
Expand Down

0 comments on commit 97733ca

Please sign in to comment.