thcud.comp: address #911 and #675

LinuxCNC · Oct 3, 2023 · 946b421 · 946b421
1 parent 994abb4
commit 946b421
Showing 1 changed file with 14 additions and 15 deletions.
diff --git a/src/hal/components/thcud.comp b/src/hal/components/thcud.comp
@@ -81,13 +81,12 @@ pin out float z_fb_out "Z Position Feedback to Axis";
 pin out float cur_offset "The Current Offset";
 pin out bit vel_status "When the THC thinks we are at requested speed";
 pin out bit removing_offset "Pin for testing";
+pin in float correction_vel "The Velocity to move Z to correct";
 
 // Parameters
 param rw float velocity_tol "The deviation percent from planned velocity";
-param rw float correction_vel "The Velocity to move Z to correct";
 
 // Global Variables
-variable float offset;
 variable float last_z_in;
 
 function _;
@@ -104,39 +103,39 @@ FUNCTION(_) {
 
         if(torch_on && arc_ok && vel_status){ // allow correction
             if(torch_down){
-                offset -= correction_vel;
+                cur_offset -= correction_vel;
             }
             if(torch_up){
-                offset += correction_vel;
+                cur_offset += correction_vel;
             }    
             last_z_in = 0;
         }
         if(!torch_on){ // remove any offset
             float z_diff;
             z_diff = z_pos_in - last_z_in;
-            if(z_diff > 0 && offset != 0){ // torch is moving up
+            if(z_diff > 0 && cur_offset != 0){ // torch is moving up
                 removing_offset = 1;
-                if(offset > 0){ // positive offset
-                    if(offset > z_diff){ // remove some
-                        offset -= z_diff;
+                if(cur_offset > 0){ // positive offset
+                    if(cur_offset > z_diff){ // remove some
+                        cur_offset -= z_diff;
                     }
-                    else {offset = 0;}
+                    else {cur_offset = 0;}
                 }
-                if(offset < 0){ // negative offset
-                    if(offset < z_diff){ // remove some
-                        offset += z_diff;
+                if(cur_offset < 0){ // negative offset
+                    if(cur_offset < z_diff){ // remove some
+                        cur_offset += z_diff;
                     }
-                    else {offset = 0;}
+                    else {cur_offset = 0;}
                 }
             }
             else {removing_offset = 0;}
             last_z_in = z_pos_in;
         }
-        z_pos_out = z_pos_in + offset;
+        z_pos_out = z_pos_in + cur_offset;
         z_fb_out = z_pos_in; // keep axis motor position fb from being confused
     }
     if(!enable){
-        z_pos_out = z_pos_in;
+        z_pos_out = z_pos_in + cur_offset;
         z_fb_out = z_pos_in; // keep axis motor position fb from being confused
     }
 }