Proper acceleration code added, and data needed for GCode look-ahead.

Z moves have been deliberately broken.  This will be fixed in the next
checkin.

.~lock.acceleration-aprx-simulation.ods#

@@ -1 +1 @@
-Adrian Bowyer,ensab,Charles,21.05.2013 15:01,file:///home/ensab/.config/libreoffice/3;
+Adrian Bowyer,ensab,Charles,22.05.2013 16:41,file:///home/ensab/.config/libreoffice/3;

Configuration.h

@@ -56,9 +56,10 @@ Licence: GPL
 #define STRING_LENGTH 1000
 #define POST_LENGTH 200
 #define START_FEED_RATE 200
+#define GCODE_LENGTH 100 // Maximum lenght of internally-generated G Code string
 
 // The axes etc in a GCode
 
-#define GCODE_LETTERS {'X', 'Y', 'Z', 'E', 'F' }
+#define GCODE_LETTERS { 'X', 'Y', 'Z', 'E', 'F' }
 
 #endif

GCodes.ino

@@ -255,9 +255,9 @@ void GCodes::Spin()
   if(!active)
     return;
     
-  if(webserver->Available())
+  if(webserver->GCodeAvailable())
   {
-    if(webGCode->Put(webserver->Read()))
+    if(webGCode->Put(webserver->ReadGCode()))
       ActOnGcode(webGCode);
   }
   

Move.h

@@ -27,7 +27,7 @@ class DDA
 {  
   public:
     DDA(Move* m, Platform* p);
-    boolean Init(float currentPosition[], float targetPosition[]);
+    boolean Init(float currentPosition[], float targetPosition[], float& u, float& v);
     void Start(boolean noTest);
     void Step(boolean noTest);
     boolean Active();
@@ -37,13 +37,16 @@ class DDA
     Platform* platform;
     long counter[DRIVES+1];
     long delta[DRIVES+1];
-    char directions[DRIVES+1];
+    boolean directions[DRIVES+1];
     long totalSteps;
     long stepCount;
     float timeStep;
     float velocity;
     long stopAStep;
     long startDStep;
+    float distance;
+    float dCross;
+    boolean velocitiesAltered;
     volatile boolean active;
 };
 

Move.ino

@@ -68,19 +68,14 @@ void Move::Spin()
 
 void Move::Qmove()
 {
-  //char scratchString[STRING_LENGTH];
   if(!gCodes->ReadMove(nextMove))
     return;
-/*  platform->Message(HOST_MESSAGE, "Move - got a move:");
-  for(char i = 0; i <= DRIVES; i++)
-  {
-    ftoa(scratchString, nextMove[i], 3);
-    platform->Message(HOST_MESSAGE, scratchString);
-    platform->Message(HOST_MESSAGE, ", ");
-  }
-  platform->Message(HOST_MESSAGE, "\n");*/
+    
+  //FIXME
+  float u = platform->Jerk(X_AXIS);
+  float v = u;
   
-  boolean work = dda->Init(currentPosition, nextMove);
+  boolean work = dda->Init(currentPosition, nextMove, u, v);
   
   for(char i = 0; i <= AXES; i++)
     currentPosition[i] = nextMove[i];
@@ -110,21 +105,40 @@ DDA::DDA(Move* m, Platform* p)
   platform = p;
 }
 
-boolean DDA::Init(float currentPosition[], float targetPosition[])
+/*
+
+This sets up the DDA to take us between two positions and extrude states.
+The start velocity is u, and the end one is v.  The requested maximum feedrate
+is in targetPosition[DRIVES].
+
+Almost everything that needs to be done to set this up is also useful
+for GCode look-ahead, so this one function is used for both.  It flags when
+u and v cannot be satisfied with the distance available and reduces them 
+proportionately to give values that can just be achieved, which is why they
+are passed by reference.
+
+The return value is true for an actual move, false for a zero-length (i.e. null) move.
+
+*/
+boolean DDA::Init(float currentPosition[], float targetPosition[], float& u, float& v)
 {
   char drive;
   active = false;
+  velocitiesAltered = false;
   totalSteps = -1;
-  float dist = 0; // X+Y+Z
+  distance = 0; // X+Y+Z
   float d;
   char axesMoving = 0;
+  
+  // How far are we going, both in steps and in mm?
+  
   for(drive = 0; drive < DRIVES; drive++)
   {
     if(drive < AXES)
     {
       d = targetPosition[drive] - currentPosition[drive];
       delta[drive] = (long)(d*platform->DriveStepsPerUnit(drive));  //Absolute
-      dist += d*d;
+      distance += d*d;
     } else
       delta[drive] = (long)(targetPosition[drive]*platform->DriveStepsPerUnit(drive));  // Relative
     if(delta[drive] >= 0)
@@ -132,42 +146,128 @@ boolean DDA::Init(float currentPosition[], float targetPosition[])
     else
       directions[drive] = BACKWARDS;
     delta[drive] = abs(delta[drive]);
+    
+    // Which axes (if any) are moving?
+    
     if(drive == X_AXIS && delta[drive] > 0)
       axesMoving |= 1;
     if(drive == Y_AXIS && delta[drive] > 0)
       axesMoving |= 2;
     if(drive == Z_AXIS && delta[drive] > 0)
-      axesMoving |= 4;      
+      axesMoving |= 4;   
+    
+    // Keep track of the biggest drive move in totalSteps
+    
     if(delta[drive] > totalSteps)
       totalSteps = delta[drive];    
   }
+  
+  // Not going anywhere?
+  
   if(totalSteps <= 0)
     return false;
+  
+  // Set up the DDA
+  
   counter[0] = totalSteps/2;
   for(drive = 1; drive < DRIVES; drive++)
     counter[drive] = counter[0];
-  dist = sqrt(dist);
+  
+  // Acceleration and velocity calculations
+  
+  distance = sqrt(distance);
   float acc;
-  if(axesMoving|4)
+  
+  if(axesMoving|4) // Z involved?
   {
     acc = platform->Acceleration(Z_AXIS);
     velocity = platform->Jerk(Z_AXIS);
-  } else
+  } else // No - only X, Y and Es
   {
     acc = platform->Acceleration(X_AXIS);
     velocity = platform->Jerk(X_AXIS);
   }
+ 
+  // If velocities requested are (almost) zero, set them to the jerk
   
-  timeStep = move->stepDistances[1]/velocity;
-  d = 0.5*(targetPosition[DRIVES]*targetPosition[DRIVES] - velocity*velocity)/acc; // d = (v^2 - u^2)/2a
-  stopAStep = (long)((d*totalSteps)/dist);
-  startDStep = totalSteps - stopAStep;
-  if(stopAStep > startDStep)
+  if(v < 0.01)
+    v = velocity; 
+  if(u < 0.01)
+    u = velocity;
+
+  // At which DDA step should we stop accelerating?
+  
+  d = 0.5*(targetPosition[DRIVES]*targetPosition[DRIVES] - u*u)/acc; // d = (v1^2 - v0^2)/2a
+  stopAStep = (long)((d*totalSteps)/distance);
+  
+  // At which DDA step should we start decelerating?
+  
+  d = 0.5*(v*v - targetPosition[DRIVES]*targetPosition[DRIVES])/acc;  // This should be 0 or negative...
+  startDStep = totalSteps + (long)((d*totalSteps)/distance);
+  
+  // If acceleration stop is at or after deceleration start, then the distance moved
+  // is not enough to get to full speed.
+  
+  if(stopAStep >= startDStep)
   {
-      stopAStep = totalSteps/2;
-      startDStep = stopAStep + 1;
+    // Work out the point at which to stop accelerating and then
+    // immediately start decelerating.
+    
+    dCross = 0.5*(0.5*(v*v - u*u)/acc + distance);
+    
+    if(dCross < 0.0 || dCross > distance)
+    {
+      // With the acceleration available, it is not possible
+      // to satisfy u and v within the distance; reduce u and v 
+      // proportionately to get ones that work and flag the fact.  
+      // The result is two velocities that can just be accelerated
+      // or decelerated between over the distance to get
+      // from one to the other.
+      
+      float k = v/u;
+      u = 2.0*acc*distance/(k*k - 1);
+      if(u >= 0.0)
+      {
+        u = sqrt(u);
+        v = k*u;
+      } else
+      {
+        v = sqrt(-u);
+        u = v/k;
+      }
+      
+      dCross = 0.5*(0.5*(v*v - u*u)/acc + distance);
+      velocitiesAltered = true;
+    }
+    
+    // The DDA steps at which acceleration stops and deceleration starts
+    
+    stopAStep = (long)((dCross*totalSteps)/distance);
+    startDStep = stopAStep + 1;
   }
+  
+  // The initial velocity
+  
+  velocity = u;
+  
+  // Sanity check
+  
+  if(velocity <= 0.0)
+  {
+    velocity = 1.0;
+    platform->Message(HOST_MESSAGE, "DDA.Init(): Zero or negative initial velocity!");
+  }
+  
+  // How far have we gone?
+  
   stepCount = 0;
+  
+  // Guess that the first DDA move will be in roughly the direction 
+  // recorded in axesMoving.  This is a simple heuristic, and any
+  // small error will be forgotten with the very next step.
+  
+  timeStep = move->stepDistances[axesMoving]/velocity;
+  
   return true;
 }
 

Platform.h

@@ -59,24 +59,23 @@ Licence: GPL
 
 #define STEP_PINS {54, 60, 46, 26}
 #define DIRECTION_PINS {55, 61, 48, 28}
-#define FORWARDS 1     // What to send to go... 
-#define BACKWARDS 0    // ...in each direction
+#define FORWARDS true     // What to send to go... 
+#define BACKWARDS false    // ...in each direction
 #define ENABLE_PINS {38, -1, 62, -1}
-#define ENABLE 0      // What to send to enable... 
-#define DISABLE 1     // ...and disable a drive
+#define ENABLE false      // What to send to enable... 
+#define DISABLE true     // ...and disable a drive
 #define DISABLE_DRIVES {false, false, true, false} // Set true to disable a drive when it becomes idle
 #define LOW_STOP_PINS {3, 14, 17, -1}
 #define HIGH_STOP_PINS {-1, -1, -1, -1}
 #define ENDSTOP_HIT 1 // when a stop == this it is hit
 #define MAX_FEEDRATES {300, 300, 3, 45}    // mm/sec   
 #define ACCELERATIONS {800, 800, 30, 250}    // mm/sec^2??
-//#define ACCELERATIONS {80, 80, 3, 25}
 #define DRIVE_STEPS_PER_UNIT {91.4286, 91.4286, 4000, 929}
 #define JERKS {15.0, 15.0, 0.4, 15.0}    // (mm/sec)
-//#define JERKS {5.0, 5.0, 0.1, 5.0}    // (mm/sec)
-// AXES
 
 
+// AXES
+
 #define AXIS_LENGTHS {210, 210, 120} // mm
 #define FAST_HOME_FEEDRATES {50*60, 50*60, 1*60}  // mm/min
 
@@ -84,6 +83,7 @@ Licence: GPL
 #define Y_AXIS 1  // The index of the Y axis
 #define Z_AXIS 2  // The index of the Z axis
 
+
 // HEATERS - Bed is assumed to be the first
 
 #define TEMP_SENSE_PINS {10, 9}  // Analogue pin numbers
@@ -152,7 +152,7 @@ Licence: GPL
 
 #define BAUD_RATE 115200 // Communication speed of the USB if needed.
 
-#define GCODE_LENGTH 100 // Maximum lenght of internally-generated G Code string
+
 
 
 /****************************************************************************************************/

RepRapFirmware.ino

@@ -106,7 +106,9 @@ void RepRap::InterruptTime()
   DDA* dda = new DDA(move, platform);
   float a[] = {1.0, 2.0, 3.0, 4.0, 5.0};
   float b[] = {2.0, 3.0, 4.0, 5.0, 6.0};
-  dda->Init(a, b);
+  float u = 50;
+  float v = 50;
+  dda->Init(a, b, u, v);
   dda->Start(false);
   unsigned long t = platform->Time();
   for(long i = 0; i < 100000; i++) 

Webserver.h

@@ -36,8 +36,8 @@ class Webserver
   public:
   
     Webserver(Platform* p);
-    boolean Available();
-    byte Read();
+    boolean GCodeAvailable();
+    byte ReadGCode();
     void Init();
     void Spin();
     void Exit();

Webserver.ino

@@ -61,12 +61,12 @@ boolean Webserver::MatchBoundary(char c)
 
 // Feeding G Codes to the GCodes class
 
-boolean Webserver::Available()
+boolean Webserver::GCodeAvailable()
 {
   return gcodeAvailable;
 }
 
-byte Webserver::Read()
+byte Webserver::ReadGCode()
 {
   byte c = gcodeBuffer[gcodePointer];
   if(!c)