X-Git-Url: http://git.hcoop.net/clinton/Smoothieware.git/blobdiff_plain/e560f057750990a3234eed27fe0d98d656df97c3..49de901734414b04c54cd1890f3a6bd2a03e4d49:/src/modules/robot/Planner.cpp

diff --git a/src/modules/robot/Planner.cpp b/src/modules/robot/Planner.cpp
index 35f745d7..82f58b92 100644
--- 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"
@@ -78,8 +77,8 @@ bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors,
     }
 
     // info needed by laser
-    block->s_value= s_value;
-    block->is_g123= g123;
+    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;
@@ -90,9 +89,19 @@ bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors,
         if(block->steps[GAMMA_STEPPER] != 0) {
             // z only move
             if(!isnan(this->z_junction_deviation)) junction_deviation = this->z_junction_deviation;
+
         } else {
             // is not a primary axis move
-            block->primary_axis = false;
+            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
+
         }
     }
 
@@ -142,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)));