still not running, but a lot further along

config.c

@@ -49,11 +49,10 @@ void dump_settings() {
   printPgmString(PSTR(" (microseconds step pulse)\r\n$4 = ")); printFloat(settings.default_feed_rate);
   printPgmString(PSTR(" (mm/min default feed rate)\r\n$5 = ")); printFloat(settings.default_seek_rate);
   printPgmString(PSTR(" (mm/min default seek rate)\r\n$6 = ")); printFloat(settings.mm_per_arc_segment);
-  printPgmString(PSTR(" (mm/min^2 max acceleration)\r\n$7 = ")); printFloat(settings.acceleration);
+  printPgmString(PSTR(" (mm/arc segment)\r\n$7 = ")); printInteger(settings.invert_mask); 
-  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(")\r\n$8 = ")); printFloat(settings.acceleration);
-  printPgmString(PSTR(" (acceleration in mm/min^2)\r\n$10 = ")); printFloat(settings.max_jerk);
+  printPgmString(PSTR(" (acceleration in mm/sec^2)\r\n$9 = ")); 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"));
 }
@@ -83,9 +82,9 @@ void store_setting(int parameter, double value) {
     case 4: settings.default_feed_rate = value; break;
     case 5: settings.default_seek_rate = value; break;
     case 6: settings.mm_per_arc_segment = value; break;
-    case 7: settings.acceleration = value; break;
+    case 7: settings.invert_mask = trunc(value); break;
-    case 8: settings.invert_mask = trunc(value); break;
+    case 8: settings.acceleration = value; break;
-    case 9: settings.acceleration = fabs(value); break;
+    case 9: settings.max_jerk = fabs(value); break;
     default: 
       printPgmString(PSTR("Unknown parameter\r\n"));
       return;

config.h

@@ -21,7 +21,7 @@
 #ifndef config_h
 #define config_h
 
-#define VERSION "0.51"
+#define VERSION "0.6b"
 #include <math.h>
 #include <inttypes.h>
 
@@ -110,7 +110,7 @@ void store_setting(int parameter, double value);
 // #define STEPPING_INVERT_MASK (STEP_MASK | (1<<X_DIRECTION_BIT) | (1<<Y_DIRECTION_BIT))
 
 // The temporal resolution of the acceleration management subsystem
-#define ACCELERATION_TICKS_PER_SECOND 10                           
+#define ACCELERATION_TICKS_PER_SECOND 10L
 
 // Some useful constants
 #define STEP_MASK ((1<<X_STEP_BIT)|(1<<Y_STEP_BIT)|(1<<Z_STEP_BIT)) // All step bits

main.c

@@ -33,15 +33,13 @@
 
 int main(void)
 {
-  beginSerial(BAUD_RATE);
+  sp_init(); // initialize the serial protocol
-  printString("A");
   config_init();
   plan_init(); // initialize the stepper plan subsystem
   st_init(); // initialize the stepper subsystem
   mc_init(); // initialize motion control subsystem
   spindle_init(); // initialize spindle controller
   gc_init(); // initialize gcode-parser
-  sp_init(); // initialize the serial protocol
   
   DDRD |= (1<<3)|(1<<4)|(1<<5);
   

script/console

@@ -1,4 +1,4 @@
-socat -d -d READLINE /dev/tty.usbserial-A700e0GO,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1
+socat -d -d READLINE /dev/tty.usbmodem24121,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1
 socat -d -d READLINE /dev/tty.usbserial-A9007QcR,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1
 
 #socat -d -d READLINE /dev/tty.FireFly-A964-SPP-1,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1

serial_protocol.c

@@ -38,7 +38,6 @@ void prompt() {
 void sp_init() 
 {
   beginSerial(BAUD_RATE);  
-  
   printPgmString(PSTR("\r\nGrbl "));
   printPgmString(PSTR(VERSION));
   printPgmString(PSTR("\r\n"));  

stepper.c

@@ -36,7 +36,7 @@ void set_step_events_per_minute(uint32_t steps_per_minute);
 
 #define ENABLE_STEPPER_DRIVER_INTERRUPT()  TIMSK1 |= (1<<OCIE1A)
 #define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
-
+#define MINIMUM_STEPS_PER_MINUTE 1200
 #define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND)
 
 struct Block *current_block;    // A convenience pointer to the block currently being traced
@@ -46,12 +46,16 @@ uint8_t out_bits;               // The next stepping-bits to be output
 int32_t counter_x, 
         counter_y, 
         counter_z;              // counter variables for the bresenham line tracer
-uint32_t iterations;            // The number of iterations left to complete the current_block
+uint32_t step_events_left;      // The number of step events left to complete the current_block
+uint32_t step_event_count;      // The count of step events executed in the current block
 volatile int busy;              // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
-uint32_t cycles_per_step_event;
-uint32_t trapezoid_tick_cycle_counter;
 
-// Values and variables used by the speed trapeziod generator  
+uint32_t cycles_per_step_event;        // The number of machine cycles between each step event
+uint32_t trapezoid_tick_cycle_counter; // The cycles since last trapezoid_tick used to generate ticks without 
+                                       // allocating a separate timer
+uint32_t trapezoid_rate;               // The current rate of step_events according to the trapezoid generator
+
+// Two trapezoids:
 //         __________________________
 //        /|                        |\     _________________         ^
 //       / |                        | \   /|               |\        |
@@ -63,27 +67,17 @@ uint32_t trapezoid_tick_cycle_counter;
 //
 //                           time ----->
 // 
-//  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates for 
+//  The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates until
+//   
 //  block->accelerate_ticks by block->rate_delta each tick, then stays up for block->plateau_ticks and 
 //  decelerates for the rest of the block until the trapezoid generator is reset for the next block. 
 //  The slope of acceleration is always +/- block->rate_delta. Any stage may be skipped by setting the 
 //  duration to 0 ticks. 
      
-#define TRAPEZOID_STAGE_ACCELERATING 0
-#define TRAPEZOID_STAGE_PLATEAU 1
-#define TRAPEZOID_STAGE_DECELERATING 2
-uint8_t trapezoid_stage;
-uint16_t trapezoid_stage_ticks;
-uint32_t trapezoid_rate;
-int16_t trapezoid_delta;
-
 
 // Initializes the trapezoid generator from the current block. Called whenever a new 
 // block begins.
 inline void reset_trapezoid_generator() {        
-  trapezoid_stage = TRAPEZOID_STAGE_ACCELERATING;
-  trapezoid_stage_ticks = current_block->accelerate_ticks;
-  trapezoid_delta = current_block->rate_delta;
   trapezoid_rate = current_block->initial_rate;  
   set_step_events_per_minute(trapezoid_rate);
 }
@@ -92,37 +86,31 @@ inline void reset_trapezoid_generator() {
 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
 // current_block stays untouched by outside handlers for the duration of this function call.
 inline void trapezoid_generator_tick() {     
-  if (trapezoid_stage_ticks) {
+  PORTD ^= (1<<2); 
-    trapezoid_stage_ticks--;
+  if (current_block) {
-    if (trapezoid_delta) {
+    if (step_event_count < current_block->accelerate_until) {
-      trapezoid_rate += trapezoid_delta;
+      trapezoid_rate += current_block->rate_delta;
+      set_step_events_per_minute(trapezoid_rate);
+    } else if (step_event_count > current_block->decelerate_after) {
+      trapezoid_rate -= current_block->rate_delta;
       set_step_events_per_minute(trapezoid_rate);
-    }
     } else {
-    // Is there a block currently in execution?
+      printInteger(trapezoid_rate);
-    if(!current_block) {return;}    
+      while(1){};
-    // Trapezoid stage complete, move on
-    if(trapezoid_stage == TRAPEZOID_STAGE_ACCELERATING) {
-      // Progress to plateau stage
-      trapezoid_delta = 0;
-      trapezoid_stage_ticks = current_block->plateau_ticks;
-      trapezoid_stage = TRAPEZOID_STAGE_PLATEAU;
-    } else if (trapezoid_stage == TRAPEZOID_STAGE_PLATEAU) {
-      // Progress to deceleration stage
-      trapezoid_delta = -current_block->rate_delta;
-      trapezoid_stage_ticks = 0xffff; // "forever" until the block is complete
-      trapezoid_stage = TRAPEZOID_STAGE_DECELERATING;
     }
   }
+  PORTD ^= (1<<2);
 }
 
 // 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 st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds, double millimeters) {
+  PORTD ^= (1<<2);
   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();
+  PORTD ^= (1<<2);
 }
 
 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is  executed at the rate set with
@@ -135,7 +123,6 @@ SIGNAL(SIG_OUTPUT_COMPARE1A)
 #endif
 {
   if(busy){ return; } // The busy-flag is used to avoid reentering this interrupt
-  
   // Set the direction pins a cuple of nanoseconds before we step the steppers
   STEPPING_PORT = (STEPPING_PORT & ~DIRECTION_MASK) | (out_bits & DIRECTION_MASK);
   // Then pulse the stepping pins
@@ -159,7 +146,8 @@ SIGNAL(SIG_OUTPUT_COMPARE1A)
       counter_x = -(current_block->step_event_count >> 1);
       counter_y = counter_x;
       counter_z = counter_x;
-      iterations = current_block->step_event_count;
+      step_events_left = current_block->step_event_count;
+      step_event_count = 0;
     } else {
       DISABLE_STEPPER_DRIVER_INTERRUPT();
     }    
@@ -183,8 +171,8 @@ SIGNAL(SIG_OUTPUT_COMPARE1A)
       counter_z -= current_block->step_event_count;
     }
     // If current block is finished, reset pointer 
-    iterations -= 1;
+    step_events_left -= 1; step_event_count += 1;
-    if (iterations <= 0) {
+    if (step_events_left <= 0) {
       current_block = NULL;
       // move the block buffer tail to the next instruction
       block_buffer_tail = (block_buffer_tail + 1) % BLOCK_BUFFER_SIZE;      
@@ -242,11 +230,16 @@ void st_init()
   TCCR2B = (1<<CS21); // Full speed, 1/8 prescaler
   TIMSK2 |= (1<<TOIE2);      
   
+  set_step_events_per_minute(6000);
   DISABLE_STEPPER_DRIVER_INTERRUPT();  
+  trapezoid_tick_cycle_counter = 0;
   
   // set enable pin     
   STEPPERS_ENABLE_PORT |= 1<<STEPPERS_ENABLE_BIT;
   
+  DDRD |= (1<<2);
+  PORTD |= (1<<2);
+   
   sei();
 }
 
@@ -279,19 +272,19 @@ uint32_t config_step_timer(uint32_t cycles)
 	} else if (cycles <= 0x7ffffL) {
     ceiling = cycles >> 3;
     prescaler = 1; // prescaler: 8
-    actual_cycles = ceiling * 8;
+    actual_cycles = ceiling * 8L;
 	} else if (cycles <= 0x3fffffL) {
 		ceiling =  cycles >> 6;
     prescaler = 2; // prescaler: 64
-    actual_cycles = ceiling * 64;
+    actual_cycles = ceiling * 64L;
 	} else if (cycles <= 0xffffffL) {
 		ceiling =  (cycles >> 8);
     prescaler = 3; // prescaler: 256
-    actual_cycles = ceiling * 256;
+    actual_cycles = ceiling * 256L;
 	} else if (cycles <= 0x3ffffffL) {
 		ceiling = (cycles >> 10);
     prescaler = 4; // prescaler: 1024
-    actual_cycles = ceiling * 1024;    
+    actual_cycles = ceiling * 1024L;    
 	} else {
 	  // Okay, that was slower than we actually go. Just set the slowest speed
 		ceiling = 0xffff;
@@ -306,6 +299,7 @@ uint32_t config_step_timer(uint32_t cycles)
 }
 
 void set_step_events_per_minute(uint32_t steps_per_minute) {
+  if (steps_per_minute < MINIMUM_STEPS_PER_MINUTE) { steps_per_minute = MINIMUM_STEPS_PER_MINUTE; }
   cycles_per_step_event = config_step_timer((TICKS_PER_MICROSECOND*1000000*60)/steps_per_minute);
 }
 

stepper_plan.c

@@ -26,49 +26,72 @@
 #include "nuts_bolts.h"
 #include "stepper.h"
 #include "config.h"
+#include "wiring_serial.h"
 
 struct Block block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
 volatile int block_buffer_head;           // Index of the next block to be pushed
 volatile int block_buffer_tail;           // Index of the block to process now
 uint8_t acceleration_management;              // Acceleration management active?
 
-inline uint32_t estimate_acceleration_distance(int32_t current_rate, int32_t target_rate, int32_t acceleration) {
+
-  return((target_rate*target_rate-current_rate*current_rate)/(2*acceleration));
+
+// The distance it takes to accelerate from initial_rate to target_rate using the given acceleration
+inline double estimate_acceleration_distance(double initial_rate, double target_rate, double acceleration) {
+  return((target_rate*target_rate-initial_rate*initial_rate)/(2L*acceleration));
 }
 
-inline uint32_t estimate_acceleration_ticks(int32_t start_rate, int32_t acceleration_per_tick, int32_t step_events) {
+// This function gives you the point at which you must start braking (at the rate of -acceleration) if 
-  return(
+// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
-    round(
+// a total travel of distance. This can be used to compute the intersection point between acceleration and
-      (sqrt(2*acceleration_per_tick*step_events+(start_rate*start_rate))-start_rate)/
+// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
-      acceleration_per_tick));
+/*                          
+
+                          +    <- some rate that must be < maximum allowable rate
+                         /|\
+                        / | \
+                       /  |  + <- final_rate
+                      /   |  |
+     initial_rate -> +----+--+  
+                     0    ^  ^ 
+                          |  |
+                     result  distance
+*/                   
+inline double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance) {
+  return((2*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4*acceleration));
 }
 
+// See bottom of this module for a comment outlining the reasoning behind the mathematics behind the
+// preceding functions.
+
 // 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) {
   block->initial_rate = round(block->nominal_rate*entry_factor);
   int32_t final_rate = round(block->nominal_rate*entry_factor);
   int32_t acceleration_per_second = block->rate_delta*ACCELERATION_TICKS_PER_SECOND;
-  int32_t acceleration_steps = 
+  int32_t accelerate_steps = 
-    estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_second);
+    round(estimate_acceleration_distance(block->initial_rate, block->nominal_rate, acceleration_per_second));
-  int32_t decelleration_steps = 
+  int32_t decelerate_steps = 
     estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration_per_second);
+  printString("ir="); printInteger(block->initial_rate); printString("\n\r");
+  printString("nr="); printInteger(block->nominal_rate); printString("\n\r");
+  printString("rd="); printInteger(block->rate_delta); printString("\n\r");
+  printString("aps="); printInteger(acceleration_per_second); printString("\n\r");
+  printString("acs="); printInteger(accelerate_steps); printString("\n\r");
+  printString("dcs="); printInteger(decelerate_steps); printString("\n\r");
+  printString("ts="); printInteger(block->step_event_count); printString("\n\r");
   // Check if the acceleration and decelleration periods overlap. In that case nominal_speed will
   // never be reached but that's okay. Just truncate both periods proportionally so that they
   // fit within the allotted step events.
-  int32_t plateau_steps = block->step_event_count-acceleration_steps-decelleration_steps;
+  int32_t plateau_steps = block->step_event_count-accelerate_steps-decelerate_steps;
   if (plateau_steps < 0) {  
-    int32_t half_overlap_region = fabs(plateau_steps)/2;
+    accelerate_steps = round(
+      intersection_distance(block->initial_rate, final_rate, acceleration_per_second, block->step_event_count));
     plateau_steps = 0;
-    acceleration_steps = max(acceleration_steps-half_overlap_region,0);
+    printString("No plateau, so: acs="); printInteger(accelerate_steps); printString("\n\r");
-    decelleration_steps = max(decelleration_steps-half_overlap_region,0);
-  }
-  block->accelerate_ticks = estimate_acceleration_ticks(block->initial_rate, block->rate_delta, acceleration_steps);
-  if (plateau_steps) {
-    block->plateau_ticks = round(1.0*plateau_steps/(block->nominal_rate*ACCELERATION_TICKS_PER_SECOND));
-  } else {
-    block->plateau_ticks = 0;
   }  
+  block->accelerate_until = accelerate_steps;
+  block->decelerate_after = accelerate_steps+plateau_steps;
 }                    
 
 inline double estimate_max_speed(double max_acceleration, double target_velocity, double distance) {
@@ -185,15 +208,17 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
   // axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
   // To generate trapezoids with contant acceleration between blocks the rate_delta must be computed 
   // specifically for each line to compensate for this phenomenon:
-  double travel_per_step = (1.0*millimeters)/block->step_event_count;
+  double travel_per_step = millimeters/block->step_event_count;
+  printString("travel_per_step*10000=");
+  printInteger(travel_per_step*10000);printString("\n\r");
   block->rate_delta = round(
-    (settings.acceleration/(60.0*ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/min per acceleration_tick
+    ((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick
     travel_per_step);                                           // convert to: acceleration steps/min/acceleration_tick    
   if (acceleration_management) {
     calculate_trapezoid_for_block(block,0,0);                     // compute a conservative acceleration trapezoid for now
   } else {
-    block->accelerate_ticks = 0;
+    block->accelerate_until = 0;
-    block->plateau_ticks = 0;
+    block->decelerate_after = 0;
     block->rate_delta = 0;
   }
   
@@ -206,3 +231,35 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
   block_buffer_head = next_buffer_head;
 }
 
+
+/*  
+  Mathematica reasoning behind the mathematics in this module:
+  
+  s == speed, a == acceleration, t == time, d == distance
+
+  Basic definitions:
+
+    Speed[s_, a_, t_] := s + (a*t) 
+    Travel[s_, a_, t_] := Integrate[Speed[s, a, t], t]
+
+  Distance to reach a specific speed with a constant acceleration:
+
+    Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, d, t]
+      d -> (m^2 - s^2)/(2 a) --> estimate_acceleration_distance()
+
+  Speed after a given distance of travel with constant acceleration:
+
+    Solve[{Speed[s, a, t] == m, Travel[s, a, t] == d}, m, t]
+      m -> Sqrt[2 a d + s^2]    
+
+    DestinationSpeed[s_, a_, d_] := Sqrt[2 a d + s^2]
+
+  When to start braking (di) to reach a specified destionation speed (s2) after accelerating
+  from initial speed s1 without ever stopping at a plateau:
+
+    Solve[{DestinationSpeed[s1, a, di] == DestinationSpeed[s2, a, d - di]}, di]
+      di -> (2 a d - s1^2 + s2^2)/(4 a) --> intersection_distance()
+
+    IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
+*/
+                                                                                                            

stepper_plan.h

@@ -44,10 +44,12 @@ struct Block {
   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
+  
+  // Settings for the trapezoid generator
   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)
+  int32_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 
+  uint32_t accelerate_until;          // The index of the step event on which to stop acceleration
-  uint16_t plateau_ticks;             // The number of acceleration-ticks to maintain top speed
+  uint32_t decelerate_after;          // The index of the step event on which to start decelerating
 };
       
 extern struct Block block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions