Update MarlinRobotArm.java

MarginallyClever · Dec 23, 2023 · 2d897d4 · 2d897d4
1 parent 907f95d
commit 2d897d4
Showing 1 changed file with 23 additions and 19 deletions.
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
@@ -18,7 +18,6 @@
 import javax.vecmath.Matrix4d;
 import javax.vecmath.Vector3d;
 import java.awt.*;
-import java.awt.event.ActionEvent;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.List;
@@ -210,24 +209,10 @@ public void sendGCode(String gcode) {
             fireMarlinMessage( "Ok: "+response );
             return;
         } else if(gcode.equals("ik")) {
-            if(endEffector==null) {
-                fireMarlinMessage( "Error: no end effector" );
-                return;
-            }
-            double [] cartesian = getCartesianFromWorld(endEffector.getWorld());
-            int i=0;
-            String response = "G1"
-                    +" X"+StringHelper.formatDouble(cartesian[i++])
-                    +" Y"+StringHelper.formatDouble(cartesian[i++])
-                    +" Z"+StringHelper.formatDouble(cartesian[i++])
-                    +" U"+StringHelper.formatDouble(cartesian[i++])
-                    +" V"+StringHelper.formatDouble(cartesian[i++])
-                    +" W"+StringHelper.formatDouble(cartesian[i++]);
-            fireMarlinMessage( "Ok: "+response );
-            return;
+            fireMarlinMessage(getEndEffectorIK());
         } else if(gcode.equals("aj")) {
             ApproximateJacobianFiniteDifferences jacobian = new ApproximateJacobianFiniteDifferences(this);
-            fireMarlinMessage( "Ok: "+jacobian.toString() );
+            fireMarlinMessage( "Ok: "+jacobian );
             return;
         } else if(gcode.startsWith("G1")) {
             fireMarlinMessage( parseG1(gcode) );
@@ -263,8 +248,8 @@ private String parseG0(String gcode) {
 
     /**
      * <p>G1 Linear move.</p>
-     * <p>Parse gcode for names and values, then set the new target position.Names are XYZ for linear, UVW for angular.
-     * Angular values should be in degrees.</p>
+     * <p>Parse gcode for names and values, then set the new target position.  Names are XYZ for linear, UVW for
+     * angular. Angular values should be in degrees.</p>
      * <p>Movement will occur on {@link #update(double)} provided the {@link #linearVelocity} and the update time are
      * greater than zero.</p>
      * @param gcode GCode command
@@ -293,6 +278,23 @@ private String parseG1(String gcode) {
         return "Ok";
     }
 
+    private String getEndEffectorIK() {
+        if(endEffector==null) {
+            return ( "Error: no end effector" );
+        }
+        double [] cartesian = getCartesianFromWorld(endEffector.getWorld());
+        int i=0;
+        String response = "G1"
+                +" F"+StringHelper.formatDouble(linearVelocity)
+                +" X"+StringHelper.formatDouble(cartesian[i++])
+                +" Y"+StringHelper.formatDouble(cartesian[i++])
+                +" Z"+StringHelper.formatDouble(cartesian[i++])
+                +" U"+StringHelper.formatDouble(cartesian[i++])
+                +" V"+StringHelper.formatDouble(cartesian[i++])
+                +" W"+StringHelper.formatDouble(cartesian[i++]);
+        return ( "Ok: "+response );
+    }
+
     /**
      * @param cartesian XYZ translation and UVW rotation of the end effector.  UVW is in degrees.
      * @return the matrix that represents the given cartesian position.
@@ -384,6 +386,7 @@ public void setTarget(Pose target) {
     @Override
     public void update(double dt) {
         super.update(dt);
+        if(dt==0) return;
         moveTowardsTarget(dt);
     }
 
@@ -422,6 +425,7 @@ public static void capVectorToMagnitude(double[] vector, double maxLen) {
      * @throws RuntimeException if the robot cannot be moved in the direction of the cartesian force.
      */
     public void moveEndEffectorInCartesianDirection(double[] cartesianVelocity) {
+        // is it a really small move?
         double sum = sumCartesianVelocityComponents(cartesianVelocity);
         if(sum<0.0001) return;
         if(sum <= 1) {