Allow TABS in config

[clinton/Smoothieware.git] / src / modules / robot / Block.cpp

diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp
index 78abc05..acea78e 100644 (file)
--- a/src/modules/robot/Block.cpp
+++ b/src/modules/robot/Block.cpp
@@ -13,9 +13,14 @@
 #include "Block.h"
 #include "Planner.h"
 #include "Conveyor.h"
+#include "Gcode.h"
+#include "libs/StreamOutputPool.h"
+#include "Stepper.h"
+
+#include "mri.h"
+
 using std::string;
 #include <vector>
-#include "../communication/utils/Gcode.h"
 
 // A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves.
 // It's stacked on a queue, and that queue is then executed in order, to move the motors.
@@ -34,27 +39,28 @@ void Block::clear()
     gcodes.clear();
     clear_vector(this->steps);
 
-    steps_event_count= 0;
-    nominal_rate= 0;
-    nominal_speed= 0.0F;
-    millimeters= 0.0F;
-    entry_speed= 0.0F;
-    rate_delta= 0.0F;
-    initial_rate= -1;
-    final_rate= -1;
-    accelerate_until= 0;
-    decelerate_after= 0;
-    direction_bits= 0;
-    recalculate_flag= false;
-    nominal_length_flag= false;
-    max_entry_speed= 0.0F;
-    is_ready= false;
-    times_taken= 0;
+    steps_event_count   = 0;
+    nominal_rate        = 0;
+    nominal_speed       = 0.0F;
+    millimeters         = 0.0F;
+    entry_speed         = 0.0F;
+    exit_speed          = 0.0F;
+    rate_delta          = 0.0F;
+    initial_rate        = -1;
+    final_rate          = -1;
+    accelerate_until    = 0;
+    decelerate_after    = 0;
+    direction_bits      = 0;
+    recalculate_flag    = false;
+    nominal_length_flag = false;
+    max_entry_speed     = 0.0F;
+    is_ready            = false;
+    times_taken         = 0;
 }
 
 void Block::debug()
 {
-    THEKERNEL->serial->printf("%p: steps:X%04d Y%04d Z%04d(max:%4d) nominal:r%10d/s%6.1f mm:%9.6f rdelta:%8f acc:%5d dec:%5d rates:%10d>%10d  entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d nomlen:%d\r\n",
+    THEKERNEL->streams->printf("%p: steps:X%04d Y%04d Z%04d(max:%4d) nominal:r%10d/s%6.1f mm:%9.6f rdelta:%8f acc:%5d dec:%5d rates:%10d>%10d  entry/max: %10.4f/%10.4f taken:%d ready:%d recalc:%d nomlen:%d\r\n",
                                this,
                                          this->steps[0],
                                                this->steps[1],
@@ -94,13 +100,13 @@ void Block::calculate_trapezoid( float entryspeed, float exitspeed )
         return;
 
     // The planner passes us factors, we need to transform them in rates
-    this->initial_rate = ceil(this->nominal_rate * entryspeed / this->nominal_speed);   // (step/min)
-    this->final_rate   = ceil(this->nominal_rate * exitspeed  / this->nominal_speed);   // (step/min)
+    this->initial_rate = ceil(this->nominal_rate * entryspeed / this->nominal_speed);   // (step/s)
+    this->final_rate   = ceil(this->nominal_rate * exitspeed  / this->nominal_speed);   // (step/s)
 
     // How many steps to accelerate and decelerate
-    float acceleration_per_minute = this->rate_delta * THEKERNEL->stepper->acceleration_ticks_per_second * 60.0; // ( step/min^2)
-    int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_minute ) );
-    int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate,  -acceleration_per_minute ) );
+    float acceleration_per_second = this->rate_delta * THEKERNEL->stepper->acceleration_ticks_per_second; // ( step/s^2)
+    int accelerate_steps = ceil( this->estimate_acceleration_distance( this->initial_rate, this->nominal_rate, acceleration_per_second ) );
+    int decelerate_steps = floor( this->estimate_acceleration_distance( this->nominal_rate, this->final_rate,  -acceleration_per_second ) );
 
     // Calculate the size of Plateau of Nominal Rate ( during which we don't accelerate nor decelerate, but just cruise )
     int plateau_steps = this->steps_event_count - accelerate_steps - decelerate_steps;
@@ -109,7 +115,7 @@ void Block::calculate_trapezoid( float entryspeed, float exitspeed )
     // have to use intersection_distance() to calculate when to abort acceleration and start braking
     // in order to reach the final_rate exactly at the end of this block.
     if (plateau_steps < 0) {
-        accelerate_steps = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_minute, this->steps_event_count));
+        accelerate_steps = ceil(this->intersection_distance(this->initial_rate, this->final_rate, acceleration_per_second, this->steps_event_count));
         accelerate_steps = max( accelerate_steps, 0 ); // Check limits due to numerical round-off
         accelerate_steps = min( accelerate_steps, int(this->steps_event_count) );
         plateau_steps = 0;
@@ -239,6 +245,9 @@ void Block::begin()
 {
     recalculate_flag = false;
 
+    if (!is_ready)
+        __debugbreak();
+
     times_taken = -1;
 
     // execute all the gcodes related to this block
@@ -270,9 +279,14 @@ void Block::release()
 {
     if (--this->times_taken <= 0)
     {
-        THEKERNEL->call_event(ON_BLOCK_END, this);
+        times_taken = 0;
+        if (is_ready)
+        {
+            is_ready = false;
+            THEKERNEL->call_event(ON_BLOCK_END, this);
 
-        // ensure conveyor gets called last
-        THEKERNEL->conveyor->on_block_end(this);
+            // ensure conveyor gets called last
+            THEKERNEL->conveyor->on_block_end(this);
+        }
     }
 }