Merge pull request #160 from Paciente8159/short-line-subsegments-opti…

…mization

improved subsegment planner computations

CHANGELOG.md

@@ -14,10 +14,13 @@
 
 ### Changed
 
+- improved subsegment planner computations by skipping junction speed calculations after initial calculation for first subsegment (#160) 
+
 ### Fixed
 
 - fixed random angle error calculation between planner line segments that cause random speed drops (#158)
 - fixed compilation errors for main config file options (#159)
+- fixed unit vector calculation in motion control line function (#160) 
 
 ## [1.4.0-beta2] - 2022-04-01
 

uCNC/src/core/motion_control.c

@@ -243,7 +243,7 @@ uint8_t mc_line(float *target, motion_data_t *block_data)
 
 #if (KINEMATIC == KINEMATIC_DELTA)
     float line_dist = fast_flt_sqrt(inv_dist);
-    inv_dist = 1.0f / inv_dist;
+    inv_dist = 1.0f / line_dist;
 #else
     inv_dist = fast_flt_invsqrt(inv_dist);
 #endif
@@ -319,11 +319,13 @@ uint8_t mc_line(float *target, motion_data_t *block_data)
                 break;
             }
         }
+        block_data->is_subsegment = true;
     }
 
     // stores the new position for the next motion
     memcpy(mc_last_target, target, sizeof(mc_last_target));
     block_data->feed = feed;
+    block_data->is_subsegment = false;
     return error;
 }
 

uCNC/src/core/motion_control.h

@@ -1,19 +1,19 @@
 /*
-	Name: motion_control.h
-	Description: Contains the building blocks for performing motions/actions in µCNC.
+        Name: motion_control.h
+        Description: Contains the building blocks for performing motions/actions in µCNC.
 
-	Copyright: Copyright (c) João Martins
-	Author: João Martins
-	Date: 19/11/2019
+        Copyright: Copyright (c) João Martins
+        Author: João Martins
+        Date: 19/11/2019
 
-	µCNC 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. Please see <http://www.gnu.org/licenses/>
+        µCNC 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. Please see <http://www.gnu.org/licenses/>
 
-	µCNC is distributed WITHOUT ANY WARRANTY;
-	Also without the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
-	See the	GNU General Public License for more details.
+        µCNC is distributed WITHOUT ANY WARRANTY;
+        Also without the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+        See the	GNU General Public License for more details.
 */
 
 #ifndef MOTION_CONTROL_H
@@ -44,8 +44,8 @@ extern "C"
                 step_t steps[STEPPER_COUNT];
                 float dir_vect[AXIS_COUNT];
                 uint8_t dirbits;
-                uint32_t full_steps; //number of steps of all linear actuators
-                step_t total_steps;  //the number of pulses needed to generate all steps (maximum of all linear actuators)
+                uint32_t full_steps; // number of steps of all linear actuators
+                step_t total_steps;  // the number of pulses needed to generate all steps (maximum of all linear actuators)
                 float feed;
                 uint8_t main_stepper;
                 int16_t spindle;
@@ -56,22 +56,23 @@ extern "C"
 #endif
                 bool update_tools;
                 bool feed_override;
+                bool is_subsegment;
         } motion_data_t;
 
         void mc_init(void);
         bool mc_get_checkmode(void);
         bool mc_toogle_checkmode(void);
 
-        //async motions
+        // async motions
         uint8_t mc_line(float *target, motion_data_t *block_data);
         uint8_t mc_arc(float *target, float center_offset_a, float center_offset_b, float radius, uint8_t axis_0, uint8_t axis_1, bool isclockwise, motion_data_t *block_data);
 
-        //sync motions
+        // sync motions
         uint8_t mc_dwell(motion_data_t *block_data);
         uint8_t mc_pause(void);
         uint8_t mc_update_tools(motion_data_t *block_data);
 
-        //mixed/special motions
+        // mixed/special motions
         uint8_t mc_home_axis(uint8_t axis, uint8_t axis_limit);
         uint8_t mc_probe(float *target, uint8_t flags, motion_data_t *block_data);
 

uCNC/src/core/planner.c

@@ -111,12 +111,21 @@ void planner_add_line(motion_data_t *block_data)
     float dir_vect[STEPPER_COUNT];
     memset(dir_vect, 0, sizeof(dir_vect));
 #else
-    for (uint8_t i = AXIS_COUNT; i != 0;)
+
+    if (!block_data->is_subsegment)
     {
-        i--;
-        cos_theta += block_data->dir_vect[i] * last_dir_vect[i];
-        last_dir_vect[i] = block_data->dir_vect[i];
+        for (uint8_t i = AXIS_COUNT; i != 0;)
+        {
+            i--;
+            cos_theta += block_data->dir_vect[i] * last_dir_vect[i];
+            last_dir_vect[i] = block_data->dir_vect[i];
+        }
     }
+    else
+    {
+        cos_theta = 1;
+    }
+
 #endif
 
     for (uint8_t i = STEPPER_COUNT; i != 0;)
@@ -192,32 +201,39 @@ void planner_add_line(motion_data_t *block_data)
     // if more than one move stored cals juntion speeds and recalculates speed profiles
     if (cos_theta != 0 && !CHECKFLAG(block_data->motion_mode, PLANNER_MOTION_EXACT_STOP | MOTIONCONTROL_MODE_BACKLASH_COMPENSATION))
     {
-        // calculates the junction angle with previous
-        if (cos_theta > 0)
+        if (!block_data->is_subsegment)
         {
-            // uses the half angle identity conversion to convert from cos(theta) to tan(theta/2) where:
-            //	tan(theta/2) = sqrt((1-cos(theta)/(1+cos(theta))
-            // to simplify the calculations it multiplies by sqrt((1+cos(theta)/(1+cos(theta))
-            // transforming the equation to sqrt((1^2-cos(theta)^2))/(1+cos(theta))
-            // this way the output will be between 0<tan(theta/2)<inf
-            // but if theta is 0<theta<90 the tan(theta/2) will be 0<tan(theta/2)<1
-            // all angles greater than 1 that can be excluded
-            angle_factor = 1.0f / (1.0f + cos_theta);
-            cos_theta = (1.0f - fast_flt_pow2(cos_theta));
-            angle_factor *= fast_flt_sqrt(cos_theta);
-        }
+            // calculates the junction angle with previous
+            if (cos_theta > 0)
+            {
+                // uses the half angle identity conversion to convert from cos(theta) to tan(theta/2) where:
+                //	tan(theta/2) = sqrt((1-cos(theta)/(1+cos(theta))
+                // to simplify the calculations it multiplies by sqrt((1+cos(theta)/(1+cos(theta))
+                // transforming the equation to sqrt((1^2-cos(theta)^2))/(1+cos(theta))
+                // this way the output will be between 0<tan(theta/2)<inf
+                // but if theta is 0<theta<90 the tan(theta/2) will be 0<tan(theta/2)<1
+                // all angles greater than 1 that can be excluded
+                angle_factor = 1.0f / (1.0f + cos_theta);
+                cos_theta = (1.0f - fast_flt_pow2(cos_theta));
+                angle_factor *= fast_flt_sqrt(cos_theta);
+            }
 
-        // sets the maximum allowed speed at junction (if angle doesn't force a full stop)
-        float factor = ((!CHECKFLAG(block_data->motion_mode, PLANNER_MOTION_CONTINUOUS)) ? 0 : g_settings.g64_angle_factor);
-        angle_factor = CLAMP(0, angle_factor - factor, 1);
+            // sets the maximum allowed speed at junction (if angle doesn't force a full stop)
+            float factor = ((!CHECKFLAG(block_data->motion_mode, PLANNER_MOTION_CONTINUOUS)) ? 0 : g_settings.g64_angle_factor);
+            angle_factor = CLAMP(0, angle_factor - factor, 1);
 
-        if (angle_factor < 1.0f)
+            if (angle_factor < 1.0f)
+            {
+                float junc_feed_sqr = (1 - angle_factor);
+                junc_feed_sqr = fast_flt_pow2(junc_feed_sqr);
+                junc_feed_sqr *= planner_data[prev].feed_sqr;
+                // the maximum feed is the minimal feed between the previous feed given the angle and the current feed
+                planner_data[planner_data_write].entry_max_feed_sqr = MIN(planner_data[planner_data_write].feed_sqr, junc_feed_sqr);
+            }
+        }
+        else
         {
-            float junc_feed_sqr = (1 - angle_factor);
-            junc_feed_sqr = fast_flt_pow2(junc_feed_sqr);
-            junc_feed_sqr *= planner_data[prev].feed_sqr;
-            // the maximum feed is the minimal feed between the previous feed given the angle and the current feed
-            planner_data[planner_data_write].entry_max_feed_sqr = MIN(planner_data[planner_data_write].feed_sqr, junc_feed_sqr);
+            planner_data[planner_data_write].entry_max_feed_sqr = MIN(planner_data[planner_data_write].feed_sqr, planner_data[prev].feed_sqr);
         }
 
         // forces reaclculation with the new block