First implementation of the optimised maximum speed in any direction code. This is still buggy. To print with, use the previous release.

Configuration.h

@@ -24,8 +24,8 @@ Licence: GPL
 #define CONFIGURATION_H
 
 #define NAME "RepRapFirmware"
-#define VERSION "0.59"
+#define VERSION "0.60"
-#define DATE "2014-01-15"
+#define DATE "2014-01-16"
 #define LAST_AUTHOR "reprappro.com"
 
 // Other firmware that we might switch to be compatible with.

Move.cpp

@@ -179,46 +179,65 @@ void Move::Spin()
     for(int8_t drive = 0; drive < DRIVES; drive++)
     	nextMachineEndPoints[drive] = LookAhead::EndPointToMachine(drive, nextMove[drive]);
 
-    int8_t movementType = GetMovementType(lastMove->MachineEndPoints(), nextMachineEndPoints);
+    float minS, maxS, maxA, steps;
+    int8_t axis;
+
+    //int8_t movementType = GetMovementType(lastMove->MachineEndPoints(), nextMachineEndPoints);
 
     // Throw it away if there's no real movement.
     
-    if(movementType == noMove)
+    if(!MaxTruncatedProjection(lastMove->MachineEndPoints(),  nextMachineEndPoints,  platform->MaxFeedrate(), maxS, axis))
     {
        platform->ClassReport("Move", longWait);
        return;
     }
 
+    MaxTruncatedProjection(lastMove->MachineEndPoints(),  nextMachineEndPoints,  platform->InstantDv(), minS, axis);
+
+    MaxTruncatedProjection(lastMove->MachineEndPoints(),  nextMachineEndPoints,  platform->Acceleration(), maxA, axis);
+
+    steps = platform->DriveStepsPerUnit(axis);
+
+    SerialUSB.print("min, max, a and s: ");
+    SerialUSB.print(minS);
+    SerialUSB.print(" ");
+    SerialUSB.print(maxS);
+    SerialUSB.print(" ");
+    SerialUSB.print(maxA);
+    SerialUSB.print(" ");
+    SerialUSB.print(steps);
+    SerialUSB.print("\n");
+     
     // Real move - record its feedrate with it, not here.
     
     currentFeedrate = -1.0;
     
     // Promote minimum feedrates and restrict maximum feedrates; assumes xy overrides e overrides z FIXME??
     
-    float minS, maxS, maxA, steps;
+//
-    
+//
-    if(movementType & xyMove)
+//    if(movementType & xyMove)
-    {
+//    {
-    	minS = platform->InstantDv(X_AXIS);
+//    	minS = platform->InstantDv(X_AXIS);
-    	maxS = platform->MaxFeedrate(X_AXIS);  // Assumes X and Y are equal.  FIXME?
+//    	maxS = platform->MaxFeedrate(X_AXIS);  // Assumes X and Y are equal.  FIXME?
-    	maxA = platform->Acceleration(X_AXIS);
+//    	maxA = platform->Acceleration(X_AXIS);
-    	steps = platform->DriveStepsPerUnit(X_AXIS);
+//
-    	//platform->Message(HOST_MESSAGE, " xyMove\n");
+//    	//platform->Message(HOST_MESSAGE, " xyMove\n");
-    } else if(movementType & eMove)
+//    } else if(movementType & eMove)
-    {
+//    {
-    	minS = platform->InstantDv(AXES);
+//    	minS = platform->InstantDv(AXES);
-    	maxS = platform->MaxFeedrate(AXES); // Picks up the value for the first extruder.  FIXME?
+//    	maxS = platform->MaxFeedrate(AXES); // Picks up the value for the first extruder.  FIXME?
-    	maxA = platform->Acceleration(AXES);
+//    	maxA = platform->Acceleration(AXES);
-    	steps = platform->DriveStepsPerUnit(AXES);
+//    	steps = platform->DriveStepsPerUnit(AXES);
-    	//platform->Message(HOST_MESSAGE, " eMove\n");
+//    	//platform->Message(HOST_MESSAGE, " eMove\n");
-    } else // Must be z
+//    } else // Must be z
-    {
+//    {
-    	minS = platform->InstantDv(Z_AXIS);
+//    	minS = platform->InstantDv(Z_AXIS);
-    	maxS = platform->MaxFeedrate(Z_AXIS);
+//    	maxS = platform->MaxFeedrate(Z_AXIS);
-    	maxA = platform->Acceleration(Z_AXIS);
+//    	maxA = platform->Acceleration(Z_AXIS);
-    	steps = platform->DriveStepsPerUnit(Z_AXIS);
+//    	steps = platform->DriveStepsPerUnit(Z_AXIS);
-    	//platform->Message(HOST_MESSAGE, " zMove\n");
+//    	//platform->Message(HOST_MESSAGE, " zMove\n");
-    }
+//    }
 
     nextMove[DRIVES] = fmax(fmin(nextMove[DRIVES], maxS), minS);
     
@@ -271,6 +290,70 @@ void Move::Diagnostics()
     */
 }
 
+/*
+ * Box[] is a constraint on a vector - say a list of maximum speeds for
+ * each axis.  sp[] and ep[] are the start and end points of that vector.
+ * This finds the length of that vector such that it extends to the surface
+ * of the box.  It then sets the length of the resulting vector.  It also
+ * returns the axis corresponding to the longest component of vector.
+ *
+ * If the machine moves along the vector at that speed, then all the speeds along
+ * all the other directions are guaranteed not to lie outside the box.
+ *
+ * Note that this means that for, say, a diagonal move in X and Y with equal
+ * maxima along each axis, then the maximum speed returned will be sqrt(2) times
+ * that speed.
+ *
+ * The function returns true if input vector is not of 0 length.
+ */
+bool Move::MaxTruncatedProjection(long sp[], long ep[], float box[], float& length, int8_t& axis)
+{
+	float vector[DRIVES];
+	long lVector;
+	float s;
+	float t = FLT_MAX;  // Slight hack
+	int8_t drive;
+
+	for(drive = 0; drive < DRIVES; drive++)
+	{
+		lVector = labs(ep[drive] - sp[drive]);
+		if(lVector)
+		{
+			vector[drive] = MachineToPoint(lVector, drive);
+			s = box[drive]/vector[drive];
+			if(s < t)
+				t = s;
+		} else
+			vector[drive] = 0.0;
+	}
+
+	if(t == FLT_MAX) // No movement, so result doesn't matter.  But safest not to set it to 0.
+	{
+		length = box[0];
+		return false;
+	}
+
+	length = 0.0;
+
+	s = vector[0];
+	axis = 0;
+
+	for(drive = 0; drive < DRIVES; drive++)
+	{
+		length += vector[drive]*vector[drive];
+
+		if(vector[drive] > s)
+		{
+			s = vector[drive];
+			axis = drive;
+		}
+	}
+
+	length = sqrt(length)*t;
+
+	return true;
+}
+
 // This returns false if it is not possible
 // to use the result as the basis for the
 // next move because the look ahead ring
@@ -307,36 +390,36 @@ bool Move::GetCurrentState(float m[])
 // for the bed's plane, which means that a move is MAINLY and XY move, or MAINLY a Z move. It
 // is the main type of move that is returned.
 
-int8_t Move::GetMovementType(long p0[], long p1[])
+//int8_t Move::GetMovementType(long p0[], long p1[])
-{
+//{
-  int8_t result = noMove;
+//  int8_t result = noMove;
-  long dxy = 0;
+//  long dxy = 0;
-  long dz = 0;
+//  long dz = 0;
-  long d;
+//  long d;
-
+//
-  for(int8_t drive = 0; drive < DRIVES; drive++)
+//  for(int8_t drive = 0; drive < DRIVES; drive++)
-  {
+//  {
-	  if(drive < AXES)
+//	  if(drive < AXES)
-	  {
+//	  {
-		  d = llabs(p1[drive] - p0[drive]);
+//		  d = llabs(p1[drive] - p0[drive]);
-		  if(drive == Z_AXIS)
+//		  if(drive == Z_AXIS)
-			  dz = d;
+//			  dz = d;
-		  else if(d > dxy)
+//		  else if(d > dxy)
-			  dxy = d;
+//			  dxy = d;
-	  } else
+//	  } else
-	  {
+//	  {
-		  if( p1[drive] )
+//		  if( p1[drive] )
-			  result |= eMove;
+//			  result |= eMove;
-	  }
+//	  }
-  }
+//  }
-  dxy *= (long)roundf(platform->DriveStepsPerUnit(Z_AXIS)/platform->DriveStepsPerUnit(X_AXIS));
+//  dxy *= (long)roundf(platform->DriveStepsPerUnit(Z_AXIS)/platform->DriveStepsPerUnit(X_AXIS));
-  if(dxy > dz)
+//  if(dxy > dz)
-	  result |= xyMove;
+//	  result |= xyMove;
-  else if(dz)
+//  else if(dz)
-	  result |= zMove;
+//	  result |= zMove;
-
+//
-  return result;
+//  return result;
-}
+//}
 
 void Move::SetStepHypotenuse()
 {

Move.h

@@ -194,7 +194,7 @@ class Move
     void Diagnostics();
     float ComputeCurrentCoordinate(int8_t drive, LookAhead* la, DDA* runningDDA);
     void SetStepHypotenuse();
-    
+    float MachineToPoint(long steps, int8_t drive);
 
     friend class DDA;
     
@@ -211,7 +211,7 @@ class Move
     bool LookAheadRingFull();
     bool LookAheadRingAdd(long ep[], float feedRate, float vv, bool ce, float minS, float maxS, float maxA, float s);
     LookAhead* LookAheadRingGet();
-    int8_t GetMovementType(long sp[], long ep[]);
+    bool MaxTruncatedProjection(long sp[], long ep[], float box[], float& length, int8_t& axis);
 
     float liveCoordinates[DRIVES + 1];
     
@@ -276,9 +276,7 @@ inline float LookAhead::V()
 
 inline float LookAhead::MachineToEndPoint(int8_t drive)
 {
-	if(drive >= DRIVES)
+	return move->MachineToPoint(endPoint[drive], drive);
-		platform->Message(HOST_MESSAGE, "MachineToEndPoint() called for feedrate!\n");
-	return ((float)(endPoint[drive]))/platform->DriveStepsPerUnit(drive);
 }
 
 
@@ -425,6 +423,13 @@ inline void Move::LiveCoordinates(float m[])
 	InverseTransform(m);
 }
 
+inline float Move::MachineToPoint(long steps, int8_t drive)
+{
+	if(drive >= DRIVES)
+		platform->Message(HOST_MESSAGE, "MachineToPoint() called for feedrate!\n");
+	return ((float)(steps))/platform->DriveStepsPerUnit(drive);
+}
+
 
 // These are the actual numbers that we want to be the coordinates, so
 // don't transform them.

Platform.cpp

@@ -407,7 +407,7 @@ void MassStorage::Init()
 	hsmciPinsinit();
 	// Initialize SD MMC stack
 	sd_mmc_init();
-	delay(20);
+	delay(5);
 	int sdPresentCount = 0;
 	while ((CTRL_NO_PRESENT == sd_mmc_check(0)) && (sdPresentCount < 5))
 	{

Platform.h

@@ -507,10 +507,15 @@ class Platform
   void CoolingFan(float speed);
   //void SetHeatOn(int8_t ho); //TEMPORARY - this will go away...
 
+  friend class Move;
+
 //-------------------------------------------------------------------------------------------------------
   protected:
   
   void ReturnFileStore(FileStore* f);
+  float* Acceleration();
+  float* MaxFeedrate();
+  float* InstantDv();
   
   private:
   
@@ -720,6 +725,21 @@ inline float Platform::InstantDv(int8_t drive)
   return instantDvs[drive]; 
 }
 
+inline float* Platform::Acceleration()
+{
+	return accelerations;
+}
+
+inline float* Platform::MaxFeedrate()
+{
+	return maxFeedrates;
+}
+
+inline float* Platform::InstantDv()
+{
+	return instantDvs;
+}
+
 inline bool Platform::HighStopButNotLow(int8_t axis)
 {
 	return (lowStopPins[axis] < 0)  && (highStopPins[axis] >= 0);