clean up defines of vector when not actually used

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

diff --git a/src/modules/robot/Planner.cpp b/src/modules/robot/Planner.cpp
index c4711fa..82f58b9 100644 (file)
--- a/src/modules/robot/Planner.cpp
+++ b/src/modules/robot/Planner.cpp
@@ -6,7 +6,6 @@
 */
 
 using namespace std;
-#include <vector>
 
 #include "mri.h"
 #include "nuts_bolts.h"
@@ -50,19 +49,19 @@ void Planner::config_load()
 
 
 // Append a block to the queue, compute it's speed factors
-bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors, float rate_mm_s, float distance, float *unit_vec, float acceleration)
+bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors, float rate_mm_s, float distance, float *unit_vec, float acceleration, float s_value, bool g123)
 {
     // Create ( recycle ) a new block
     Block* block = THECONVEYOR->queue.head_ref();
 
     // Direction bits
-    bool has_steps= false;
+    bool has_steps = false;
     for (size_t i = 0; i < n_motors; i++) {
         int32_t steps = THEROBOT->actuators[i]->steps_to_target(actuator_pos[i]);
         // Update current position
         if(steps != 0) {
             THEROBOT->actuators[i]->update_last_milestones(actuator_pos[i], steps);
-            has_steps= true;
+            has_steps = true;
         }
 
         // find direction
@@ -71,28 +70,45 @@ bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors,
         block->steps[i] = labs(steps);
     }
 
-    // sometimres even though there is a detectable movement it turns out there are no steps to be had from such a small move
-    if(!has_steps) return false;
+    // sometimes even though there is a detectable movement it turns out there are no steps to be had from such a small move
+    if(!has_steps) {
+        block->clear();
+        return false;
+    }
+
+    // info needed by laser
+    block->s_value = roundf(s_value*(1<<11)); // 1.11 fixed point
+    block->is_g123 = g123;
 
     // use default JD
     float junction_deviation = this->junction_deviation;
 
     // use either regular junction deviation or z specific and see if a primary axis move
-    block->primary_axis= true;
-    if(block->steps[ALPHA_STEPPER] == 0 && block->steps[BETA_STEPPER] == 0){
+    block->primary_axis = true;
+    if(block->steps[ALPHA_STEPPER] == 0 && block->steps[BETA_STEPPER] == 0) {
         if(block->steps[GAMMA_STEPPER] != 0) {
             // z only move
             if(!isnan(this->z_junction_deviation)) junction_deviation = this->z_junction_deviation;
-        }else{
+
+        } else {
             // is not a primary axis move
             block->primary_axis= false;
+            #if N_PRIMARY_AXIS > 3
+                for (int i = 3; i < N_PRIMARY_AXIS; ++i) {
+                    if(block->steps[i] != 0){
+                        block->primary_axis= true;
+                        break;
+                    }
+                }
+            #endif
+
         }
     }
 
     block->acceleration = acceleration; // save in block
 
     // Max number of steps, for all axes
-    auto mi= std::max_element(block->steps.begin(), block->steps.end());
+    auto mi = std::max_element(block->steps.begin(), block->steps.end());
     block->steps_event_count = *mi;
 
     block->millimeters = distance;
@@ -127,7 +143,7 @@ bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors,
 
     // if unit_vec was null then it was not a primary axis move so we skip the junction deviation stuff
     if (unit_vec != nullptr && !THECONVEYOR->is_queue_empty()) {
-        Block *prev_block= THECONVEYOR->queue.item_ref(THECONVEYOR->queue.prev(THECONVEYOR->queue.head_i));
+        Block *prev_block = THECONVEYOR->queue.item_ref(THECONVEYOR->queue.prev(THECONVEYOR->queue.head_i));
         float previous_nominal_speed = prev_block->primary_axis ? prev_block->nominal_speed : 0;
 
         if (junction_deviation > 0.0F && previous_nominal_speed > 0.0F) {
@@ -135,13 +151,18 @@ bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors,
             // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
             float cos_theta = - this->previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
                               - this->previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
-                              - this->previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
+                              - this->previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS];
+            #if N_PRIMARY_AXIS > 3
+                for (int i = 3; i < N_PRIMARY_AXIS; ++i) {
+                    cos_theta -= this->previous_unit_vec[i] * unit_vec[i];
+                }
+            #endif
 
             // Skip and use default max junction speed for 0 degree acute junction.
-            if (cos_theta < 0.95F) {
+            if (cos_theta <= 0.9999F) {
                 vmax_junction = std::min(previous_nominal_speed, block->nominal_speed);
                 // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
-                if (cos_theta > -0.95F) {
+                if (cos_theta >= -0.9999F) {
                     // Compute maximum junction velocity based on maximum acceleration and junction deviation
                     float sin_theta_d2 = sqrtf(0.5F * (1.0F - cos_theta)); // Trig half angle identity. Always positive.
                     vmax_junction = std::min(vmax_junction, sqrtf(acceleration * junction_deviation * sin_theta_d2 / (1.0F - sin_theta_d2)));
@@ -172,7 +193,7 @@ bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors,
     // Update previous path unit_vector and nominal speed
     if(unit_vec != nullptr) {
         memcpy(previous_unit_vec, unit_vec, sizeof(previous_unit_vec)); // previous_unit_vec[] = unit_vec[]
-    }else{
+    } else {
         memset(previous_unit_vec, 0, sizeof(previous_unit_vec));
     }