Make lookahead basically working.

This means, modify existing code to let the lookahead algorithms
do their work. It also means to remove some unused code in
dda_lookahead.c and reordering some code to make it work with
LOOKAHEAD undefined.

dda.c

@@ -10,6 +10,7 @@
 #include	<avr/interrupt.h>
 
 #include	"dda_maths.h"
+#include	"dda_lookahead.h"
 #include	"timer.h"
 #include	"serial.h"
 #include	"sermsg.h"
@@ -82,9 +83,13 @@ void dda_new_startpoint(void) {
 
 	This algorithm is probably the main limiting factor to print speed in terms of firmware limitations
 */
-void dda_create(DDA *dda, TARGET *target) {
+void dda_create(DDA *dda, TARGET *target, DDA *prev_dda) {
 	uint32_t	steps, x_delta_um, y_delta_um, z_delta_um, e_delta_um;
 	uint32_t	distance, c_limit, c_limit_calc;
+  #ifdef LOOKAHEAD
+  // Number the moves to identify them; allowed to overflow.
+  static uint8_t idcnt = 0;
+  #endif
 
 	// initialise DDA to a known state
 	dda->allflags = 0;
@@ -95,6 +100,15 @@ void dda_create(DDA *dda, TARGET *target) {
 	// we end at the passed target
 	memcpy(&(dda->endpoint), target, sizeof(TARGET));
 
+  #ifdef LOOKAHEAD
+  // Set the start and stop speeds to zero for now = full stops between
+  // moves. Also fallback if lookahead calculations fail to finish in time.
+  dda->F_start = 0;
+  dda->F_end = 0;
+  // Give this move an identifier.
+  dda->id = idcnt++;
+  #endif
+
 // TODO TODO: We should really make up a loop for all axes.
 //            Think of what happens when a sixth axis (multi colour extruder)
 //            appears?
@@ -129,6 +143,14 @@ void dda_create(DDA *dda, TARGET *target) {
 		dda->e_direction = (target->E >= startpoint.E)?1:0;
 	}
 
+  #ifdef LOOKAHEAD
+  // Also displacements in micrometers, but for the lookahead alogrithms.
+  dda->delta.X = target->X - startpoint.X;
+  dda->delta.Y = target->Y - startpoint.Y;
+  dda->delta.Z = target->Z - startpoint.Z;
+  dda->delta.E = target->e_relative ? target->E : target->E - startpoint.E;
+  #endif
+
 	if (DEBUG_DDA && (debug_flags & DEBUG_DDA))
 		sersendf_P(PSTR("%ld,%ld,%ld,%ld] ["), target->X - startpoint.X, target->Y - startpoint.Y, target->Z - startpoint.Z, target->E - startpoint.E);
 
@@ -285,13 +307,43 @@ void dda_create(DDA *dda, TARGET *target) {
 				dda->c_min = c_limit;
 // This section is plain wrong, like in it's only half of what we need. This factor 960000 is dependant on STEPS_PER_MM.
 			// overflows at target->F > 65535; factor 16. found by try-and-error; will overshoot target speed a bit
-			dda->rampup_steps = target->F * target->F / (uint32_t)(STEPS_PER_M_X * ACCELERATION / 960000.);
+      //dda->rampup_steps = target->F * target->F / (uint32_t)(STEPS_PER_M_X * ACCELERATION / 960000.);
 //sersendf_P(PSTR("rampup calc %lu\n"), dda->rampup_steps);
-			dda->rampup_steps = 100000; // replace mis-calculation by a safe value
+			//dda->rampup_steps = 100000; // replace mis-calculation by a safe value
 // End of wrong section.
-			if (dda->rampup_steps > dda->total_steps / 2)
+      /**
-				dda->rampup_steps = dda->total_steps / 2;
+        Assuming: F is in mm/min, STEPS_PER_M_X is in steps/m, ACCELERATION is in mm/s²
-			dda->rampdown_steps = dda->total_steps - dda->rampup_steps;
+        Given:
+         - Velocity v at time t given acceleration a: v(t) = a*t
+         - Displacement s at time t given acceleration a: s(t) = 1/2 * a * t²
+         - Displacement until reaching target velocity v: s = 1/2 * (v² / a)
+         - Final result: steps needed to reach velocity v given acceleration a:
+         steps = (STEPS_PER_M_X * F^2) / (7200000 * ACCELERATION)
+         To keep precision, break up in floating point and integer part:
+           F^2 * (int)(STEPS_PER_M_X / (7200000 * ACCELERATION))
+         Note: the floating point part is static so its calculated during compilation.
+         Note 2: the floating point part will be smaller than one, invert it:
+                   steps = F^2 / (int)((7200000 * ACCELERATION) / STEPS_PER_M_X)
+         Note 3: As mentioned, setting F to 65535 or larger will overflow the
+                 calculation. Make sure this does not happen.
+         Note 4: Anyone trying to run their machine at 65535 mm/min > 1m/s is nuts
+       */
+      if (target->F > 65534) target->F = 65534;
+      dda->rampup_steps = ACCELERATE_RAMP_LEN(target->F);
+      // Quick hack: we do not do Z move joins as jerk on the Z axis is undesirable;
+      // as the ramp length is calculated for XY, its incorrect for Z: apply the original
+      // 'fix' to simply specify a large enough ramp for any speed.
+      if (x_delta_um == 0 && y_delta_um == 0) {
+        dda->rampup_steps = 100000; // replace mis-calculation by a safe value
+      }
+
+      if (dda->rampup_steps > dda->total_steps / 2)
+        dda->rampup_steps = dda->total_steps / 2;
+      dda->rampdown_steps = dda->total_steps - dda->rampup_steps;
+
+      #ifdef LOOKAHEAD
+        dda_join_moves(prev_dda, dda);
+      #endif
 		#elif defined ACCELERATION_TEMPORAL
 			// TODO: limit speed of individual axes to MAXIMUM_FEEDRATE
 			// TODO: calculate acceleration/deceleration for each axis
@@ -617,6 +669,8 @@ void dda_step(DDA *dda) {
 			move_state.n += 4;
 			// be careful of signedness!
 			move_state.c = (int32_t)move_state.c - ((int32_t)(move_state.c * 2) / (int32_t)move_state.n);
+      //sersendf_P(PSTR("n:%ld; c:%ld; steps: %ld / %lu\n"), move_state.n,
+      //           move_state.c, move_state.step_no, move_state.y_steps);
 		}
 		move_state.step_no++;
 // Print the number of steps actually needed for ramping up
@@ -687,6 +741,11 @@ void dda_step(DDA *dda) {
 	if (move_state.x_steps == 0 && move_state.y_steps == 0 &&
 	    move_state.z_steps == 0 && move_state.e_steps == 0) {
 		dda->live = 0;
+    #ifdef LOOKAHEAD
+    // If look-ahead was using this move, it could have missed our activation:
+    // make sure the ids do not match.
+    dda->id--;
+    #endif
 		#ifdef	DC_EXTRUDER
 			heater_set(DC_EXTRUDER, 0);
 		#endif

dda.h

@@ -15,6 +15,9 @@
 	types
 */
 
+// Enum to denote an axis
+enum axis_e { X, Y, Z, E };
+
 /**
 	\struct TARGET
 	\brief target is simply a point in space/time
@@ -34,6 +37,19 @@ typedef struct {
 	uint8_t		e_relative				:1; ///< bool: e axis relative? Overrides all_relative
 } TARGET;
 
+/**
+ \struct VECTOR4D
+ \brief 4 dimensional vector used to describe the difference between moves.
+
+  Units are in micrometers and usually based off 'TARGET'.
+*/
+typedef struct {
+  int32_t X;
+  int32_t Y;
+  int32_t Z;
+  int32_t E;
+} VECTOR4D;
+
 /**
 	\struct MOVE_STATE
 	\brief this struct is made for tracking the current state of the movement
@@ -128,6 +144,21 @@ typedef struct {
 	uint32_t					rampdown_steps;
 	/// 24.8 fixed point timer value, maximum speed
 	uint32_t					c_min;
+  #ifdef LOOKAHEAD
+  // With the look-ahead functionality, it is possible to retain physical
+  // movement between G1 moves. These variables keep track of the entry and
+  // exit speeds between moves.
+  uint32_t          F_start;
+  uint32_t          F_end;
+  // Displacement vector, in um, based between the difference of the starting
+  // point and the target. Required to obtain the jerk between 2 moves.
+  // Note: x_delta and co are in steps, not um.
+  VECTOR4D          delta;
+  // Number the moves to be able to test at the end of lookahead if the moves
+  // are the same. Note: we do not need a lot of granularity here: more than
+  // MOVEBUFFER_SIZE is already enough.
+  uint8_t           id;
+  #endif
 	#endif
 	#ifdef ACCELERATION_TEMPORAL
 	uint32_t					x_step_interval; ///< time between steps on X axis
@@ -166,7 +197,7 @@ void dda_init(void);
 void dda_new_startpoint(void);
 
 // create a DDA
-void dda_create(DDA *dda, TARGET *target);
+void dda_create(DDA *dda, TARGET *target, DDA *prev_dda);
 
 // start a created DDA (called from timer interrupt)
 void dda_start(DDA *dda)																						__attribute__ ((hot));

dda_lookahead.c

@@ -25,7 +25,6 @@
 #include "debug.h"
 #include "sersendf.h"
 #include "pinio.h"
-#include "config.h"
 
 extern uint8_t use_lookahead;
 
@@ -144,7 +143,7 @@ void dda_emergency_shutdown(PGM_P msg) {
   serial_writestr_P(PSTR("error: emergency stop:"));
   if(msg!=NULL) serial_writestr_P(msg);
   serial_writestr_P(PSTR("\r\n"));
-  delay(20000);   // Delay so the buffer can be flushed - otherwise the message is never sent
+  delay_ms(20);   // Delay so the buffer can be flushed - otherwise the message is never sent
   timer_stop();
   queue_flush();
   power_off();
@@ -168,8 +167,6 @@ void dda_emergency_shutdown(PGM_P msg) {
  * last move (= 'current'); as a result a lot of small moves will still limit the speed.
  */
 void dda_join_moves(DDA *prev, DDA *current) {
-  // Run-time option: only proceed if we are enabled.
-  if(use_lookahead==0) return;
 
   // Calculating the look-ahead settings can take a while; before modifying
   // the previous move, we need to locally store any values and write them
@@ -181,7 +178,6 @@ void dda_join_moves(DDA *prev, DDA *current) {
   // Note: we assume 'current' will not be dispatched while this function runs, so we do not to
   // back up the move settings: they will remain constant.
   uint32_t this_F_start, this_rampup, this_rampdown;
-  enum axis_e prev_lead;
   int32_t jerk, jerk_e;       // Expresses the forces if we would change directions at full speed
   static uint32_t la_cnt = 0;     // Counter: how many moves did we join?
   #ifdef LOOKAHEAD_DEBUG
@@ -224,7 +220,6 @@ void dda_join_moves(DDA *prev, DDA *current) {
       prev_rampup = prev->rampup_steps;
       prev_rampdown = prev->rampdown_steps;
       prev_total_steps = prev->total_steps;
-      prev_lead = prev->lead;
     }
 
     // The initial crossing speed is the minimum between both target speeds

dda_lookahead.h

@@ -27,8 +27,6 @@
 #error "Look-ahead requires steps per m to be identical on the X and Y axis (for now)"
 #endif
 
-// Note: the floating point bit is optimized away during compilation
-#define ACCELERATE_RAMP_LEN(speed) (((speed)*(speed)) / (uint32_t)((7200000.0f * ACCELERATION) / (float)STEPS_PER_M_X))
 // This is the same to ACCELERATE_RAMP_LEN but now the steps per m can be switched.
 // Note: use this with a macro so the float is removed by the preprocessor
 #define ACCELERATE_RAMP_SCALER(spm) (uint32_t)((7200000.0f * ACCELERATION) / (float)spm)

dda_maths.h

@@ -63,4 +63,7 @@ uint16_t int_sqrt(uint32_t a);
 // 2 ^ msbloc(v) >= v
 const uint8_t msbloc (uint32_t v);
 
+// Note: the floating point bit is optimized away during compilation
+#define ACCELERATE_RAMP_LEN(speed) (((speed)*(speed)) / (uint32_t)((7200000.0f * ACCELERATION) / (float)STEPS_PER_M_X))
+
 #endif	/* _DDA_MATHS_H */

dda_queue.c

@@ -100,9 +100,10 @@ void enqueue_home(TARGET *t, uint8_t endstop_check, uint8_t endstop_stop_cond) {
 	h &= (MOVEBUFFER_SIZE - 1);
 
 	DDA* new_movebuffer = &(movebuffer[h]);
-	
+  DDA* prev_movebuffer = (queue_empty() != 0) ? NULL : &movebuffer[mb_head];
-	if (t != NULL) {
+
-		dda_create(new_movebuffer, t);
+  if (t != NULL) {
+    dda_create(new_movebuffer, t, prev_movebuffer);
 		new_movebuffer->endstop_check = endstop_check;
 		new_movebuffer->endstop_stop_cond = endstop_stop_cond;
 	}