Skip to content

Commit

Permalink
Merge pull request openpnp#1073 from markmaker/feature/advanced-motio…
Browse files Browse the repository at this point in the history 
…n-control--5

Feature / Advanced Motion Control - Update 5
  • Loading branch information
@markmaker
markmaker committed Nov 1, 2020
2 parents dfba626 + 9f7cd65 commit 56eae4d
Show file tree
Hide file tree
Showing 13 changed files with 937 additions and 356 deletions.
95 changes: 78 additions & 17 deletions src/main/java/org/openpnp/gui/components/SimpleGraphView.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,8 @@ public class SimpleGraphView extends JComponent implements MouseMotionListener,
private Integer xMouse;
private Integer yMouse;
private Double selectedX;
private int displayCycle = -1;//all displayed
private int displayCycleMask;

public SimpleGraphView() {
addMouseMotionListener(this);
Expand All @@ -67,6 +69,17 @@ public SimpleGraph getGraph() {
}
public void setGraph(SimpleGraph graph) {
this.graph = graph;
displayCycleMask = 0;
if (graph != null) {
for (DataScale dataScale : graph.getDataScales()) {
for (DataRow dataRow : dataScale.getDataRows()) {
if (dataRow.size() >= 2) {
displayCycleMask |= dataRow.getDisplayCycleMask();
}
}
}
}
displayCycle = displayCycleMask;
repaint();
}

Expand Down Expand Up @@ -101,6 +114,7 @@ protected String formatNumber(double y, double unit) {
public void paintComponent(Graphics g) {
super.paintComponent(g);
Graphics2D g2d = (Graphics2D) g;
g2d.setRenderingHint(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_QUALITY);
g2d.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);

Font font = getFont();
Expand All @@ -115,6 +129,12 @@ public void paintComponent(Graphics g) {
for (DataScale dataScale : graph.getDataScales()) {
Point2D.Double min = graph.getMinimum(dataScale);
Point2D.Double max = graph.getMaximum(dataScale);
if (dataScale.isSymmetricIfSigned()) {
if (min != null && max != null && min.y < 0.0 && max.y > 0) {
max.y = Math.max(max.y, -min.y);
min.y = Math.min(-max.y, min.y);
}
}
if (min != null && max != null && (max.y-min.y) > 0.0 && (max.x-min.x) > 0.0) {
double xOrigin = (w-1)*graph.getRelativePaddingLeft();
double xScale = (w-1)*(1.0-graph.getRelativePaddingLeft()-graph.getRelativePaddingRight())/(max.x-min.x);
Expand Down Expand Up @@ -219,25 +239,59 @@ else if (xUnitFont < xUnit/2) {
}
}
}
// Draw the actual curves.
for (DataRow dataRow : dataScale.getDataRows()) {
Set<Double> xAxis = dataRow.getXAxis();
if (xAxis != null) {
double y0 = Double.NaN;
double x0 = Double.NaN;
// Draw the actual curves.
g2d.setColor(dataRow.getColor());
for (double x : xAxis) {
double y = dataRow.getDataPoint(x);
if (!Double.isNaN(x0)) {
g2d.drawLine((int)(xOrigin+(x0-min.x)*xScale), (int)(yOrigin-(y0-min.y)*yScale),
(int)(xOrigin+(x-min.x)*xScale), (int)(yOrigin-(y-min.y)*yScale));
if ((dataRow.getDisplayCycleMask() & displayCycle) != 0) {
Set<Double> xAxis = dataRow.getXAxis();
if (xAxis != null) {
// Convert to pixel coordinates
int size = dataRow.size();
if (size >= 2) {
double [] xfPlot = new double [size];
double [] yfPlot = new double [size];
int i = 0;
for (double x : xAxis) {
double y = dataRow.getDataPoint(x);
xfPlot[i] = xOrigin+(x-min.x)*xScale;
yfPlot[i] = yOrigin-(y-min.y)*yScale;
i++;
}
// Analyze the curve and only plot relevant curve points.
int [] xPlot = new int [size];
int [] yPlot = new int [size];
int s = 0;
// Always add first point.
xPlot[s] = (int) xfPlot[0];
yPlot[s] = (int) yfPlot[0];
s++;
for (i = 1; i < size-1; i++) {
double dx0 = xfPlot[i]-xfPlot[i-1];
double dy0 = yfPlot[i]-yfPlot[i-1];
double dx1 = xfPlot[i+1]-xfPlot[i];
double dy1 = yfPlot[i+1]-yfPlot[i];
double n0 = Math.sqrt(dx0*dx0+dy0*dy0);
double n1 = Math.sqrt(dx1*dx1+dy1*dy1);
double cosine = ((dx0*dx1) + (dy0*dy1))/n0/n1;
if (cosine < 0.99
|| Math.abs(xfPlot[i]-xPlot[s-1]) > 12 || Math.abs(yfPlot[i]-yPlot[s-1]) > 1.5) {
// Corner point or relevant change.
xPlot[s] = (int) xfPlot[i];
yPlot[s] = (int) yfPlot[i];
s++;
}
}
// Always add last point.
xPlot[s] = (int) xfPlot[size-1];
yPlot[s] = (int) yfPlot[size-1];
s++;
// Draw as polyline.
g2d.setColor(dataRow.getColor());
g2d.drawPolyline(xPlot, yPlot, s);
if (selectedX != null) {
drawYIndicator(g2d, dfm, fontAscent, w, min, max, yOrigin, yScale, yUnit,
dataRow.getInterpolated(selectedX), dataRow.getColor());
}
}
x0 = x;
y0 = y;
}
if (selectedX != null) {
drawYIndicator(g2d, dfm, fontAscent, w, min, max, yOrigin, yScale, yUnit,
dataRow.getInterpolated(selectedX), dataRow.getColor());
}
}
}
Expand Down Expand Up @@ -289,6 +343,13 @@ public void mouseMoved(MouseEvent e) {
}
@Override
public void mouseClicked(MouseEvent e) {
if (displayCycleMask != 0) {
displayCycle--;
while ((displayCycle & displayCycleMask) == 0) {
displayCycle--;
}
}
repaint();
}
@Override
public void mousePressed(MouseEvent e) {
Expand Down
30 changes: 23 additions & 7 deletions src/main/java/org/openpnp/machine/reference/driver/AbstractMotionPlanner.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -338,6 +338,7 @@ protected synchronized void executeMotionPlan(CompletionType completionType) thr
}
ReferenceMachine machine = (ReferenceMachine) Configuration.get().getMachine();
List<Head> movedHeads = new ArrayList<>();
boolean first = true;
for (Motion plannedMotion : executionPlan) {
if (!plannedMotion.hasOption(MotionOption.Stillstand)) {
// Put into timed plan.
Expand All @@ -351,7 +352,9 @@ protected synchronized void executeMotionPlan(CompletionType completionType) thr
ReferenceHeadMountable hm = (ReferenceHeadMountable) plannedMotion.getHeadMountable();
if (hm != null) {
movedHeads.add(hm.getHead());
executeMoveTo(machine, hm, plannedMotion);
if (executeMoveTo(machine, hm, plannedMotion, first)) {
first = false;
}
}
}
}
Expand Down Expand Up @@ -379,28 +382,41 @@ protected synchronized void executeMotionPlan(CompletionType completionType) thr
* @param machine
* @param hm
* @param plannedMotion
* @param firstAfterCoordination
* @return true if a driver move was executed.
* @throws Exception
*/
protected void executeMoveTo(ReferenceMachine machine, ReferenceHeadMountable hm,
Motion plannedMotion) throws Exception {
protected boolean executeMoveTo(ReferenceMachine machine, ReferenceHeadMountable hm,
Motion plannedMotion, boolean firstAfterCoordination) throws Exception {
AxesLocation motionSegment = plannedMotion.getLocation0().motionSegmentTo(plannedMotion.getLocation1());
// Note, this loop will be empty if the motion is empty, i.e. if it only contains VirtualAxis movement.
boolean firstDriver = true;
for (Driver driver : motionSegment.getAxesDrivers(machine)) {
for (Motion.MoveToCommand moveToCommand : plannedMotion.interpolatedMoveToCommands(driver, isInterpolationRetiming())) {
for (Motion.MoveToCommand moveToCommand : plannedMotion
.interpolatedMoveToCommands(driver, isInterpolationRetiming())) {
driver.moveTo(hm, moveToCommand);
recordDiagnostics(plannedMotion, moveToCommand, driver);
try {
recordDiagnostics(plannedMotion, moveToCommand, driver, firstAfterCoordination, firstDriver);
}
catch (Exception e) {
Logger.error(driver.getName()+" diagnostics failed: {}", e);
}
}
firstDriver = false;
}
return !firstDriver;
}

/**
* Sub.classes with diagnostics can override this method to record (interpolated) motion.
*
* @param plannedMotion
* @param moveToCommand
* @param driver TODO
* @param driver
* @param firstAfterCoordination
* @param firstDriver
*/
protected void recordDiagnostics(Motion plannedMotion, MoveToCommand moveToCommand, Driver driver) {
protected void recordDiagnostics(Motion plannedMotion, MoveToCommand moveToCommand, Driver driver, boolean firstAfterCoordination, boolean firstDriver) {
}

protected void publishDiagnostics() {
Expand Down
78 changes: 61 additions & 17 deletions src/main/java/org/openpnp/machine/reference/driver/GcodeAsyncDriver.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@
* While the GcodeDriver (through its on-by-one hand-shaking) knows when each and every command is
* acknowledged by the controller, the GcodeAsyncDriver does not. The responses (mostly a stream of
* "ok"s) are too generic to reliably detect how many and which commands have been acknowledged. Sometimes
* controllers will also output additional unsolicitated messages. GcodeAsyncDriver must therefore find a new
* controllers will also output additional unsolicited messages. GcodeAsyncDriver must therefore find a new
* way to implement hand-shaking when (and only when) it is really needed. Most importantly this is the case
* when OpenPnP wants to wait for the machine to physically have completed a motion sequence. GcodeAsyncDriver
* will therefore issue specific reporting commands where needed, making the responses uniquely recognizable,
Expand Down Expand Up @@ -109,17 +109,20 @@ public class GcodeAsyncDriver extends GcodeDriver {
@Attribute(required=false)
private boolean confirmationFlowControl = true;

@Attribute(required=false)
private boolean reportedLocationConfirmation = true;

@Attribute(required = false)
private int interpolationMaxSteps = 32;

@Attribute(required = false)
private int interpolationJerkSteps = 4; // relative to max acceleration

@Attribute(required = false)
private double interpolationTimeStep = 0.01;
private double interpolationTimeStep = 0.001;

@Attribute(required = false)
private int interpolationMinStep = 8;
private int interpolationMinStep = 16;

@Element(required = false)
private Length junctionDeviation = new Length(0.02, LengthUnit.Millimeters);
Expand All @@ -146,13 +149,26 @@ public long getTimeout() {
protected LinkedBlockingQueue<CommandLine> commandQueue;

private boolean waitedForCommands;
private boolean confirmationComplete;

public boolean isConfirmationFlowControl() {
return confirmationFlowControl;
}

public void setConfirmationFlowControl(boolean confirmationFlowControl) {
Object oldValue = confirmationFlowControl;
this.confirmationFlowControl = confirmationFlowControl;
firePropertyChange("confirmationFlowControl", oldValue, confirmationFlowControl);
}

public boolean isReportedLocationConfirmation() {
return reportedLocationConfirmation;
}

public void setReportedLocationConfirmation(boolean reportedLocationConfirmation) {
Object oldValue = reportedLocationConfirmation;
this.reportedLocationConfirmation = reportedLocationConfirmation;
firePropertyChange("reportedLocationConfirmation", oldValue, reportedLocationConfirmation);
}

@Override
Expand Down Expand Up @@ -228,11 +244,10 @@ protected class WriterThread extends Thread {

@Override
public void run() {
CommandLine lastCommand = null;
// Get the copy that is valid for this thread.
LinkedBlockingQueue<CommandLine> commandQueue = GcodeAsyncDriver.this.commandQueue;
CommandLine lastCommand = null;

long wantedConfirmations = 0;
while (!disconnectRequested) {
CommandLine command;
try {
Expand All @@ -249,18 +264,27 @@ public void run() {
if (confirmationFlowControl && lastCommand != null) {
try {
// Before we can send the new command, make sure the wanted confirmation count of the last command was received.
waitForConfirmation(lastCommand.toString(), lastCommand.getTimeout(), wantedConfirmations);
waitForConfirmation(lastCommand.toString(), lastCommand.getTimeout());
}
finally {
// Whatever happens, never wait for this one again.
lastCommand = null;
}
}
// Set up the wanted confirmations for next time.
wantedConfirmations = receivedConfirmations.get() + 1;
lastCommand = command;
getCommunications().writeLine(command.line);
Logger.trace("[{}] >> {}", getCommunications().getConnectionName(), command);
if (command.line != null) {
// Set up the wanted confirmations for next time.
lastCommand = command;
receivedConfirmationsQueue.clear();
getCommunications().writeLine(command.line);
Logger.trace("[{}] >> {}", getCommunications().getConnectionName(), command);
}
else {
confirmationComplete = true;
synchronized(GcodeAsyncDriver.this) {
GcodeAsyncDriver.this.notify();
}
//Logger.trace("[{}] confirmation released.", getCommunications().getConnectionName());
}
}
catch (IOException e) {
Logger.error("Write error on {}: {}", getCommunications().getConnectionName(), e);
Expand Down Expand Up @@ -317,13 +341,33 @@ public void waitForCompletion(ReferenceHeadMountable hm,
}
// Issue the M400 in the super class.
super.waitForCompletion(hm, completionType);
// Then make sure we get a uniquely recognizable confirmation.
if (completionType.isWaitingForDrivers()) {
// Explicitly wait for the controller's acknowledgment here.
// This is signaled with a position report.
getReportedLocation(
completionType == CompletionType.WaitForStillstandIndefinitely ?
-1 : getTimeoutAtMachineSpeed());
// Explicitly wait for the controller's acknowledgment here.
long timeout = (completionType == CompletionType.WaitForStillstandIndefinitely ?
infinityTimeoutMilliseconds : getTimeoutAtMachineSpeed());
if (reportedLocationConfirmation) {
// Then make sure we get a uniquely recognizable confirmation.
// Confirmation is signaled with a position report.
getReportedLocation(timeout);
}
else {
// Normal confirmation report wanted. We queue a null command to drain the queue and confirm
// the last real command.
confirmationComplete = false;
CommandLine commandLine = new CommandLine(null, 1);
commandQueue.offer(commandLine, writerQueueTimeout, TimeUnit.MILLISECONDS);
while (!confirmationComplete) {
try {
synchronized(this) {
wait(timeout);
}
}
catch (InterruptedException e) {
Logger.warn(getName() +" was interrupted while waiting for completion.", e);
}
}
}
Logger.trace("{} confirmation complete.", getName());
}
}

Expand Down

0 comments on commit 56eae4d

Please sign in to comment.