pcomplete, fully untested, support for accelleration management with look ahead optimization, finally

Makefile

@@ -31,7 +31,7 @@ DEVICE     = atmega328p
 CLOCK      = 16000000
 PROGRAMMER = -c avrisp2 -P usb
 OBJECTS    = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o \
-             eeprom.o config.o motion_plan.o
+             eeprom.o config.o stepper_plan.o
 # FUSES      = -U hfuse:w:0xd9:m -U lfuse:w:0x24:m
 FUSES      = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m
 

config.c

@@ -26,6 +26,8 @@
 #include "wiring_serial.h"
 #include <avr/pgmspace.h>
 
+struct Settings settings;
+
 void reset_settings() {
   settings.steps_per_mm[0] = X_STEPS_PER_MM;
   settings.steps_per_mm[1] = Y_STEPS_PER_MM;
@@ -36,6 +38,7 @@ void reset_settings() {
   settings.acceleration = DEFAULT_ACCELERATION;
   settings.mm_per_arc_segment = MM_PER_ARC_SEGMENT;
   settings.invert_mask = STEPPING_INVERT_MASK;
+  settings.max_jerk = DEFAULT_MAX_JERK;
 }
 
 void dump_settings() {
@@ -50,7 +53,8 @@ void dump_settings() {
   printPgmString(PSTR(" (mm/arc segment)\r\n$8 = ")); printInteger(settings.invert_mask); 
   printPgmString(PSTR(" (step port invert mask. binary = ")); printIntegerInBase(settings.invert_mask, 2);  
   printPgmString(PSTR(")\r\n$9 = ")); printFloat(settings.acceleration);
-  printPgmString(PSTR(" (acceleration in mm/min^2)"));
+  printPgmString(PSTR(" (acceleration in mm/min^2)\r\n$10 = ")); printFloat(settings.max_jerk);
+  printPgmString(PSTR(" (max instant cornering speed change in delta mm/min)"));
   printPgmString(PSTR("\r\n'$x=value' to set parameter or just '$' to dump current settings\r\n"));
 }
 

config.h

@@ -72,8 +72,9 @@ struct Settings {
   uint8_t invert_mask;
   double mm_per_arc_segment;
   double acceleration;
+  double max_jerk;
 };
-struct Settings settings;
+extern struct Settings settings;
 
 // Initialize the configuration subsystem (load settings from EEPROM)
 void config_init();
@@ -97,6 +98,7 @@ void store_setting(int parameter, double value);
 #define RAPID_FEEDRATE 480.0 // in millimeters per minute
 #define DEFAULT_FEEDRATE 480.0
 #define DEFAULT_ACCELERATION (DEFAULT_FEEDRATE/100.0)
+#define DEFAULT_MAX_JERK 50.0
 
 // Use this line for default operation (step-pulses high)
 #define STEPPING_INVERT_MASK 0

main.c

@@ -21,7 +21,7 @@
 #include <avr/io.h>
 #include <avr/sleep.h>
 #include <util/delay.h>
-#include "motion_plan.h"
+#include "stepper_plan.h"
 #include "stepper.h"
 #include "spindle_control.h"
 #include "motion_control.h"
@@ -36,7 +36,7 @@ int main(void)
   beginSerial(BAUD_RATE);
   printString("A");
   config_init();
-  mp_init(); // initialize the motion plan subsystem
+  plan_init(); // initialize the stepper plan subsystem
   st_init(); // initialize the stepper subsystem
   mc_init(); // initialize motion control subsystem
   spindle_init(); // initialize spindle controller

readme.txt

@@ -20,6 +20,7 @@ Prioritized to-do:
 * Documentation and web-site
 * Support for a alphanumeric LCD readout, a joystick and a few buttons for program control
 * Support "headless" fabrication by buffering all code to SD-card or similar
+* Backlash compensation
 
 Limitations by design:
 * Limited GCode-support. Focus on the kind of GCode produced by CAM tools. Leave human GCoders frustrated.

stepper.c

@@ -29,7 +29,7 @@
 #include <util/delay.h>
 #include "nuts_bolts.h"
 #include <avr/interrupt.h>
-#include "motion_plan.h"
+#include "stepper_plan.h"
 #include "wiring_serial.h"
 
 void set_step_events_per_minute(uint32_t steps_per_minute);
@@ -120,7 +120,7 @@ inline void trapezoid_generator_tick() {
 // steps. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
 void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters) {
-  mp_buffer_line(steps_x, steps_y, steps_z, microseconds, millimeters);
+  plan_buffer_line(steps_x, steps_y, steps_z, microseconds, millimeters);
   // Ensure that block processing is running by enabling The Stepper Driver Interrupt
   ENABLE_STEPPER_DRIVER_INTERRUPT();
 }

stepper_plan.c

@@ -1,5 +1,5 @@
 /*
-  motion_plan.c - buffers movement commands and manages the acceleration profile plan
+  stepper_plan.c - buffers movement commands and manages the acceleration profile plan
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
@@ -22,7 +22,7 @@
 #include <math.h>       
 #include <stdlib.h>
 
-#include "motion_plan.h"
+#include "stepper_plan.h"
 #include "nuts_bolts.h"
 #include "stepper.h"
 #include "config.h"
@@ -43,27 +43,6 @@ inline uint32_t estimate_acceleration_ticks(int32_t start_rate, int32_t accelera
       acceleration_per_tick));
 }
 
-void mp_enable_acceleration_management() {
-  if (!acceleration_management) {
-    st_synchronize();
-    acceleration_management = TRUE;
-  }
-}
-
-void mp_disable_acceleration_management() {
-  if(acceleration_management) {
-    st_synchronize();
-    acceleration_management = FALSE;
-  }
-}
-
-void mp_init() {
-  block_buffer_head = 0;
-  block_buffer_tail = 0;
-  mp_enable_acceleration_management();
-}
-
-
 // Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
 // In practice both factors must be in the range 0 ... 1.0
 void calculate_trapezoid_for_block(struct Block *block, double entry_factor, double exit_factor) {
@@ -92,10 +71,92 @@ void calculate_trapezoid_for_block(struct Block *block, double entry_factor, dou
   }
 }
 
+inline double estimate_max_speed(double max_acceleration, double target_velocity, double distance) {
+  return(sqrt(-2*max_acceleration*distance+target_velocity*target_velocity));
+}
+
+inline double estimate_jerk(struct Block *before, struct Block *after) {
+  return(max(fabs(before->speed_x-after->speed_x),
+    max(fabs(before->speed_y-after->speed_y), 
+    fabs(before->speed_z-after->speed_z))));
+}
+
+// Builds plan for a single block provided. Returns TRUE if changes were made to this block
+// that requires any earlier blocks to be recalculated too.
+int8_t build_plan_for_single_block(struct Block *previous, struct Block *current, struct Block *next) {
+  if(!current){return(TRUE);}
+  double exit_factor;
+  double entry_factor = 1.0;
+  if (next) {
+    exit_factor = next->entry_factor;
+  } else {
+    exit_factor = 0.0;
+  }
+  
+  if (previous) {
+    double jerk = estimate_jerk(previous, current);
+    if (jerk > settings.max_jerk) {
+      entry_factor = (settings.max_jerk/jerk);
+    } 
+    if (exit_factor<entry_factor) {
+      double max_entry_speed = estimate_max_speed(-settings.acceleration,current->nominal_speed*exit_factor, 
+        current->millimeters);
+      double max_entry_factor = max_entry_speed/current->nominal_speed;
+      if (max_entry_factor < entry_factor) {
+        entry_factor = max_entry_factor;
+      }
+    }    
+  } else {
+    entry_factor = 0.0;
+  }
+  // Check if we made a difference for this block. If we didn't, the planner can call it quits
+  // here. No need to process any earlier blocks.
+  int8_t keep_going = (entry_factor > current->entry_factor ? TRUE : FALSE);  
+  // Store result and recalculate trapezoid parameters
+  current->entry_factor = entry_factor;
+  calculate_trapezoid_for_block(current, entry_factor, exit_factor);
+  return(keep_going);
+}
+
+void recalculate_plan() {
+  int8_t block_index = block_buffer_head;
+  struct Block *block[3] = {NULL, NULL, NULL};
+  while(block_index != block_buffer_tail) {
+    block[2]= block[1];
+    block[1]= block[0];
+    block[0] = &block_buffer[block_index];
+    if (!build_plan_for_single_block(block[0], block[1], block[2])) {return;}
+    block_index = (block_index-1) % BLOCK_BUFFER_SIZE;
+  }
+  if (block[1]) {
+    calculate_trapezoid_for_block(block[0], block[0]->entry_factor, block[1]->entry_factor);
+  }
+}
+
+void plan_enable_acceleration_management() {
+  if (!acceleration_management) {
+    st_synchronize();
+    acceleration_management = TRUE;
+  }
+}
+
+void plan_disable_acceleration_management() {
+  if(acceleration_management) {
+    st_synchronize();
+    acceleration_management = FALSE;
+  }
+}
+
+void plan_init() {
+  block_buffer_head = 0;
+  block_buffer_tail = 0;
+  plan_enable_acceleration_management();
+}
+
 // Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in 
 // steps. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
-void mp_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters) {
+void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters) {
   // Calculate the buffer head after we push this byte
 	int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE;	
 	// If the buffer is full: good! That means we are well ahead of the robot. 
@@ -115,7 +176,8 @@ void mp_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t
   block->speed_x = block->steps_x*multiplier/settings.steps_per_mm[0];
   block->speed_y = block->steps_y*multiplier/settings.steps_per_mm[1];
   block->speed_z = block->steps_z*multiplier/settings.steps_per_mm[2];
-  block->nominal_rate = round(block->step_event_count*multiplier);
+  block->nominal_speed = millimeters*multiplier;
+  block->nominal_rate = round(block->step_event_count*multiplier);  
   
   // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line
   // average travel per step event changes. For a line along one axis the travel per step event

stepper_plan.h

@@ -1,5 +1,5 @@
 /*
-  motion_plan.h - buffers movement commands and manages the acceleration profile plan
+  stepper_plan.h - buffers movement commands and manages the acceleration profile plan
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
@@ -18,8 +18,8 @@
   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-#ifndef motion_plan_h
+#ifndef stepper_plan_h
-#define motion_plan_h
+#define stepper_plan_h
                  
 #include <inttypes.h>
 
@@ -27,19 +27,23 @@
 #ifdef __AVR_ATmega328P__
 #define BLOCK_BUFFER_SIZE 20   // Atmega 328 has one full kilobyte of extra RAM!
 #else
-#define BLOCK_BUFFER_SIZE 5
+#define BLOCK_BUFFER_SIZE 10
 #endif
 
-// This struct is used when buffering the setup for each linear movement
+// This struct is used when buffering the setup for each linear movement "nominal" values are as specified in 
-// "nominal" values are as specified in the source g-code and may never
+// the source g-code and may never actually be reached if acceleration management is active.
-// actually be reached if acceleration management is active.
 struct Block {
+  // Fields used by the bresenham algorithm for tracing the line
   uint32_t steps_x, steps_y, steps_z; // Step count along each axis
   uint8_t  direction_bits;            // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
   int32_t  step_event_count;          // The number of step events required to complete this block
   uint32_t nominal_rate;              // The nominal step rate for this block in step_events/minute
-  // Values used for acceleration management
+  
-  double  speed_x, speed_y, speed_z;   // Nominal mm/minute for each axis
+  // Fields used by the motion planner to manage acceleration
+  double  speed_x, speed_y, speed_z;  // Nominal mm/minute for each axis
+  double nominal_speed;               // The nominal speed for this block in mm/min
+  double millimeters;
+  double entry_factor;                // The factors representing the change in speed at the start of the trapezoid
   uint32_t initial_rate;              // The jerk-adjusted step rate at start of block
   int16_t rate_delta;                 // The steps/minute to add or subtract when changing speed (must be positive)
   uint16_t accelerate_ticks;          // The number of acceleration-ticks to accelerate 
@@ -51,19 +55,18 @@ extern volatile int block_buffer_head;           // Index of the next block to b
 extern volatile int block_buffer_tail;           // Index of the block to process now
 
 // Initialize the motion plan subsystem      
-void mp_init();
+void plan_init();
 
+// Do not call directly unless you are writing a motor driver. In current iteration this is called by 
+// st_buffer_line which also wakes up the stepper subsystem.
 // Add a new linear movement to the buffer. steps_x, _y and _z is the signed, relative motion in 
 // steps. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
-// Do not call directly unless you are writing a motor driver. In current iteration this is called by 
+void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters);
-// st_buffer_line which also wakes up the stepper subsystem.
-void mp_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters);
 
 // Enables acceleration-management for upcoming blocks
-void mp_enable_acceleration_management();
+void plan_enable_acceleration_management();
 // Disables acceleration-management for upcoming blocks
-void mp_disable_acceleration_management();
+void plan_disable_acceleration_management();
-
 
 #endif