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 b83c17819..16a78076c 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 @@ -5,9 +5,12 @@ import com.marginallyclever.ro3.node.Node; import com.marginallyclever.ro3.node.NodePath; import com.marginallyclever.ro3.node.nodes.HingeJoint; -import com.marginallyclever.ro3.node.nodes.limbsolver.ApproximateJacobianFiniteDifferences; import com.marginallyclever.ro3.node.nodes.limbsolver.LimbSolver; import com.marginallyclever.ro3.node.nodes.Motor; +import com.marginallyclever.ro3.node.nodes.marlinsimulation.MarlinCoordinate; +import com.marginallyclever.ro3.node.nodes.marlinsimulation.MarlinSettings; +import com.marginallyclever.ro3.node.nodes.marlinsimulation.MarlinSimulation; +import com.marginallyclever.ro3.node.nodes.marlinsimulation.MarlinSimulationBlock; import com.marginallyclever.ro3.node.nodes.pose.Pose; import com.marginallyclever.ro3.node.nodes.pose.poses.Limb; import org.json.JSONObject; @@ -36,6 +39,12 @@ public class MarlinRobotArm extends Node { public final NodePath solver = new NodePath<>(this,LimbSolver.class); private final NodePath gripperMotor = new NodePath<>(this,Motor.class); private double reportInterval=1.0; // seconds + private final MarlinSettings settings = new MarlinSettings(); + private final MarlinSimulation simulation = new MarlinSimulation(settings); + private MarlinSimulationBlock currentBlock = null; + private double feedrate = settings.getDouble(MarlinSettings.MAX_FEEDRATE); + private double acceleration = settings.getDouble(MarlinSettings.MAX_ACCELERATION); + public MarlinRobotArm() { this("MarlinRobotArm"); @@ -197,30 +206,39 @@ private String parseG0(String gcode) { } String [] parts = gcode.split("\\s+"); try { + var destination = new MarlinCoordinate(); + + int i=0; for (NodePath paths : getLimb().getSubject().getMotors()) { Motor motor = paths.getSubject(); if (motor != null && motor.hasHinge()) { + String motorName = motor.getName(); for (String p : parts) { - if (p.startsWith(motor.getName())) { + if (p.startsWith(motorName)) { // TODO check new value is in range. - motor.getHinge().setAngle(Double.parseDouble(p.substring(motor.getName().length()))); + destination.p[i] = Double.parseDouble(p.substring(motor.getName().length())); break; } } } + i++; } // gripper motor Motor gripperMotor = this.gripperMotor.getSubject(); if (gripperMotor != null && gripperMotor.hasHinge()) { + String motorName = gripperMotor.getName(); for (String p : parts) { - if (p.startsWith(gripperMotor.getName())) { + if (p.startsWith(motorName)) { // TODO check new value is in range. - gripperMotor.getHinge().setAngle(Double.parseDouble(p.substring(gripperMotor.getName().length()))); + destination.p[i] = Double.parseDouble(p.substring(gripperMotor.getName().length())); break; } } + i++; } // else ignore unused parts + + simulation.bufferLine(destination,feedrate,acceleration); } catch( NumberFormatException e ) { logger.error("Number format exception: "+e.getMessage()); return "Error: "+e.getMessage(); @@ -232,9 +250,59 @@ private String parseG0(String gcode) { @Override public void update(double dt) { super.update(dt); - // TODO Simulate Marlin behavior here. - // Queue up GCode commands and send "Ok" at the appropriate time. + + // Simulate Marlin behavior. + if(currentBlock==null) { + currentBlock = findBlock(); + if(currentBlock!=null) { + logger.debug("starting block " + currentBlock.id); + currentBlock.busy = true; + } + } + if(currentBlock==null) return; + + // advance time in the block + currentBlock.now_s += dt; + double extra = currentBlock.now_s - currentBlock.end_s; + if (currentBlock.now_s >= currentBlock.end_s) { + // no overflow! + currentBlock.now_s = currentBlock.end_s; + } + // Drive motors using trapezoidal velocity profiles. + // update motors according to currentBlock + logger.debug("working block " + currentBlock.id); + int i=0; + for(NodePath paths : getLimb().getSubject().getMotors()) { + Motor motor = paths.getSubject(); + double fraction = currentBlock.now_s / currentBlock.end_s; + if(motor!=null && motor.hasHinge()) { + HingeJoint hinge = motor.getHinge(); + hinge.setAngle( + currentBlock.start.p[i] + currentBlock.delta.p[i] * fraction); + } + ++i; + } + + // is block done? + if (currentBlock.now_s >= currentBlock.end_s) { + logger.debug("ending block " + currentBlock.id); + currentBlock.busy = false; + simulation.getQueue().remove(currentBlock); + + currentBlock = findBlock(); + if(currentBlock!=null) { + logger.debug("starting block " + currentBlock.id); + currentBlock.busy = true; + currentBlock.now_s = extra; + } + } + + // Queue up GCode commands and send "Ok" at the appropriate time. + } + + private MarlinSimulationBlock findBlock() { + return (simulation.getQueue().isEmpty()) ? null : simulation.getQueue().peek(); } /** diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/marlinsimulation/MarlinCoordinate.java b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinsimulation/MarlinCoordinate.java new file mode 100644 index 000000000..3a5f72e94 --- /dev/null +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinsimulation/MarlinCoordinate.java @@ -0,0 +1,66 @@ +package com.marginallyclever.ro3.node.nodes.marlinsimulation; + +import javax.vecmath.Vector3d; +import java.util.Arrays; + +public class MarlinCoordinate { + public static final int SIZE=6; + public final double [] p = new double[SIZE]; + + public MarlinCoordinate() {} + + public MarlinCoordinate(MarlinCoordinate other) { + set(other); + } + + public void add(MarlinCoordinate other) { + for(int i=0;i 3000; + case MAX_FEEDRATE -> 5; + case MIN_SEGMENT_LENGTH -> 0.5; + case MINIMUM_PLANNER_SPEED -> 0.05; + default -> 0; + }; + } + + public int getInteger(int key) { + return switch (key) { + case SEGMENTS_PER_SECOND -> 5; + case BLOCK_BUFFER_SIZE -> 16; + case MIN_SEG_TIME -> 100; + default -> 0; + }; + } + + public boolean getBoolean(int key) { + if (key == HANDLE_SMALL_SEGMENTS) { + return false; + } + return false; + } +} diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/marlinsimulation/MarlinSimulation.java b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinsimulation/MarlinSimulation.java new file mode 100644 index 000000000..6d2fb3ede --- /dev/null +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinsimulation/MarlinSimulation.java @@ -0,0 +1,434 @@ +package com.marginallyclever.ro3.node.nodes.marlinsimulation; + +import com.marginallyclever.ro3.node.NodePath; +import com.marginallyclever.ro3.node.nodes.Motor; +import com.marginallyclever.ro3.node.nodes.pose.poses.Limb; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import javax.vecmath.Vector3d; +import java.util.ArrayList; +import java.util.Iterator; +import java.util.LinkedList; +import java.util.List; + +/** + *

{@link MarlinSimulation} is meant to be a 1:1 Java replica of Marlin's 'Planner' and 'Motor' classes. + * It is used to estimate the time to draw a set of gcode commands by a robot running Marlin 3D printer firmware.

+ *

Users should call {@link #bufferLine(MarlinCoordinate, double, double)}, which will add to the {@link #queue}. The queue + * must not exceed MarlinSettings#getInteger(PlotterSettings.BLOCK_BUFFER_SIZE) items in length. Consuming + * items from the head of the queue

+ */ +public class MarlinSimulation { + private static final Logger logger = LoggerFactory.getLogger(MarlinSimulation.class); + private final MarlinCoordinate poseNow = new MarlinCoordinate(); + private final LinkedList queue = new LinkedList<>(); + private MarlinCoordinate previousSpeed = new MarlinCoordinate(); + private double previousSafeSpeed = 0; + private final MarlinSettings settings; + + enum JerkType { + CLASSIC_JERK, + JUNCTION_DEVIATION, + DOT_PRODUCT, + NONE, + }; + private final JerkType jerkType = JerkType.CLASSIC_JERK; + + // Unit vector of previous path line segment + private final MarlinCoordinate previousNormal = new MarlinCoordinate(); + + private double previousNominalSpeed=0; + private double junction_deviation = 0.05; + + private final MarlinCoordinate maxJerk = new MarlinCoordinate(); + + public MarlinSimulation(MarlinSettings settings) { + this.settings = settings; + } + + /** + * Add this destination to the queue and attempt to optimize travel between destinations. + * @param destination destination (mm) + * @param feedrate (mm/s) + * @param acceleration (mm/s/s) + */ + public void bufferLine(final MarlinCoordinate destination, double feedrate, double acceleration) { + var delta = new MarlinCoordinate(); + delta.sub(destination,poseNow); + + acceleration = Math.min(settings.getDouble(MarlinSettings.MAX_ACCELERATION), acceleration); + + double len = delta.length(); + double seconds = len / feedrate; + int segments = (int)Math.ceil(seconds * settings.getInteger(MarlinSettings.SEGMENTS_PER_SECOND)); + int maxSeg = (int)Math.ceil(len / settings.getDouble(MarlinSettings.MIN_SEGMENT_LENGTH)); + segments = Math.max(1,Math.min(maxSeg,segments)); + var deltaSegment = new MarlinCoordinate(delta); + deltaSegment.scale(1.0/segments); + + var temp = new MarlinCoordinate(poseNow); + while(--segments>0) { + temp.add(deltaSegment); + bufferSegment(temp,feedrate,acceleration,deltaSegment); + } + bufferSegment(destination,feedrate,acceleration,deltaSegment); + } + + /** + * return the queue of blocks. + * @return the queue of blocks. + */ + public LinkedList getQueue() { + return queue; + } + + /** + * add this destination to the queue and attempt to optimize travel between destinations. + * @param to destination position + * @param feedrate velocity (mm/s) + * @param acceleration (mm/s/s) + * @param cartesianDelta move (mm) + */ + private void bufferSegment(final MarlinCoordinate to, final double feedrate, final double acceleration,final MarlinCoordinate cartesianDelta) { + MarlinSimulationBlock block = new MarlinSimulationBlock(to,cartesianDelta); + block.feedrate = feedrate; + + // zero distance? do nothing. + if(block.distance<=6.0/80.0) return; + + double inverse_secs = feedrate / block.distance; + + // slow down if the buffer is nearly empty. + if( queue.size() >= 2 && queue.size() <= (settings.getInteger(MarlinSettings.BLOCK_BUFFER_SIZE)/2)-1 ) { + long segment_time_us = Math.round(1000000.0f / inverse_secs); + long timeDiff = settings.getInteger(MarlinSettings.MIN_SEG_TIME) - segment_time_us; + if( timeDiff>0 ) { + double nst = segment_time_us + Math.round(2.0 * timeDiff / queue.size()); + inverse_secs = 1000000.0 / nst; + } + } + + block.nominalSpeed = block.distance * inverse_secs; + + // find if speed exceeds any joint max speed. + MarlinCoordinate currentSpeed = new MarlinCoordinate(block.delta); + currentSpeed.scale(inverse_secs); + double speedFactor=1.0; + double cs; + for(double v : currentSpeed.p ) { + cs = Math.abs(v); + if( cs > feedrate ) { + speedFactor = Math.min(speedFactor, feedrate/cs); + } + } + + // apply speed limit + if(speedFactor<1.0) { + currentSpeed.scale(speedFactor); + block.nominalSpeed *= speedFactor; + } + + block.acceleration = acceleration; + + // limit jerk between moves + double vmax_junction = switch (jerkType) { + case CLASSIC_JERK -> classicJerk(block, currentSpeed, block.nominalSpeed); + case JUNCTION_DEVIATION -> junctionDeviationJerk(block, block.nominalSpeed); + case DOT_PRODUCT -> dotProductJerk(block); + default -> block.nominalSpeed; + }; + + block.allowableSpeed = maxSpeedAllowed(-block.acceleration,settings.getDouble(MarlinSettings.MINIMUM_PLANNER_SPEED),block.distance); + block.entrySpeedMax = vmax_junction; + block.entrySpeed = Math.min(vmax_junction, block.allowableSpeed); + block.nominalLength = ( block.allowableSpeed >= block.nominalSpeed ); + block.recalculate = true; + + previousNominalSpeed = block.nominalSpeed; + currentSpeed.set(previousSpeed); + + queue.add(block); + poseNow.set(to); + + recalculateAcceleration(); + } + + private double dotProductJerk(MarlinSimulationBlock next) { + double vmax_junction = next.nominalSpeed * next.normal.dot(previousNormal) * 1.1; + vmax_junction = Math.min(vmax_junction, next.nominalSpeed); + vmax_junction = Math.max(vmax_junction, settings.getDouble(MarlinSettings.MINIMUM_PLANNER_SPEED)); + previousNormal.set(next.normal); + + return vmax_junction; + } + + private double junctionDeviationJerk(MarlinSimulationBlock next,double nominalSpeed) { + double vmax_junction = nominalSpeed; + // Skip first block or when previousNominalSpeed is used as a flag for homing and offset cycles. + if (!queue.isEmpty() && previousNominalSpeed > 1e-6) { + // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) + // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. + double junction_cos_theta = -previousNormal.dot(next.normal); + + // NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta). + if (junction_cos_theta > 0.999999f) { + // For a 0 degree acute junction, just set minimum junction speed. + vmax_junction = settings.getDouble(MarlinSettings.MINIMUM_PLANNER_SPEED); + } else { + // Check for numerical round-off to avoid divide by zero. + junction_cos_theta = Math.max(junction_cos_theta, -0.999999f); + + // Convert delta vector to unit vector + var junction_unit_vec = new MarlinCoordinate(); + junction_unit_vec.sub(next.normal, previousNormal); + junction_unit_vec.normalize(); + if (junction_unit_vec.length() > 0) { + final double junction_acceleration = limit_value_by_axis_maximum(next.acceleration,junction_unit_vec, settings.getDouble(MarlinSettings.MAX_ACCELERATION)); + // Trig half angle identity. Always positive. + final double sin_theta_d2 = Math.sqrt(0.5 * (1.0 - junction_cos_theta)); + + vmax_junction = junction_acceleration * junction_deviation * sin_theta_d2 / (1.0f - sin_theta_d2); + + if (settings.getBoolean(MarlinSettings.HANDLE_SMALL_SEGMENTS)) { + // For small moves with >135° junction (octagon) find speed for approximate arc + if (next.distance < 1 && junction_cos_theta < -0.7071067812f) { + double junction_theta = Math.acos(-junction_cos_theta); + // NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0. + double limit = (next.distance * junction_acceleration) / junction_theta; + vmax_junction = Math.min(vmax_junction, limit); + } + + } + } + } + + // Get the lowest speed + vmax_junction = Math.min(vmax_junction, next.nominalSpeed); + vmax_junction = Math.min(vmax_junction, previousNominalSpeed); + } else { + // Init entry speed to zero. Assume it starts from rest. Planner will correct + // this later. + vmax_junction = 0; + } + + previousNormal.set(next.normal); + + return vmax_junction; + } + + private double limit_value_by_axis_maximum(double max_value, MarlinCoordinate junction_unit_vec,double maxAcceleration) { + double limit_value = max_value; + + for(int i=0;i maxAcceleration) { + limit_value = Math.abs( maxAcceleration / junction_unit_vec.p[i] ); + } + } + } + + return limit_value; + } + + private double classicJerk(MarlinSimulationBlock next,MarlinCoordinate currentSpeed,double safeSpeed) { + boolean limited=false; + + for(int i=0;i maxj ) { + if(limited) { + double mjerk = maxj * next.nominalSpeed; + if( jerk * safeSpeed > mjerk ) safeSpeed = mjerk/jerk; + } else { + safeSpeed *= maxj / jerk; + limited=true; + } + } + } + + double vmax_junction; + + if(!queue.isEmpty()) { + // look at difference between this move and previous move + MarlinSimulationBlock prev = queue.getLast(); + if(prev.nominalSpeed > 1e-6) { + vmax_junction = Math.min(next.nominalSpeed,prev.nominalSpeed); + limited=false; + + double vFactor=0; + double smallerSpeedFactor = vmax_junction / prev.nominalSpeed; + + for(int i=0;i vEntry) ? ((vEntry>0 || vExit<0) ? (vExit-vEntry) : Math.max(vExit, -vEntry)) + : ((vEntry<0 || vExit>0) ? (vEntry-vExit) : Math.max(-vExit, vEntry)); + if( jerk > maxJerk.p[i] ) { + vFactor = maxJerk.p[i] / jerk; + limited = true; + } + } + if(limited) vmax_junction *= vFactor; + + double vmax_junction_threshold = vmax_junction * 0.99; + if( previousSafeSpeed > vmax_junction_threshold && safeSpeed > vmax_junction_threshold ) { + vmax_junction = safeSpeed; + } + } else { + vmax_junction = safeSpeed; + } + } else { + vmax_junction = safeSpeed; + } + + previousSafeSpeed = safeSpeed; + + return vmax_junction; + } + + private void recalculateAcceleration() { + recalculateBackwards(); + recalculateForwards(); + recalculateTrapezoids(); + } + + private void recalculateBackwards() { + MarlinSimulationBlock current; + MarlinSimulationBlock next = null; + Iterator ri = queue.descendingIterator(); + while(ri.hasNext()) { + current = ri.next(); + recalculateBackwardsBetween(current,next); + next = current; + } + } + + private void recalculateBackwardsBetween(MarlinSimulationBlock current,MarlinSimulationBlock next) { + double top = current.entrySpeedMax; + if(current.entrySpeed != top || (next!=null && next.recalculate)) { + current.entrySpeed = current.nominalLength + ? top + : Math.min( top, maxSpeedAllowed( -current.acceleration, (next!=null? next.entrySpeed : settings.getDouble(MarlinSettings.MINIMUM_PLANNER_SPEED)), current.distance)); + current.recalculate = true; + } + } + + private void recalculateForwards() { + MarlinSimulationBlock current; + MarlinSimulationBlock prev = null; + for (MarlinSimulationBlock marlinSimulationBlock : queue) { + current = marlinSimulationBlock; + recalculateForwardsBetween(prev, current); + prev = current; + } + } + + private void recalculateForwardsBetween(MarlinSimulationBlock prev,MarlinSimulationBlock current) { + if(prev==null) return; + if(!prev.nominalLength && prev.entrySpeed < current.entrySpeed) { + double newEntrySpeed = maxSpeedAllowed(-prev.acceleration, prev.entrySpeed, prev.distance); + if(newEntrySpeed < current.entrySpeed) { + current.recalculate=true; + current.entrySpeed = newEntrySpeed; + } + } + } + + private void recalculateTrapezoids() { + MarlinSimulationBlock current=null; + + double currentEntrySpeed=0, nextEntrySpeed=0; + for( MarlinSimulationBlock next : queue ) { + nextEntrySpeed = next.entrySpeed; + if(current!=null) { + if(current.recalculate || next.recalculate) { + current.recalculate = true; + if( !current.busy ) { + recalculateTrapezoidForBlock(current, currentEntrySpeed, nextEntrySpeed); + } + current.recalculate = false; + } + } + current = next; + currentEntrySpeed = nextEntrySpeed; + } + + if(current!=null) { + current.recalculate = true; + if( !current.busy ) { + recalculateTrapezoidForBlock(current, currentEntrySpeed, settings.getDouble(MarlinSettings.MINIMUM_PLANNER_SPEED)); + } + current.recalculate = false; + } + } + + private void recalculateTrapezoidForBlock(MarlinSimulationBlock block, double entrySpeed, double exitSpeed) { + if( entrySpeed < settings.getDouble(MarlinSettings.MINIMUM_PLANNER_SPEED) ) entrySpeed = settings.getDouble(MarlinSettings.MINIMUM_PLANNER_SPEED); + if( exitSpeed < settings.getDouble(MarlinSettings.MINIMUM_PLANNER_SPEED) ) exitSpeed = settings.getDouble(MarlinSettings.MINIMUM_PLANNER_SPEED); + + double accel = block.acceleration; + double accelerateD = estimateAccelerationDistance(entrySpeed, block.nominalSpeed, accel); + double decelerateD = estimateAccelerationDistance(block.nominalSpeed, exitSpeed, -accel); + double cruiseRate; + double plateauD = block.distance - accelerateD - decelerateD; + if( plateauD < 0 ) { + // never reaches nominal v + double d = Math.ceil(intersectionDistance(entrySpeed, exitSpeed, accel, block.distance)); + accelerateD = Math.min(Math.max(d, 0), block.distance); + decelerateD = 0; + plateauD = 0; + cruiseRate = finalRate(accel,entrySpeed,accelerateD); + } else { + cruiseRate = block.nominalSpeed; + } + block.accelerateUntilD = accelerateD; + block.decelerateAfterD = accelerateD + plateauD; + block.entrySpeed = entrySpeed; + block.exitSpeed = exitSpeed; + block.plateauD = plateauD; + + double accelerateT = (cruiseRate - entrySpeed) / accel; + double decelerateT = (cruiseRate - exitSpeed) / accel; + double nominalT = plateauD/block.nominalSpeed; + + block.accelerateUntilT = accelerateT; + block.decelerateAfterT = accelerateT + nominalT; + block.end_s = accelerateT + nominalT + decelerateT; + + if(Double.isNaN(block.end_s)) { + logger.debug("recalculateTrapezoidSegment() Uh oh"); + } + } + + private double finalRate(double acceleration, double startV, double distance) { + return Math.sqrt( (startV*startV) + 2.0 * acceleration*distance); + } + + /** + * Calculate the maximum allowable speed at this point, in order to reach 'targetVelocity' using + * 'acceleration' within a given 'distance'. + * @param acceleration (cm/s/s) + * @param targetVelocity (cm/s) + * @param distance (cm) + */ + private double maxSpeedAllowed( double acceleration, double targetVelocity, double distance ) { + return Math.sqrt( (targetVelocity*targetVelocity) - 2 * acceleration * distance ); + } + + // (endV^2 - startV^2) / 2a + private double estimateAccelerationDistance(final double initialRate, final double targetRate, final double accel) { + if(accel == 0) return 0; + return ( (targetRate*targetRate) - (initialRate*initialRate) ) / (accel * 2.0); + } + + private double intersectionDistance(final double startRate, final double endRate, final double accel, final double distance) { + if(accel == 0) return 0; + return ( 2.0 * accel * distance - (startRate*startRate) + (endRate*endRate) ) / (4.0 * accel); + } +} diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/marlinsimulation/MarlinSimulationBlock.java b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinsimulation/MarlinSimulationBlock.java new file mode 100644 index 000000000..30fa8b08a --- /dev/null +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinsimulation/MarlinSimulationBlock.java @@ -0,0 +1,93 @@ +package com.marginallyclever.ro3.node.nodes.marlinsimulation; + +import com.marginallyclever.convenience.helpers.StringHelper; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import javax.vecmath.Vector3d; + +/** + * {@link MarlinSimulationBlock} is one block in the queue of blocks inside a {@link MarlinSimulation}. + * For more details, please see Marlin documentation. + * @author Dan Royer + * @since 7.24.0 + */ +public class MarlinSimulationBlock { + private static final Logger logger = LoggerFactory.getLogger(MarlinSimulationBlock.class); + + public static int counter=0; + public int id; + + public MarlinCoordinate start = new MarlinCoordinate(); + public MarlinCoordinate end = new MarlinCoordinate(); + public MarlinCoordinate delta = new MarlinCoordinate(); + public MarlinCoordinate normal = new MarlinCoordinate(); + + //public double start_s; + public double end_s; + public double now_s; + + public double feedrate; + + public double distance; + public double nominalSpeed; // top speed in this segment + public double entrySpeed; // per second + public double exitSpeed; // per second + public double acceleration; // per second per second + + public double entrySpeedMax; + public double accelerateUntilD; // distance + public double decelerateAfterD; // distance + public double plateauD; // distance + + public double accelerateUntilT; // seconds + public double decelerateAfterT; // seconds + + public double allowableSpeed; + + // when optimizing, should we recheck the entry + exit v of this segment? + public boolean recalculate; + // is this segment 100% full speed, end to end? + public boolean nominalLength; + // is the robot moving through this segment right now? + public boolean busy; + + + // delta is calculated here in the constructor. + public MarlinSimulationBlock(final MarlinCoordinate endPose,final MarlinCoordinate deltaPose) { + end.set(endPose); + delta.set(deltaPose); + normal.set(deltaPose); + normal.normalize(); + start.sub(end,delta); + + id=counter++; + distance = delta.length(); + busy=false; + recalculate=true; + } + + public void report() { + String res = "S" + "\t" + id + + "\t" + start + + "\t" + end + + "\t" + delta + + "\t" + normal + + "\t" + StringHelper.formatDouble(end_s) + + "\t" + StringHelper.formatDouble(feedrate) + + "\t" + StringHelper.formatDouble(distance) + + "\t" + StringHelper.formatDouble(nominalSpeed) + + "\t" + StringHelper.formatDouble(entrySpeed) + + "\t" + StringHelper.formatDouble(exitSpeed) + + "\t" + StringHelper.formatDouble(entrySpeedMax) + + "\t" + StringHelper.formatDouble(allowableSpeed) + + "\t" + StringHelper.formatDouble(acceleration) + + "\t" + StringHelper.formatDouble(accelerateUntilD) + + "\t" + StringHelper.formatDouble(plateauD) + + "\t" + StringHelper.formatDouble(decelerateAfterD) + + "\t" + StringHelper.formatDouble(accelerateUntilT) + + "\t" + StringHelper.formatDouble(decelerateAfterT) + + "\t" + (nominalLength ? 1 : 0); + logger.debug(res); + } +} \ No newline at end of file