rough accelleration stuff

acceleration.c

@@ -0,0 +1,62 @@
+/*
+  acceleration.c - support methods for acceleration-related calcul
+  Part of Grbl
+
+  Copyright (c) 2009 Simen Svale Skogsrud
+
+  Grbl is free software: you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation, either version 3 of the License, or
+  (at your option) any later version.
+
+  Grbl is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+// Estimate the maximum speed at a given distance when you need to reach the given 
+// target_velocity with max_accelleration.
+double estimate_max_speed(double max_accelleration, double target_velocity, double distance) {
+  return(sqrt(-2*max_accelleration*distance+target_velocity*target_velocity))
+}
+
+// At what distance must we start accellerating/braking to reach target_speed from current_speed given the 
+// specified constant accelleration.
+double estimate_brake_distance(double current_speed, double target_speed, double acceleration) {
+  return((target_speed*target_speed-current_speed*current_speed)/(2*acceleration));
+}
+
+// Calculate feed rate in length-units/second for a single axis
+double axis_feed_rate(double steps_per_stepping, uint32_t stepping_rate, double steps_per_unit) {
+  if (stepping_rate == 0) { return(0.0); }
+  return((TICKS_PER_MICROSECOND*1000000)*steps_per_stepping/(stepping_rate*steps_per_unit));
+}
+
+// The 'swerve' of a joint is equal to the maximum accelleration of any single
+// single axis in the corner between the outgoing and the incoming line. Accelleration control
+// will regulate speed to avoid excessive swerve.
+
+double calculate_swerve(struct Line* outgoing, struct Line* incoming) {  
+  double x_swerve = abs(
+    axis_feed_rate(
+      ((double)incoming->steps_x)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[X_AXIS])
+    - axis_feed_rate(
+      ((double)incoming->steps_x)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[X_AXIS]));
+  double y_swerve = abs(
+    axis_feed_rate(
+      ((double)incoming->steps_y)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[Y_AXIS])
+    - axis_feed_rate(
+      ((double)incoming->steps_y)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Y_AXIS]));
+  double z_swerve = abs(
+    axis_feed_rate(
+      ((double)incoming->steps_z)/incoming->maximum_steps, incoming->rate, settings.steps_per_mm[Z_AXIS])
+    - axis_feed_rate(
+      ((double)incoming->steps_z)/incoming->maximum_steps, outgoing-> rate, settings.steps_per_mm[Z_AXIS]));
+  return max(x_swerve, max(y_swerve, z_swerve));
+}
+

acceleration.h

@@ -0,0 +1,12 @@
+#ifndef acceleration_h
+#define acceleration_h
+
+// Estimate the maximum speed at a given distance when you need to reach the given 
+// target_velocity with max_accelleration.
+double estimate_max_speed(double max_accelleration, double target_velocity, double distance);
+
+// At what distance must we start accellerating/braking to reach target_speed from current_speed given the 
+// specified constant accelleration.
+double estimate_brake_distance(double current_speed, double target_speed, double acceleration);
+
+#endif

config.c

@@ -33,6 +33,8 @@ void reset_settings() {
   settings.pulse_microseconds = STEP_PULSE_MICROSECONDS;
   settings.default_feed_rate = DEFAULT_FEEDRATE;
   settings.default_seek_rate = RAPID_FEEDRATE;
+  settings.dead_feed_rate = DEFAULT_FEEDRATE/5;
+  settings.acceleration = DEFAULT_FEEDRATE/100;
   settings.mm_per_arc_segment = MM_PER_ARC_SEGMENT;
   settings.invert_mask = STEPPING_INVERT_MASK;
 }
@@ -44,8 +46,10 @@ void dump_settings() {
   printPgmString(PSTR(" (steps/mm z)\r\n$3 = ")); printInteger(settings.pulse_microseconds);
   printPgmString(PSTR(" (microseconds step pulse)\r\n$4 = ")); printFloat(settings.default_feed_rate);
   printPgmString(PSTR(" (mm/sec default feed rate)\r\n$5 = ")); printFloat(settings.default_seek_rate);
-  printPgmString(PSTR(" (mm/sec default seek rate)\r\n$6 = ")); printFloat(settings.mm_per_arc_segment);
+  printPgmString(PSTR(" (mm/sec default seek rate)\r\n$7 = ")); printFloat(settings.dead_feed_rate);
-  printPgmString(PSTR(" (mm/arc segment)\r\n$7 = ")); printInteger(settings.invert_mask); 
+  printPgmString(PSTR(" (mm/sec max start and stop feed rate)\r\n$8 = ")); printFloat(settings.mm_per_arc_segment);
+  printPgmString(PSTR(" (mm/sec^2 max acceleration)\r\n$9 = ")); printFloat(settings.acceleration);
+  printPgmString(PSTR(" (mm/arc segment)\r\n$10 = ")); printInteger(settings.invert_mask); 
   printPgmString(PSTR(" (step port invert mask. binary = ")); printIntegerInBase(settings.invert_mask, 2);  
   printPgmString(PSTR(")\r\n\r\n'$x=value' to set parameter or just '$' to dump current settings\r\n"));
 }
@@ -74,8 +78,10 @@ void store_setting(int parameter, double value) {
     case 3: settings.pulse_microseconds = round(value); break;
     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 6: settings.dead_feed_rate = value; break;
-    case 7: settings.invert_mask = trunc(value); break;
+    case 8: settings.mm_per_arc_segment = value; break;
+    case 9: settings.acceleration = value; break;
+    case 10: settings.invert_mask = trunc(value); break;
     default: 
     printPgmString(PSTR("Unknown parameter\r\n"));
     return;

config.h

@@ -69,6 +69,7 @@ struct Settings {
   double default_seek_rate;
   uint8_t invert_mask;
   double mm_per_arc_segment;
+  double accelleration;
 };
 struct Settings settings;
 

motion_control.c

@@ -68,7 +68,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
   	  square(steps[Y_AXIS]/settings.steps_per_mm[1]) + 
   	  square(steps[Z_AXIS]/settings.steps_per_mm[2]));
     st_buffer_line(steps[X_AXIS], steps[Y_AXIS], steps[Z_AXIS],
-      lround((millimeters_of_travel/feed_rate)*1000000));
+      lround((millimeters_of_travel/feed_rate)*1000000), millimeters_of_travel);
 	}
 	memcpy(position, target, sizeof(target)); // position[] = target[] 
 }
@@ -79,7 +79,7 @@ void mc_line(double x, double y, double z, float feed_rate, int invert_feed_rate
 // axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
 
 // The arc is approximated by generating a huge number of tiny, linear segments. The length of each 
-// segment is configured in config.h by setting MM_PER_ARC_SEGMENT.  
+// segment is configured in settings.mm_per_arc_segment.  
 void mc_arc(double theta, double angular_travel, double radius, double linear_travel, int axis_1, int axis_2, 
   int axis_linear, double feed_rate, int invert_feed_rate)
 {
@@ -107,7 +107,8 @@ void mc_arc(double theta, double angular_travel, double radius, double linear_tr
     theta += theta_per_segment;
     target[axis_1] = center_x+sin(theta)*radius;
     target[axis_2] = center_y+cos(theta)*radius;
-    mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate);
+    mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], feed_rate, invert_feed_rate,
+      settings.mm_per_arc_segment);
   }
 }
 

script/console

@@ -1,3 +1,3 @@
-# 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.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
+#socat -d -d READLINE /dev/tty.FireFly-A964-SPP-1,clocal=1,nonblock=1,cread=1,cs8,ixon=1,ixoff=1
 

script/stream.rb

@@ -24,7 +24,9 @@ if ARGV.empty?
   exit
 end
 
-SerialPort.open('/dev/tty.FireFly-A964-SPP-1', 115200) do |sp|
+# SerialPort.open('/dev/tty.FireFly-A964-SPP-1', 115200) do |sp|
+SerialPort.open('/dev/tty.usbserial-A9007QcR', 9600) do |sp|
+
   sp.write("\r\n\r\n");
   sleep 1
   ARGV.each do |file|
@@ -44,4 +46,6 @@ SerialPort.open('/dev/tty.FireFly-A964-SPP-1', 115200) do |sp|
       end
     end
   end
+  puts "Done."
+  sleep 500
 end

stepper.c

@@ -28,6 +28,7 @@
 #include <stdlib.h>
 #include <util/delay.h>
 #include "nuts_bolts.h"
+#include "acceleration.h"
 #include <avr/interrupt.h>
 
 #include "wiring_serial.h"
@@ -43,25 +44,42 @@ struct Line {
   uint32_t steps_x, steps_y, steps_z;
   int32_t maximum_steps;
   uint8_t direction_bits;
-  uint32_t rate;
+  double average_millimeters_per_step_event;
+  uin32_t ideal_rate; // in step-events/minute
+  uin32_t exit_rate;
+  uin32_t brake_point; // the point where braking starts measured in step-events from end point
+  uint32_t rate;  // in cpu-ticks pr. step
 };
 
 struct Line line_buffer[LINE_BUFFER_SIZE]; // A buffer for step instructions
 volatile int line_buffer_head = 0;
 volatile int line_buffer_tail = 0;
+volatile int moving = FALSE;
 
 // Variables used by SIG_OUTPUT_COMPARE1A
-uint8_t out_bits; // The next stepping-bits to be output
+uint8_t out_bits;          // The next stepping-bits to be output
 struct Line *current_line; // A pointer to the line currently being traced
-volatile int32_t counter_x, counter_y, counter_z; // counter variables for the bresenham line tracer
+volatile int32_t counter_x, 
-uint32_t iterations; // The number of iterations left to complete the current_line
+  counter_y, counter_z;    // counter variables for the bresenham line tracer
-volatile int busy; // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
+uint32_t iterations;       // The number of iterations left to complete the current_line
+volatile int busy;         // TRUE when SIG_OUTPUT_COMPARE1A is being serviced. Used to avoid retriggering that handler.
 
-void config_step_timer(uint32_t microseconds);
+void set_step_events_per_minute(uint32_t steps_per_minute);
+
+uint32_t mm_per_minute_to_step_events_pr_minute(struct Line* line, double mm_per_minute) {
+  return(mm_per_minute/line->average_millimeters_per_step_event);
+}
+
+void update_accelleration_plan() {
+  // Store the current 
+  int initial_buffer_tail = line_buffer_tail;
+  
+}
 
 // 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.
+// steps. Microseconds specify how many microseconds the move should take to perform. To aid accelleration
-void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t microseconds) {
+// 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) {
   // Calculate the buffer head after we push this byte
 	int next_buffer_head = (line_buffer_head + 1) % LINE_BUFFER_SIZE;	
 	// If the buffer is full: good! That means we are well ahead of the robot. 
@@ -75,12 +93,13 @@ void st_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t
   line->maximum_steps = max(line->steps_x, max(line->steps_y, line->steps_z));
   // Bail if this is a zero-length line
   if (line->maximum_steps == 0) { return; };
-  line->rate = microseconds/line->maximum_steps;
+  line->rate = (TICKS_PER_MICROSECOND*microseconds)/line->maximum_steps;
   uint8_t direction_bits = 0;
   if (steps_x < 0) { direction_bits |= (1<<X_DIRECTION_BIT); }
   if (steps_y < 0) { direction_bits |= (1<<Y_DIRECTION_BIT); }
   if (steps_z < 0) { direction_bits |= (1<<Z_DIRECTION_BIT); }
   line->direction_bits = direction_bits;
+  line->average_millimeters_per_step_event = millimeters/line->maximum_steps
   // Move buffer head
   line_buffer_head = next_buffer_head;
   // enable stepper interrupt
@@ -126,8 +145,10 @@ SIGNAL(SIG_OUTPUT_COMPARE1A)
       counter_y = counter_x;
       counter_z = counter_x;
       iterations = current_line->maximum_steps;
+      moving = TRUE;
     } else {
       // disable this interrupt until there is something to handle
+      moving = FALSE;
     	TIMSK1 &= ~(1<<OCIE1A);
       PORTD |= (1<<4);          
     }    
@@ -225,9 +246,8 @@ void st_flush()
 }
 
 // Configures the prescaler and ceiling of timer 1 to produce the given rate as accurately as possible.
-void config_step_timer(uint32_t microseconds)
+void config_step_timer(uint32_t ticks)
 {
-  uint32_t ticks = microseconds*TICKS_PER_MICROSECOND;
   uint16_t ceiling;
   uint16_t prescaler;
 	if (ticks <= 0xffffL) {
@@ -256,6 +276,10 @@ void config_step_timer(uint32_t microseconds)
   OCR1A = ceiling;
 }
 
+void set_step_events_per_minute(uint32_t steps_per_minute) {
+  config_step_timer((TICKS_PER_MICROSECOND*1000000*60)/steps_per_minute);
+}
+
 void st_go_home()
 {
   // Todo: Perform the homing cycle