Skip to content

Commit

Permalink
Backup commit.
Browse files Browse the repository at this point in the history
  • Loading branch information
@markmaker
markmaker committed May 30, 2020
1 parent 5de4b1c commit 4a5f01f
Show file tree
Hide file tree
Showing 10 changed files with 904 additions and 262 deletions.
173 changes: 158 additions & 15 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 @@ -2,35 +2,31 @@

import java.util.ArrayList;
import java.util.List;
import java.util.Map;
import java.util.TreeMap;
import java.util.Map.Entry;

import org.openpnp.ConfigurationListener;
import org.openpnp.machine.reference.ReferenceDriver;
import org.openpnp.machine.reference.ReferenceHeadMountable;
import org.openpnp.machine.reference.ReferenceMachine;
import org.openpnp.machine.reference.axis.ReferenceControllerAxis;
import org.openpnp.machine.reference.axis.ReferenceVirtualAxis;
import org.openpnp.machine.reference.vision.ReferenceBottomVision;
import org.openpnp.model.AxesLocation;
import org.openpnp.model.Configuration;
import org.openpnp.model.Length;
import org.openpnp.model.Motion;
import org.openpnp.model.Motion.Derivative;
import org.openpnp.model.Motion.MotionOption;
import org.openpnp.spi.Axis;
import org.openpnp.spi.Axis.Type;
import org.openpnp.spi.ControllerAxis;
import org.openpnp.spi.CoordinateAxis;
import org.openpnp.spi.Driver;
import org.openpnp.spi.Head;
import org.openpnp.spi.HeadMountable;
import org.openpnp.spi.Machine;
import org.openpnp.spi.MotionPlanner;
import org.openpnp.spi.Axis.Type;
import org.openpnp.spi.MotionPlanner.CompletionType;
import org.openpnp.spi.Movable.MoveToOption;
import org.openpnp.spi.base.AbstractControllerAxis;
import org.openpnp.util.NanosecondTime;
import org.openpnp.util.Utils2D;
import org.simpleframework.xml.Attribute;
import org.simpleframework.xml.core.Commit;

/**
* The AbstractMotionPlanner provides a basic framework for sub-classing.
Expand All @@ -43,17 +39,17 @@ public abstract class AbstractMotionPlanner implements MotionPlanner {

private ReferenceMachine machine;

protected static class MotionCommand {
public static class MotionCommand {
final double timeRecorded;
final HeadMountable hm;
final HeadMountable headMountable;
final AxesLocation axesLocation;
final double speed;
final MoveToOption [] options;

public MotionCommand(HeadMountable hm, AxesLocation axesLocation, double speed,
public MotionCommand(HeadMountable headMountable, AxesLocation axesLocation, double speed,
MoveToOption[] options) {
super();
this.hm = hm;
this.headMountable = headMountable;
this.timeRecorded = NanosecondTime.getRuntimeSeconds();
this.axesLocation = axesLocation;
this.speed = speed;
Expand All @@ -68,6 +64,10 @@ public AxesLocation getAxesLocation() {
return axesLocation;
}

public HeadMountable getHeadMountable() {
return headMountable;
}

public double getSpeed() {
return speed;
}
Expand Down Expand Up @@ -183,10 +183,128 @@ else if (refAxis.isLimitRotation()) {
return axesLocation;
}

protected double planExecutedTime = 0;

public void executeMotionPlan() throws Exception {
ReferenceMachine machine = (ReferenceMachine) Configuration.get().getMachine();
List<Head> movedHeads = new ArrayList<>();
for (Motion plannedMotion : motionPlan.tailMap(planExecutedTime, false).values()) {
// Remember this motion was executed up-front, so an exception in the execution will not matter i.e. we
// are not stuck with this motion again and again.
planExecutedTime = plannedMotion.getTime();
if (!plannedMotion.hasOption(MotionOption.Stillstand)) {
for (Driver driver : plannedMotion.getLocation().getAxesDrivers(machine)) {
AxesLocation previousLocation = new AxesLocation(plannedMotion.getLocation().getAxes(driver),
(axis) -> (axis.getDriverLengthCoordinate()));
// Derive the driver's motion from the planned motion.
Motion driverMotion = Motion.computeWithLimits(plannedMotion.getMotionCommand(),
previousLocation, plannedMotion.getLocation(), 1.0, false, true,
(axis, order) -> (plannedMotion.getVector(order).getCoordinate(axis)));
if (driverMotion != null) {
ReferenceHeadMountable hm = null;
MoveToOption [] options = null;
if (plannedMotion.getMotionCommand() != null) {
options = plannedMotion.getMotionCommand().getOptions();
hm = (ReferenceHeadMountable) plannedMotion.getMotionCommand().getHeadMountable();
}
((ReferenceDriver) driver).moveTo(hm, driverMotion, options);
if (hm != null) {
movedHeads.add(hm.getHead());
}
}
}
}
}
for (Head movedHead : movedHeads) {
machine.fireMachineHeadActivity(movedHead);
}
}

protected void moveToPlanning(HeadMountable hm, AxesLocation axesLocation,
double speed, MoveToOption... options) throws Exception {
// Add the command
commandSequence.add(new MotionCommand(hm, axesLocation, speed, options));
MotionCommand motionCommand = new MotionCommand(hm, axesLocation, speed, options);
commandSequence.add(motionCommand);
queueMotion(motionCommand);
}

protected void queueMotion(MotionCommand motionCommand) throws Exception {
// Get real-time.
double now = NanosecondTime.getRuntimeSeconds();
// Get the last entry.
double lastMotionTime = Double.MIN_VALUE;
Motion lastMotion = null;
Map.Entry<Double, Motion> lastEntry = motionPlan.lastEntry();
double startTime = now;
if (lastEntry != null) {
lastMotionTime = lastEntry.getKey();
lastMotion = lastEntry.getValue();
if (lastMotionTime > now) {
// Continue after last motion.
startTime = lastMotionTime;
}
else {
// Pause between the moves.
lastMotion = new Motion(0, null,
new AxesLocation [] { lastMotion.getVector(Motion.Derivative.Location) },
MotionOption.FixedWaypoint, MotionOption.CoordinatedWaypoint, MotionOption.Stillstand);
motionPlan.put(startTime, lastMotion);
}
}
else {
// No lastMotion, create the previous waypoint from the axes.
AxesLocation previousLocation = new AxesLocation(Configuration.get().getMachine());
lastMotion = new Motion(0, null,
new AxesLocation [] { previousLocation },
MotionOption.FixedWaypoint, MotionOption.CoordinatedWaypoint, MotionOption.Stillstand);
motionPlan.put(startTime, lastMotion);
}
// Note this must include all the machine axes, not just the ones included in this moveTo().
AxesLocation lastLocation =
new AxesLocation(Configuration.get().getMachine())
.put(lastMotion.getVector(Motion.Derivative.Location));
Motion plannedMotion = Motion.computeWithLimits(motionCommand, lastLocation,
motionCommand.getAxesLocation(),
motionCommand.getSpeed(), true, false);
if (plannedMotion != null) {
motionPlan.put(startTime + plannedMotion.getTime(), plannedMotion);
}
}

@Override
public Motion getMomentaryMotion(double time) {
Map.Entry<Double, Motion> entry0 = motionPlan.floorEntry(time);
Map.Entry<Double, Motion> entry1 = motionPlan.ceilingEntry(time);
if (entry0 != null && entry1 != null) {
// We're between two way-points, interpolate linearly by time.
double dt = entry1.getKey() - entry0.getKey();
double ratio = (time - entry0.getKey())/dt;
Motion interpolatedMotion = entry0.getValue().interpolate(entry1.getValue(), ratio);
//Logger.trace("time = "+time+", ratio "+ratio+" motion="+interpolatedMotion);
return interpolatedMotion;
}
else if (entry0 != null){
// Machine stopped before this time. Just return the last location.
return new Motion(0.0,
null,
new AxesLocation [] { entry0.getValue().getVector(Derivative.Location) },
MotionOption.Stillstand);
}
else if (entry1 != null){
// Planning starts after this time. Return the first known location.
return new Motion(0.0,
null,
new AxesLocation [] { entry1.getValue().getVector(Derivative.Location) },
MotionOption.Stillstand);
}
else {
// Nothing in the plan, just get the current axes location.
AxesLocation currentLocation = new AxesLocation(Configuration.get().getMachine());
return new Motion(0.0,
null,
new AxesLocation [] { currentLocation },
MotionOption.Stillstand);
}
}

@Override
Expand All @@ -200,7 +318,7 @@ public void waitForCompletion(HeadMountable hm, CompletionType completionType)
if (refAxis.isLimitRotation() && refAxis.isWrapAroundRotation()) {
double anglePresent = refAxis.getDriverCoordinate();
double angleWrappedAround = Utils2D.normalizeAngle180(anglePresent);
if (!axis.coordinatesMatch(anglePresent, angleWrappedAround)) {
if (anglePresent != angleWrappedAround) {
((ReferenceDriver) refAxis.getDriver()).setGlobalOffsets(getMachine(),
new AxesLocation(refAxis, angleWrappedAround));
// This also reflects in the motion planner's coordinate.
Expand All @@ -211,6 +329,31 @@ public void waitForCompletion(HeadMountable hm, CompletionType completionType)
}
}

protected void waitForDriverCompletion(HeadMountable hm, CompletionType completionType)
throws Exception {
// The null motion planner is a pure proxy, so there is nothing left to do except wait for the driver(s).
ReferenceMachine machine = (ReferenceMachine) Configuration.get().getMachine();
// If the hm is given, we just wait for the drivers of that hm, otherwise we wait for all drivers of the machine axes.
AxesLocation mappedAxes = (hm != null ?
hm.getMappedAxes(machine)
: new AxesLocation(machine));

if (!mappedAxes.isEmpty()) {
for (Driver driver : mappedAxes.getAxesDrivers(machine)) {
((ReferenceDriver) driver).waitForCompletion((ReferenceHeadMountable) hm, completionType);
}
if (hm != null) {
machine.fireMachineHeadActivity(hm.getHead());
}
else {
for (Head head : machine.getHeads()) {
machine.fireMachineHeadActivity(head);
}
}
}
}


@Override
public ReferenceMachine getMachine() {
if (machine == null) {
Expand Down
124 changes: 0 additions & 124 deletions src/main/java/org/openpnp/machine/reference/driver/AbstractSimpleMotionPlanner.java
View file

This file was deleted.

0 comments on commit 4a5f01f

Please sign in to comment.