First crude hack at acceleration code. Timer claims that the interrupt

routine executes in 6.5 microseconds, so we shouldn't have any trouble
driving the steppers fast...

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

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

GCodes.ino

@@ -88,7 +88,7 @@ void GCodes::SetUpMove(GCodeBuffer *gb)
   // Deal with feedrate
   
   if(gb->Seen(gCodeLetters[DRIVES]))
-    moveBuffer[DRIVES] = gb->GetFValue()*distanceScale;
+    moveBuffer[DRIVES] = gb->GetFValue()*distanceScale/60.0; // Feedrates are in mm/minute; we need mm/sec
     
   // Remember for next time if we are switched
   // to absolute drive moves

Move.h

@@ -24,23 +24,27 @@ Licence: GPL
 #define BUFFER_LENGTH 10
 
 class DDA
-{
+{  
   public:
     DDA(Move* m, Platform* p);
     boolean Init(float currentPosition[], float targetPosition[]);
-    void Start();
-    void Step();
+    void Start(boolean noTest);
+    void Step(boolean noTest);
     boolean Active();
 
-  private:        
+  private:  
     Move* move;
     Platform* platform;
     long counter[DRIVES+1];
     long delta[DRIVES+1];
-    boolean directions[DRIVES+1];
+    char directions[DRIVES+1];
     long totalSteps;
-    long stepCountDown;
-    boolean active;
+    long stepCount;
+    float timeStep;
+    float velocity;
+    long stopAStep;
+    long startDStep;
+    volatile boolean active;
 };
 
 class Move
@@ -55,7 +59,7 @@ class Move
     void GetCurrentState(float m[]);
     void Interrupt();
 
-
+  friend class DDA;
     
   private:
   
@@ -67,6 +71,7 @@ class Move
     float currentFeedrate;
     float currentPosition[AXES]; // Note - drives above AXES are always relative moves
     float nextMove[DRIVES + 1];  // Extra is for feedrate
+    float stepDistances[8]; // Index bits: lsb -> dx, dy, dz <- msb
 };
 
 inline boolean DDA::Active()
@@ -76,7 +81,7 @@ inline boolean DDA::Active()
 
 inline void Move::Interrupt()
 {
-  dda->Step();
+  dda->Step(true);
 }
 
 

Move.ino

@@ -30,13 +30,26 @@ Move::Move(Platform* p, GCodes* g)
 
 void Move::Init()
 {
-  lastTime = platform->Time();
-  for(char i = 0; i < DRIVES; i++)
+  char i;
+  
+  for(i = 0; i < DRIVES; i++)
     platform->SetDirection(i, FORWARDS);
-  for(char i = 0; i <= AXES; i++)
+  for(i = 0; i <= AXES; i++)
     currentPosition[i] = 0.0;  
   lastTime = platform->Time();
   currentFeedrate = START_FEED_RATE;
+  float dx = 1.0/platform->DriveStepsPerUnit(X_AXIS);
+  float dy = 1.0/platform->DriveStepsPerUnit(Y_AXIS);
+  float dz = 1.0/platform->DriveStepsPerUnit(Z_AXIS);
+  stepDistances[0] = dx; // Should never be used.  Wrong, but safer than 0.0...
+  stepDistances[1] = dx;
+  stepDistances[2] = dy;
+  stepDistances[3] = sqrt(dx*dx + dy*dy);
+  stepDistances[4] = dz;
+  stepDistances[5] = sqrt(dx*dx + dz*dz);
+  stepDistances[6] = sqrt(dy*dy + dz*dz);
+  stepDistances[7] = sqrt(dx*dx + dy*dy + dz*dz);
+
   active = true;  
 }
 
@@ -73,7 +86,7 @@ void Move::Qmove()
     currentPosition[i] = nextMove[i];
     
   if(work)
-    dda->Start();
+    dda->Start(true);
 }
 
 void Move::GetCurrentState(float m[])
@@ -102,14 +115,29 @@ boolean DDA::Init(float currentPosition[], float targetPosition[])
   char drive;
   active = false;
   totalSteps = -1;
+  float dist = 0; // X+Y+Z
+  float d;
+  char axesMoving = 0;
   for(drive = 0; drive < DRIVES; drive++)
   {
     if(drive < AXES)
-      delta[drive] = (long)((targetPosition[drive] - currentPosition[drive])*platform->DriveStepsPerUnit(drive));  //Absolute
-    else
+    {
+      d = targetPosition[drive] - currentPosition[drive];
+      delta[drive] = (long)(d*platform->DriveStepsPerUnit(drive));  //Absolute
+      dist += d*d;
+    } else
       delta[drive] = (long)(targetPosition[drive]*platform->DriveStepsPerUnit(drive));  // Relative
-    directions[drive] = (delta[drive] >= 0);
+    if(delta[drive] >= 0)
+      directions[drive] = FORWARDS;
+    else
+      directions[drive] = BACKWARDS;
     delta[drive] = abs(delta[drive]);
+    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;      
     if(delta[drive] > totalSteps)
       totalSteps = delta[drive];    
   }
@@ -118,35 +146,112 @@ boolean DDA::Init(float currentPosition[], float targetPosition[])
   counter[0] = totalSteps/2;
   for(drive = 1; drive < DRIVES; drive++)
     counter[drive] = counter[0];
-  stepCountDown = totalSteps;
+  dist = sqrt(dist);
+  float acc;
+  if(axesMoving|4)
+  {
+    acc = platform->Acceleration(Z_AXIS);
+    velocity = platform->Jerk(Z_AXIS);
+  } else
+  {
+    acc = platform->Acceleration(X_AXIS);
+    velocity = platform->Jerk(X_AXIS);
+  }
+  
+  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)
+  {
+      stopAStep = totalSteps/2;
+      startDStep = stopAStep + 1;
+  }
+  stepCount = 0;
   return true;
 }
 
-void DDA::Start()
+void DDA::Start(boolean noTest)
 {
   for(char drive = 0; drive < DRIVES; drive++)
     platform->SetDirection(drive, directions[drive]);
-  platform->SetInterrupt(300);
+  if(noTest)
+    platform->SetInterrupt((long)(1.0e6*timeStep));
   active = true;  
 }
 
-void DDA::Step()
+void DDA::Step(boolean noTest)
 {
-  if(!active)
+  if(!active && noTest)
     return;
+  
+  char axesMoving = 0;
+  
+  counter[X_AXIS] += delta[X_AXIS];
+  if(counter[X_AXIS] > 0)
+  {
+    if(noTest)
+      platform->Step(X_AXIS);
+    axesMoving |= 1;
+    counter[X_AXIS] -= totalSteps;
+  }  
+  
+  counter[Y_AXIS] += delta[Y_AXIS];
+  if(counter[Y_AXIS] > 0)
+  {
+    if(noTest)
+      platform->Step(Y_AXIS);
+    axesMoving |= 2;
+    counter[Y_AXIS] -= totalSteps;
+  }  
+  
+  counter[Z_AXIS] += delta[Z_AXIS];
+  if(counter[Z_AXIS] > 0)
+  {
+    if(noTest)
+      platform->Step(Z_AXIS);
+    axesMoving |= 4;
+    counter[Z_AXIS] -= totalSteps;
+  }  
+  
     
-  for(char drive = 0; drive < DRIVES; drive++)
+  for(char drive = AXES; drive < DRIVES; drive++)
   {
     counter[drive] += delta[drive];
     if(counter[drive] > 0)
     {
-      platform->Step(drive);
+      if(noTest)
+        platform->Step(drive);
       counter[drive] -= totalSteps;
     }
   }
   
-  // Deal with feedrates here
+  if(axesMoving)
+  {
+    if(stepCount < stopAStep)
+    {
+      timeStep = move->stepDistances[axesMoving]/velocity;
+      if(axesMoving & 4)
+        velocity += platform->Acceleration(Z_AXIS)*timeStep;
+      else
+        velocity += platform->Acceleration(X_AXIS)*timeStep;
+      if(noTest)
+        platform->SetInterrupt((long)(1.0e6*timeStep));
+    }
+    if(stepCount >= startDStep)
+    {
+      timeStep = move->stepDistances[axesMoving]/velocity;
+      if(axesMoving & 4)
+        velocity -= platform->Acceleration(Z_AXIS)*timeStep;
+      else
+        velocity -= platform->Acceleration(X_AXIS)*timeStep;
+      if(noTest)
+        platform->SetInterrupt((long)(1.0e6*timeStep));
+    }
+  }
   
-  stepCountDown--;
-  active = stepCountDown > 0;
+  stepCount++;
+  active = stepCount < totalSteps;
+  if(!active && noTest)
+    platform->SetInterrupt(-1);
 }

Platform.h

@@ -69,10 +69,11 @@ Licence: GPL
 #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 MAX_ACCELERATIONS {800, 800, 30, 250}    // mm/sec^2?? Maximum start speed for accelerated moves.
+#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
 
 
@@ -219,6 +220,8 @@ class Platform
   void Disable(byte drive); // There is no drive enable; drives get enabled automatically the first time they are used.
   void Home(byte axis);
   float DriveStepsPerUnit(char drive);
+  float Acceleration(char drive);
+  float Jerk(char drive);
   
   float ZProbe();  // Return the height above the bed.  Returned value is negative if probing isn't implemented
   void ZProbe(float h); // Move to height h above the bed using the probe (if there is one).  h should be non-negative.
@@ -255,7 +258,7 @@ class Platform
   char lowStopPins[DRIVES];
   char highStopPins[DRIVES];
   float maxFeedrates[DRIVES];  
-  float maxAccelerations[DRIVES];
+  float accelerations[DRIVES];
   float driveStepsPerUnit[DRIVES];
   float jerks[DRIVES];
 
@@ -304,11 +307,6 @@ inline unsigned long Platform::Time()
   return micros();
 }
 
-inline float Platform::DriveStepsPerUnit(char drive)
-{
-  return driveStepsPerUnit[drive]; 
-}
-
 inline void Platform::Exit()
 {
   active = false;
@@ -346,6 +344,21 @@ inline char* Platform::GetTempDir()
 
 // Drive the RepRap machine
 
+inline float Platform::DriveStepsPerUnit(char drive)
+{
+  return driveStepsPerUnit[drive]; 
+}
+
+inline float Platform::Acceleration(char drive)
+{
+  return accelerations[drive]; 
+}
+
+inline float Platform::Jerk(char drive)
+{
+  return jerks[drive]; 
+}
+
 inline void Platform::SetDirection(byte drive, bool direction)
 {
   digitalWrite(directionPins[drive], direction);  

Platform.ino

@@ -26,7 +26,8 @@ Licence: GPL
 
 void setup()
 {
-  reprap.Init();  
+  reprap.Init();
+  //reprap.InterruptTime();  // Uncomment this line to time the interrupt routine on startup
 }
   
 void loop()
@@ -99,7 +100,7 @@ void Platform::Init()
     lowStopPins = LOW_STOP_PINS;
     highStopPins = HIGH_STOP_PINS;
     maxFeedrates = MAX_FEEDRATES;
-    maxAccelerations = MAX_ACCELERATIONS;
+    accelerations = ACCELERATIONS;
     driveStepsPerUnit = DRIVE_STEPS_PER_UNIT;
     jerks = JERKS;
     

RepRapFirmware.h

@@ -41,6 +41,9 @@ boolean StringStartsWith(char* string, char* starting);
 boolean StringEquals(char* s1, char* s2);
 int StringContains(char* string, char* match);
 
+//#include <stdio.h>
+//#include <stdlib.h>
+
 #include "Configuration.h"
 #include "Platform.h"
 #include "Webserver.h" 

RepRapFirmware.ino

@@ -100,6 +100,24 @@ void RepRap::Spin()
   heat->Spin();
 }
 
+void RepRap::InterruptTime()
+{
+  char buffer[50];
+  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);
+  dda->Start(false);
+  unsigned long t = platform->Time();
+  for(long i = 0; i < 100000; i++) 
+    dda->Step(false);
+  t = platform->Time() - t;
+  platform->Message(HOST_MESSAGE, "Time for 100000 calls of the interrupt function: ");
+  sprintf(buffer, "%ld", t);
+  platform->Message(HOST_MESSAGE, buffer);
+  platform->Message(HOST_MESSAGE, " microseconds.\n");
+}
+
 
 //*************************************************************************************************
 

Reprap.h

@@ -30,6 +30,7 @@ class RepRap
     void Spin();
     void Exit();
     void Interrupt();
+    void InterruptTime();
     boolean debug();
     void debug(boolean d);
     Platform* GetPlatform();