From 7327b8258515142765717cc68c324b5544603c8f Mon Sep 17 00:00:00 2001
From: Simen Svale Skogsrud <simen@bengler.no>
Date: Fri, 14 Jan 2011 17:13:33 +0100
Subject: [PATCH] interface to enable or disable acceleration management

---
 main.c        |  2 ++
 motion_plan.c | 38 +++++++++++++++++++++++++++++++++-----
 motion_plan.h |  9 +++++++++
 3 files changed, 44 insertions(+), 5 deletions(-)

diff --git a/main.c b/main.c
index 39f6526..26c7ec5 100644
--- a/main.c
+++ b/main.c
@@ -21,6 +21,7 @@
 #include <avr/io.h>
 #include <avr/sleep.h>
 #include <util/delay.h>
+#include "motion_plan.h"
 #include "stepper.h"
 #include "spindle_control.h"
 #include "motion_control.h"
@@ -35,6 +36,7 @@ int main(void)
   beginSerial(BAUD_RATE);
   printString("A");
   config_init();
+  mp_init(); // initialize the motion plan subsystem
   st_init(); // initialize the stepper subsystem
   mc_init(); // initialize motion control subsystem
   spindle_init(); // initialize spindle controller
diff --git a/motion_plan.c b/motion_plan.c
index edbffe1..be9dcde 100644
--- a/motion_plan.c
+++ b/motion_plan.c
@@ -20,14 +20,17 @@
 
 #include <inttypes.h>
 #include <math.h>       
+#include <stdlib.h>
 
 #include "motion_plan.h"
 #include "nuts_bolts.h"
 #include "stepper.h"
+#include "config.h"
 
 struct Block block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
-volatile int block_buffer_head = 0;           // Index of the next block to be pushed
-volatile int block_buffer_tail = 0;           // Index of the block to process now
+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));
@@ -40,6 +43,27 @@ 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) {
@@ -103,7 +127,13 @@ void mp_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t
   block->rate_delta = round(
     (settings.acceleration/(60.0*ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/min per acceleration_tick
     travel_per_step);                                           // convert to: acceleration steps/min/acceleration_tick    
-  calculate_trapezoid_for_block(block,0,0);                     // compute a default trapezoid
+  if (acceleration_management) {
+    calculate_trapezoid_for_block(block,0,0);                     // compute a conservative acceleration trapezoid for now
+  } else {
+    block->accelerate_ticks = 0;
+    block->plateau_ticks = 0;
+    block->rate_delta = 0;
+  }
   
   // Compute direction bits for this block
   block->direction_bits = 0;
@@ -112,7 +142,5 @@ void mp_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_t
   if (steps_z < 0) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
   // Move buffer head
   block_buffer_head = next_buffer_head;
-  // Ensure that block processing is running by enabling The Stepper Driver Interrupt
-  ENABLE_STEPPER_DRIVER_INTERRUPT();
 }
 
diff --git a/motion_plan.h b/motion_plan.h
index 74ba459..c9d3912 100644
--- a/motion_plan.h
+++ b/motion_plan.h
@@ -50,6 +50,9 @@ extern struct Block block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion
 extern volatile int block_buffer_head;           // Index of the next block to be pushed
 extern volatile int block_buffer_tail;           // Index of the block to process now
 
+// Initialize the motion plan subsystem      
+void mp_init();
+
 // 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.
@@ -57,4 +60,10 @@ extern volatile int block_buffer_tail;           // Index of the block to proces
 // 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();
+// Disables acceleration-management for upcoming blocks
+void mp_disable_acceleration_management();
+
+
 #endif
\ No newline at end of file