MotionPlanner first implementation. Before MappedAxes refactoring.

src/main/java/org/openpnp/machine/reference/ReferenceDriver.java

@@ -45,7 +45,7 @@
  */
 public interface ReferenceDriver extends Driver, WizardConfigurable, PropertySheetHolder, Closeable {
     /**
-     * Performing the hardware homing operation for the given head with mappedAxes. When this call completes 
+     * Performing the hardware homing operation for the given machine mappedAxes. When this call completes 
      * the axes should be at the given location.  
      * 
      * @throws Exception

src/main/java/org/openpnp/machine/reference/ReferenceHead.java

@@ -81,9 +81,7 @@ public void home() throws Exception {
             }
 
             // Reset the homing fiducial location as the new current location.
-            for (Driver driver : mappedAxes.getMappedDrivers(machine)) {
-                ((ReferenceDriver) driver).resetLocation(machine, new MappedAxes(mappedAxes, driver), axesHomingLocation);
-            }
+            machine.getMotionPlanner().resetLocation(machine, axesHomingLocation);
         }
         // Now that the machine is physically homed, do the logical homing.
         super.home();

src/main/java/org/openpnp/machine/reference/ReferenceMachine.java

@@ -338,11 +338,7 @@ public void home() throws Exception {
         // if one rehomes, the isHomed flag has to be removed
         this.setHomed(false);
 
-        // Home all the drivers with their respective mapped axes. 
-        for (Driver driver : getDrivers()) {
-            MappedAxes mappedAxes = new MappedAxes(this, driver);
-            ((ReferenceDriver) driver).home(this, mappedAxes);
-        }
+        getMotionPlanner().home(this);
         super.home();
 
         try {

src/main/java/org/openpnp/machine/reference/SimulationModeMachine.java

@@ -24,11 +24,16 @@
 import java.awt.Color;
 import java.awt.Graphics2D;
 
+import org.openpnp.gui.MainFrame;
 import org.openpnp.gui.support.PropertySheetWizardAdapter;
 import org.openpnp.machine.reference.camera.ImageCamera;
 import org.openpnp.machine.reference.driver.NullDriver;
+import org.openpnp.machine.reference.feeder.BlindsFeeder;
+import org.openpnp.machine.reference.feeder.ReferenceStripFeeder;
 import org.openpnp.machine.reference.wizards.SimulationModeMachineConfigurationWizard;
 import org.openpnp.model.AxesLocation;
+import org.openpnp.model.Board;
+import org.openpnp.model.BoardLocation;
 import org.openpnp.model.Configuration;
 import org.openpnp.model.Length;
 import org.openpnp.model.LengthUnit;
@@ -40,6 +45,7 @@
 import org.openpnp.spi.Camera;
 import org.openpnp.spi.Camera.Looking;
 import org.openpnp.spi.Driver;
+import org.openpnp.spi.Feeder;
 import org.openpnp.spi.HeadMountable;
 import org.openpnp.spi.Machine;
 import org.openpnp.spi.Movable.LocationOption;
@@ -238,6 +244,60 @@ public void setPickAndPlaceChecking(boolean pickAndPlaceChecking) {
         this.pickAndPlaceChecking = pickAndPlaceChecking;
     }
 
+    public void resetAllFeeders() {
+        for (Feeder feeder : getFeeders()) {
+            if (feeder instanceof ReferenceStripFeeder) {
+                ((ReferenceStripFeeder) feeder).setFeedCount(0);
+            }
+            if (feeder instanceof BlindsFeeder) {
+                ((BlindsFeeder) feeder).setFeedCount(0);
+            }
+        }
+    }
+
+    public void setMachineTableZ(Length machineTableZ) {
+        for (Feeder feeder : getFeeders()) {
+            if (feeder instanceof ReferenceFeeder) {
+                ((ReferenceFeeder) feeder)
+                 .setLocation(((ReferenceFeeder) feeder).getLocation()
+                         .derive(null, null, 
+                                 machineTableZ.convertToUnits(((ReferenceFeeder) feeder).getLocation().getUnits())
+                                 .getValue(), 
+                                 null));
+            }
+            if (feeder instanceof ReferenceStripFeeder) {
+                ((ReferenceStripFeeder) feeder)
+                .setReferenceHoleLocation(((ReferenceStripFeeder) feeder).getReferenceHoleLocation()
+                        .derive(null, null, 
+                                machineTableZ.convertToUnits(((ReferenceStripFeeder) feeder).getReferenceHoleLocation().getUnits())
+                                .getValue(), 
+                                null));
+                ((ReferenceStripFeeder) feeder)
+                .setLastHoleLocation(((ReferenceStripFeeder) feeder).getLastHoleLocation()
+                        .derive(null, null, 
+                                machineTableZ.convertToUnits(((ReferenceStripFeeder) feeder).getLastHoleLocation().getUnits())
+                                .getValue(), 
+                                null));
+            }
+        }
+        for (BoardLocation boardLocation : MainFrame.get().getJobTab().getJob().getBoardLocations()) {
+            boardLocation.setLocation(boardLocation.getLocation().derive(null, null, 
+                    machineTableZ.convertToUnits(boardLocation.getLocation().getUnits())
+                    .getValue(), 
+                    null));
+        }
+        for (Camera camera : getCameras()) {
+            if (camera instanceof ReferenceCamera) {
+                ((ReferenceCamera) camera)
+                .setHeadOffsets(((ReferenceCamera) camera).getHeadOffsets()
+                        .derive(null, null, 
+                                machineTableZ.convertToUnits(((ReferenceCamera) camera).getHeadOffsets().getUnits())
+                                .getValue(), 
+                                null));
+            }
+        }
+    }
+
     public static SimulationModeMachine getSimulationModeMachine() {
         Machine machine = Configuration.get()
                 .getMachine();

src/main/java/org/openpnp/machine/reference/axis/ReferenceControllerAxis.java

@@ -41,35 +41,82 @@ public class ReferenceControllerAxis extends AbstractControllerAxis {
     @Element(required = false, data = true)
     private String preMoveCommand;
 
-    @Override
-    public Wizard getConfigurationWizard() {
-        return new ReferenceControllerAxisConfigurationWizard(this);
-    }
+    @Element(required = false)
+    private Length homeCoordinate = new Length(0.0, LengthUnit.Millimeters);
+
+    @Element(required = false)
+    private Length feedratePerSecond = new Length(250, LengthUnit.Millimeters);
+
+    @Element(required = false)
+    private Length accelerationPerSecond2 = new Length(500, LengthUnit.Millimeters);
 
-    // Stored current axis coordinate.
-    private double coordinate;
+    @Element(required = false)
+    private Length jerkPerSecond3 = new Length(2000, LengthUnit.Millimeters);
+
+    /**
+     * The resolution of the axis will be used to determined if an axis has moved i.e. whether the sent coordinate 
+     * will be different.  
+     * @see %.4f format in CommandType.MOVE_TO_COMMAND in GcodeDriver.createDefaults() or Configuration.getLengthDisplayFormat()
+     * Comparing coordinates rounded to resolution will also suppress false differences from floating point artifacts 
+     * prompted by forward/backward raw <-> transformed calculations. 
+     */
+    @Element(required = false)
+    private double resolution = 0.0001; // 
 
     @Override
-    public double getCoordinate() {
-        return coordinate;
+    public Length getHomeCoordinate() {
+        return homeCoordinate;
     }
 
     @Override
-    public Length getLengthCoordinate() {
-        return new Length(coordinate, getUnits());
+    public void setHomeCoordinate(Length homeCoordinate) {
+        Object oldValue = this.homeCoordinate;
+        this.homeCoordinate = homeCoordinate;
+        firePropertyChange("homeCoordinate", oldValue, homeCoordinate);
     }
 
-    @Override
-    public void setCoordinate(double coordinate) {
-        Object oldValue = this.coordinate;
-        this.coordinate = coordinate;
-        firePropertyChange("coordinate", oldValue, coordinate);
-        firePropertyChange("lengthCoordinate", null, getLengthCoordinate());
+    public double getResolution() {
+        return resolution;
+    }
+
+    public void setResolution(double resolution) {
+        Object oldValue = this.resolution;
+        this.resolution = resolution;
+        firePropertyChange("resolution", oldValue, resolution);
     }
 
     @Override
-    public void setLengthCoordinate(Length coordinate) {
-        setCoordinate(coordinate.convertToUnits(getUnits()).getValue());
+    public double roundedToResolution(double coordinate) {
+        if (resolution != 0.0) {
+            return Math.round(coordinate/resolution)*resolution;
+        }
+        else {
+            return coordinate;
+        }
+    }
+
+    public Length getFeedratePerSecond() {
+        return feedratePerSecond;
+    }
+
+    public void setFeedratePerSecond(Length feedratePerSecond) {
+        this.feedratePerSecond = feedratePerSecond;
+    }
+
+    public Length getAccelerationPerSecond2() {
+        return accelerationPerSecond2;
+    }
+
+    public void setAccelerationPerSecond2(Length accelerationPerSecond2) {
+        this.accelerationPerSecond2 = accelerationPerSecond2;
+    }
+
+    public Length getJerkPerSecond3() {
+        return jerkPerSecond3;
+    }
+
+    public void setJerkPerSecond3(Length jerkPerSecond3) {
+        this.jerkPerSecond3 = jerkPerSecond3;
     }
 
     public String getPreMoveCommand() {
@@ -89,22 +136,21 @@ public void setBacklashOffset(Length backlashOffset) {
     }
 
     @Override
-    public AxesLocation toTransformed(AxesLocation location, LocationOption... options) {
-        // No transformation, obviously
-        return location;
-    }
-
-    @Override
-    public AxesLocation toRaw(AxesLocation location, LocationOption... options) {
-        // No transformation, obviously
-        return location;
+    public Wizard getConfigurationWizard() {
+        return new ReferenceControllerAxisConfigurationWizard(this);
     }
 
     @Override
-    public boolean coordinatesMatch(Length coordinateA, Length coordinateB) {
-        double a = roundedToResolution(coordinateA.convertToUnits(getUnits()).getValue());
-        double b = roundedToResolution(coordinateB.convertToUnits(getUnits()).getValue());
-        return a == b;
+    public double getMotionLimit(int order) {
+        if (order == 1) {
+            return getFeedratePerSecond().convertToUnits(LengthUnit.Millimeters).getValue();
+        }
+        else if (order == 2) {
+            return getAccelerationPerSecond2().convertToUnits(LengthUnit.Millimeters).getValue();
+        }
+        else if (order == 3) {
+            return getJerkPerSecond3().convertToUnits(LengthUnit.Millimeters).getValue();
+        }
+        return 0;
     }
-
 }

src/main/java/org/openpnp/machine/reference/axis/ReferenceLinearTransformAxis.java

@@ -45,6 +45,13 @@
 import org.simpleframework.xml.Attribute;
 import org.simpleframework.xml.Element;
 
+/**
+ * The ReferenceLinearTransformAxis is a multi-input axis linear transformation for one output axis. Multiple
+ * ReferenceLinearTransformAxis can be combined into a full Affine Transformation (many-to-many axes). In the reverse 
+ * transformation the ReferenceLinearTransformAxis need to be combined to solve for the inverse transform. The first axis 
+ * encountered will therefore do the job for the combined group. 
+ *
+ */
 public class ReferenceLinearTransformAxis extends AbstractTransformedAxis {
     // The input axes of the transformation. Any of these can be null. 
     private AbstractAxis inputAxisX;
@@ -332,7 +339,7 @@ public AxesLocation toRaw(AxesLocation location, LocationOption... options) thro
             { ReferenceLinearTransformAxis.getLinearCoordinate(location, linearAxes[3], inputAxes[3]) },
             { 1 }
         };
-        // Calculate the raw vector by applying the inverdet Affine Transform.
+        // Calculate the raw vector by applying the inverted Affine Transform.
         double [][] rawVector = Matrix.multiply(invertedAffineTransform, transformedVector);
         // Place the consolidated result in the location
         location = location.put(new AxesLocation(inputAxes[0], rawVector[0][0]));

src/main/java/org/openpnp/machine/reference/axis/ReferenceMappedAxis.java

@@ -34,9 +34,10 @@
 import org.simpleframework.xml.Element;
 
 /**
- * A TransformedAxis for heads with dual linear Z axes powered by one motor. The two Z axes are
- * defined as normal and negated. Normal gets the raw coordinate value and negated gets the same
- * value negated. So, as normal moves up, negated moves down.
+ * The ReferenceMappedAxis can map from an input Axis using two coordinate points mapped to each other.
+ * Ultimately it is a single axis linear transformation but it calculates the scale and offset for you.
+ * It can do scaling, offset, negation, axis reversal etc. while hopefully being easy to setup without any
+ * linear math knowledge. 
  */
 public class ReferenceMappedAxis extends AbstractSingleTransformedAxis {
     @Element(required = false)