From 697d31b4a79f885a75453aebb90bb4e30ca1edfc Mon Sep 17 00:00:00 2001 From: Flemming Frandsen Date: Tue, 28 Jun 2016 20:31:57 +0200 Subject: [PATCH] Fixed cornering --- host/shiver/config.yaml | 6 +- .../dk/osaa/psaw/core/BrokenCornering.java | 140 ++++++++++++++++ .../java/dk/osaa/psaw/core/Cornering.java | 150 ++++++++++-------- .../src/main/java/dk/osaa/psaw/core/Line.java | 23 ++- .../main/java/dk/osaa/psaw/core/Planner.java | 2 +- .../dk/osaa/psaw/job/JobRenderTarget.java | 6 +- 6 files changed, 238 insertions(+), 89 deletions(-) create mode 100644 host/shiver/src/main/java/dk/osaa/psaw/core/BrokenCornering.java diff --git a/host/shiver/config.yaml b/host/shiver/config.yaml index 191fff0..e3a9c13 100644 --- a/host/shiver/config.yaml +++ b/host/shiver/config.yaml @@ -36,7 +36,7 @@ machine: acceleration: 5000 maxSpeed: 1500 minSpeed: 20 - maxJerk: 2 + maxJerk: 10 mmPerStep: 0.0375 coilCurrent: 1800 microSteppingMode: 3 @@ -44,7 +44,7 @@ machine: acceleration: 500 maxSpeed: 1500 minSpeed: 20 - maxJerk: 2 + maxJerk: 10 mmPerStep: 0.0375 coilCurrent: 1800 microSteppingMode: 3 @@ -52,7 +52,7 @@ machine: acceleration: 0.1 maxSpeed: 10 minSpeed: 0.2 - maxJerk: 0.2 + maxJerk: 100 mmPerStep: 0.003125 coilCurrent: 1500 microSteppingMode: 3 diff --git a/host/shiver/src/main/java/dk/osaa/psaw/core/BrokenCornering.java b/host/shiver/src/main/java/dk/osaa/psaw/core/BrokenCornering.java new file mode 100644 index 0000000..ae228a6 --- /dev/null +++ b/host/shiver/src/main/java/dk/osaa/psaw/core/BrokenCornering.java @@ -0,0 +1,140 @@ +package dk.osaa.psaw.core; + +import dk.osaa.psaw.config.AxisConstraints; +import dk.osaa.psaw.config.PhotonSawMachineConfig; +import dk.osaa.psaw.machine.Move; +import dk.osaa.psaw.machine.MoveVector; +import lombok.Getter; +import lombok.extern.java.Log; + +import java.util.ArrayList; +import java.util.List; + +/** + * This class wraps up the algorithm needed to change direction without stopping. + * + * @author Flemming Frandsen + */ +@Log +public class BrokenCornering { + + private final List maxJerks; + /** + * The speed along the exit vector after the corner. + */ + @Getter + double exitSpeed; + + @Getter + MoveVector jerks; + + @Getter + MoveVector exitSpeeds; + + static int id = 0; + + /** + * Calculates the cornering parameters + * + * @param cfg The movement constraints that govern the mechanical system that this drives + * @param entryVector The direction of the system before the corner + * @param entrySpeed The speed of the system before the corner + * @param exitVector The direction of the system after the corner + * @param maxExitSpeed The absolute maximum exit speed to target + */ + public BrokenCornering corner(PhotonSawMachineConfig cfg, MoveVector entryVector, double entrySpeed, MoveVector exitVector, double maxExitSpeed) { + List maxJerks = new ArrayList<>(); + for (AxisConstraints ac : cfg.getAxes().getArray()) { + maxJerks.add(ac.getMaxJerk()); + } + + return new BrokenCornering(maxJerks, entryVector, entrySpeed, exitVector, maxExitSpeed); + } + + /** + * Calculates the cornering parameters + * + * @param maxJerks The max jerk limits for all the axes that govern the mechanical system that this drives + * @param entryVector The direction of the system before the corner + * @param entrySpeed The speed of the system before the corner + * @param exitVector The direction of the system after the corner + * @param maxExitSpeed The absolute maximum exit speed to target + */ + public BrokenCornering(List maxJerks, MoveVector entryVector, double entrySpeed, MoveVector exitVector, double maxExitSpeed) { + this.maxJerks = maxJerks; + + id++; + + boolean done = false; + while (!done) { + MoveVector entrySpeeds = entryVector.mul(entrySpeed); + double overSpeed = -1; + exitSpeed = maxExitSpeed; + MoveVector maxExitSpeeds = exitVector.mul(maxExitSpeed); + + /* + * Find the speed limit for each axis, + * then compare that to the entry speed + * and pick the axis that is going to constrain the corner + */ + for (int ax=0;ax 1 && os > overSpeed) { + overSpeed = os; + } + } + + if (overSpeed > 1) { + exitSpeed = exitSpeed/overSpeed; + } + + exitSpeeds = exitVector.mul(exitSpeed); + jerks = new MoveVector(); + + done = true; + for (int ax=0;ax maxJerks.get(ax)*1.01) { + done = false; + + if (Math.signum(exitVector.getAxis(ax)) == Math.signum(entryVector.getAxis(ax))) { + if (Math.abs(entrySpeeds.getAxis(ax)) > Math.abs(exitSpeeds.getAxis(ax))+maxJerks.get(ax)) { + entrySpeed /= Math.abs(entrySpeeds.getAxis(ax)) / (Math.abs(exitSpeeds.getAxis(ax))+maxJerks.get(ax)); + } else { + log.info("Fail!"+id); + } + } else { + if (Math.abs(entrySpeeds.getAxis(ax)) > maxJerks.get(ax)/2) { + entrySpeed /= Math.abs(entrySpeeds.getAxis(ax))/(maxJerks.get(ax)/2); + } else { + log.info("Fail!"+id); + } + } + } + } + } + + } + + public BrokenCornering checkJerks() { + for (int ax=0;ax maxJerks.get(ax)*1.25) { + throw new RuntimeException("Jerk was too large in axis: "+ax+" jerk was "+jerk+" max:"+maxJerks.get(ax)+" for corner id:"+id); + } + } + return this; + } +} diff --git a/host/shiver/src/main/java/dk/osaa/psaw/core/Cornering.java b/host/shiver/src/main/java/dk/osaa/psaw/core/Cornering.java index 32ecd29..03a0437 100644 --- a/host/shiver/src/main/java/dk/osaa/psaw/core/Cornering.java +++ b/host/shiver/src/main/java/dk/osaa/psaw/core/Cornering.java @@ -1,11 +1,15 @@ package dk.osaa.psaw.core; +import dk.osaa.psaw.config.AxisConstraints; import lombok.Getter; import lombok.extern.java.Log; import dk.osaa.psaw.config.PhotonSawMachineConfig; import dk.osaa.psaw.machine.Move; import dk.osaa.psaw.machine.MoveVector; +import java.util.ArrayList; +import java.util.List; + /** * This class wraps up the algorithm needed to change direction without stopping. * @@ -14,6 +18,10 @@ @Log public class Cornering { + private final List maxJerks; + private long lineNumber; + @Getter + private final double limitedEntrySpeed; /** * The speed along the exit vector after the corner. */ @@ -25,97 +33,103 @@ public class Cornering { @Getter MoveVector exitSpeeds; - + + @Getter + double maxEntrySpeed; + static int id = 0; - - private final PhotonSawMachineConfig cfg; - - + /** * Calculates the cornering parameters - * - * @param cfg The movement constraints that govern the mechanical system that this drives + * @param cfg The movement constraints that govern the mechanical system that this drives * @param entryVector The direction of the system before the corner * @param entrySpeed The speed of the system before the corner * @param exitVector The direction of the system after the corner * @param maxExitSpeed The absolute maximum exit speed to target + * @param lineNumber */ - public Cornering(PhotonSawMachineConfig cfg, MoveVector entryVector, double entrySpeed, MoveVector exitVector, double maxExitSpeed) { - this.cfg = cfg; - + public static Cornering corner(PhotonSawMachineConfig cfg, MoveVector entryVector, double entrySpeed, MoveVector exitVector, double maxExitSpeed, long lineNumber) { + List maxJerks = new ArrayList<>(); + for (AxisConstraints ac : cfg.getAxes().getArray()) { + maxJerks.add(ac.getMaxJerk()); + } + + return new Cornering(maxJerks, entryVector, entrySpeed, exitVector, maxExitSpeed, lineNumber); + } + + /** + * Calculates the cornering parameters + * @param maxJerks The max jerk limits for all the axes that govern the mechanical system that this drives + * @param entryVector The direction of the system before the corner + * @param entrySpeed The speed of the system before the corner + * @param exitVector The direction of the system after the corner + * @param maxExitSpeed The absolute maximum exit speed to target + * @param lineNumber + */ + public Cornering(List maxJerks, MoveVector entryVector, double entrySpeed, MoveVector exitVector, double maxExitSpeed, long lineNumber) { + this.maxJerks = maxJerks; + this.lineNumber = lineNumber; + id++; - if (id == 43) { - log.info("Hit"); + + //if (lineNumber == 286) { + // log.info("Hit "+id); + //} + + // Find the axis that's moving the fastest in the same direction before and after the corner + Integer fixedAxis = null; + double fixedPreservation = 0; + MoveVector preservation = entryVector.mul(exitVector); + for (int ax=0;ax fixedPreservation) { + fixedPreservation = preserved; + fixedAxis = ax; + } } - - boolean done = false; - while (!done) { - MoveVector entrySpeeds = entryVector.mul(entrySpeed); - double overSpeed = -1; - exitSpeed = maxExitSpeed; - MoveVector maxExitSpeeds = exitVector.mul(maxExitSpeed); - - /* - * Find the speed limit for each axis, - * then compare that to the entry speed - * and pick the axis that is going to constrain the corner - */ - for (int ax=0;ax 1 && os > overSpeed) { - overSpeed = os; - } + + // Limit the entry speed until all the jerks are under the max jerk limits + while (true) { + exitSpeed = 0; + + // An axis was found that moves in the same direction as before, so we'll start out trying to keep that axis moving at exactly the same speed + if (fixedAxis != null) { + exitSpeed = entrySpeed*preservation.getAxis(fixedAxis); } - - if (overSpeed > 1) { - exitSpeed = exitSpeed/overSpeed; + + // Limit the exit speed according to the absolute limit passed: + if (exitSpeed > maxExitSpeed) { + exitSpeed = maxExitSpeed; } - - exitSpeeds = exitVector.mul(exitSpeed); + + exitSpeeds = exitVector.mul(exitSpeed); + MoveVector entrySpeeds = entryVector.mul(entrySpeed); jerks = new MoveVector(); - - done = true; - for (int ax=0;ax cfg.getAxes().getArray()[ax].getMaxJerk()*1.01) { - done = false; - - if (Math.signum(exitVector.getAxis(ax)) == Math.signum(entryVector.getAxis(ax))) { - if (Math.abs(entrySpeeds.getAxis(ax)) > Math.abs(exitSpeeds.getAxis(ax))+cfg.getAxes().getArray()[ax].getMaxJerk()) { - entrySpeed /= Math.abs(entrySpeeds.getAxis(ax)) / (Math.abs(exitSpeeds.getAxis(ax))+cfg.getAxes().getArray()[ax].getMaxJerk()); - } else { - log.info("Fail!"+id); - } - } else { - if (Math.abs(entrySpeeds.getAxis(ax)) > cfg.getAxes().getArray()[ax].getMaxJerk()/2) { - entrySpeed /= Math.abs(entrySpeeds.getAxis(ax))/(cfg.getAxes().getArray()[ax].getMaxJerk()/2); - } else { - log.info("Fail!"+id); - } - } + double overspeed = jerk/maxJerks.get(ax); + if (overspeed > biggestOverspeed) { + biggestOverspeed = overspeed; } } + + entrySpeed = entrySpeed/biggestOverspeed; + if (biggestOverspeed <= 1) { + break; + } } - + + limitedEntrySpeed = entrySpeed; } public Cornering checkJerks() { for (int ax=0;ax cfg.getAxes().getArray()[ax].getMaxJerk()*1.25) { - throw new RuntimeException("Jerk was too large in axis: "+ax+" jerk was "+jerk+" max:"+cfg.getAxes().getArray()[ax].getMaxJerk()+" for corner id:"+id); + if (Math.abs(jerk) > maxJerks.get(ax)*1.25) { + throw new RuntimeException("Jerk was too large in axis: "+ax+" jerk was "+jerk+" max:"+maxJerks.get(ax)+" for corner id:"+id); } } return this; diff --git a/host/shiver/src/main/java/dk/osaa/psaw/core/Line.java b/host/shiver/src/main/java/dk/osaa/psaw/core/Line.java index dee8f5c..8844baa 100644 --- a/host/shiver/src/main/java/dk/osaa/psaw/core/Line.java +++ b/host/shiver/src/main/java/dk/osaa/psaw/core/Line.java @@ -223,7 +223,7 @@ private void updateMaxEntrySpeed(Line next) { maxExitSpeed = 0; return; } - + /* * Max entry speed is the maximum speed at the beginning of this line in the direction of this line, * it is dictated the maxExitSpeed and the acceleration that can happen for this line, so to calculate it we first need to @@ -231,7 +231,7 @@ private void updateMaxEntrySpeed(Line next) { */ //Cornering fc = new Cornering(mc, unitVector, maxSpeed, next.unitVector, next.maxSpeed); - Cornering c = new Cornering(cfg, next.unitVector, next.maxEntrySpeed, unitVector, maxSpeed); + Cornering c = Cornering.corner(cfg, next.unitVector, next.maxEntrySpeed, unitVector, maxSpeed, lineNumber); maxExitSpeed = c.getExitSpeed(); } @@ -248,7 +248,7 @@ private void updateEntrySpeed(Line prev) { return; } - Cornering c = new Cornering(cfg, prev.unitVector, prev.exitSpeed, unitVector, maxSpeed).checkJerks(); + Cornering c = Cornering.corner(cfg, prev.unitVector, prev.exitSpeed, unitVector, maxSpeed, lineNumber).checkJerks(); entrySpeed = c.getExitSpeed(); } @@ -259,10 +259,10 @@ private void old_updateMaxEntrySpeed(Line next) { maxExitSpeed = 0; } else { MoveVector maxEndSpeeds = next.unitVector.mul(next.maxEntrySpeed); // The speed limits as imposed by the max entry speed of the next line - - if (lineNumber == 160) { - log.fine(""); - } + + //if (lineNumber == 160) { + // log.fine(""); + //} // Find the maximum speed along this normal vector which doesn't exceed the maxEndSpeed limit. maxExitSpeed = maxSpeed; @@ -418,7 +418,7 @@ public void forwardPass(Line prev) { double diffExit = mandatoryExitSpeed-exitSpeed; if (Math.abs(diffExit) > 0.1) { - //log.info("The exit speed of this line was off by "+diffExit+" mm/s, it was planned to be "+exitSpeed+" but should have been "+mandatoryExitSpeed); +// log.info("The exit speed of this line was off by "+diffExit+" mm/s, it was planned to be "+exitSpeed+" but should have been "+mandatoryExitSpeed); } } @@ -447,10 +447,9 @@ public void forwardPass(Line prev) { // Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the // given acceleration: static public double estimateAccelerationDistance(double initialrate, double targetrate, double acceleration) { - return (Math.pow(targetrate, 2) - Math.pow(initialrate, 2))/(2*acceleration); - - //double time = (targetrate-initialrate)/acceleration; - //return initialrate*time + acceleration*Math.pow(time,2)/2; + // return (Math.pow(targetrate, 2) - Math.pow(initialrate, 2))/(2*acceleration); + double time = (targetrate-initialrate)/acceleration; + return initialrate*time + acceleration*Math.pow(time, 2)/2; } // This function gives you the point at which you must start braking (at the rate of -acceleration) if diff --git a/host/shiver/src/main/java/dk/osaa/psaw/core/Planner.java b/host/shiver/src/main/java/dk/osaa/psaw/core/Planner.java index bb15ebd..3e6097d 100644 --- a/host/shiver/src/main/java/dk/osaa/psaw/core/Planner.java +++ b/host/shiver/src/main/java/dk/osaa/psaw/core/Planner.java @@ -252,7 +252,7 @@ void encodeLine() { try { ready.toMoves(photonSaw); // This will block if the move buffer is full. } catch (InterruptedException e) { - log.log(Level.SEVERE, "Ignoring exeception from buffering moves", e); + log.log(Level.SEVERE, "Ignoring exception from buffering moves", e); } if (ready.isEndPosDirty()) { diff --git a/host/shiver/src/main/java/dk/osaa/psaw/job/JobRenderTarget.java b/host/shiver/src/main/java/dk/osaa/psaw/job/JobRenderTarget.java index 252d467..643b475 100644 --- a/host/shiver/src/main/java/dk/osaa/psaw/job/JobRenderTarget.java +++ b/host/shiver/src/main/java/dk/osaa/psaw/job/JobRenderTarget.java @@ -35,19 +35,15 @@ public interface JobRenderTarget { /** * Turn on the laser and move to this point at the desired speed * @param p the point to move to - * @param intensity The intensity (0..1) of the LASER during the move - * @param maxSpeed The desired speed */ void cutTo(Point p, LaserNodeSettings settings); /** * Turn on the laser and move to this point at the desired speed while engraving a scanline of pixels. * @param p the point to move to - * @param intensity The intensity (0..1) of the LASER during the move - * @param maxSpeed The desired speed * @param pixels The pixels to engrave over this line. */ - void engraveTo(Point p, LaserNodeSettings settings, boolean[] pixels);//TODO: Take LaserNodeSettings in stead + void engraveTo(Point p, LaserNodeSettings settings, boolean[] pixels); /** * Calculates the distance in mm needed to accelerate the X axis from 0 to the desired speed.