orient.comp: Add modes with preceding referencing to index signal

Author: Sigma1912

src/emc/rs274ngc/interp_check.cc

@@ -338,8 +338,8 @@ int Interp::check_other_codes(block_pointer block)       //!< pointer to a block
       CHKS(((motion == G_2 || motion == G_3 || (block->m_modes[7] == 19)) &&
 	    fabs(p_value - block->p_number) > 0.001),
 	   _("P value not an integer with M19 G2 or G3"));
-      CHKS((block->m_modes[7] == 19) && ((p_value > 2) || p_value < 0),
-	   _("P value must be 0,1,or 2 with M19"));
+      CHKS((block->m_modes[7] == 19) && ((p_value > 5) || p_value < 0),
+	   _("P value must be 0,1,2,3,4 or 5 with M19"));
       CHKS(((motion == G_2 || motion == G_3) && round_to_int(block->p_number) < 1),
           _("P value should be 1 or greater with G2 or G3"));
   }

src/hal/components/orient.comp

@@ -1,16 +1,21 @@
 component orient "Provide a PID command input for orientation mode based on current spindle position, target angle and orient mode";
 
 pin in  bit   enable      "enable angular output for orientation mode";
-pin in  s32   mode        "0: rotate - shortest move; 1: always rotate clockwise; 2: always rotate counterclockwise";
+pin in  s32   mode        "sets rotation direction";
 pin in  float position    "spindle position input, unit 1 rev";
 pin in  float angle       "orient target position in degrees, 0 ≤ angle < 360";
 pin out float command     "target spindle position, input to PID command";
 pin out float poserr      "in degrees - aid for PID tuning";
 pin out bit is-oriented   "This pin goes high when poserr < tolerance. Use to drive spindle.N.is-oriented";
 pin in float tolerance = 0.5  "The tolerance in degrees for considering the align completed";
+pin io bit index-enable   "Connect to the spindle encoder counter to reset to index before orienting";
+
+param r unsigned state "state machine for index homing";
 
 variable int   last_enable = 0;
 variable int    debounce = 0; // to prevent the in-position triggering with the spindle moving
+variable double target_angle = 0;
+variable double latched_position = 0;
 
 option fp yes;
 option period no;
@@ -37,6 +42,9 @@ The *mode* pin is interpreted as follows:
 which may be clockwise or counterclockwise.
 * 1: the spindle rotates always rotates clockwise to the new angle.
 * 2: the spindle rotates always rotates counterclockwise to the new angle.
+* 3: same as mode 0 but with homing to index before orienting to the new angle.
+* 4: same as mode 1 but with homing to index before orienting to the new angle.
+* 5: same as mode 2 but with homing to index before orienting to the new angle.
 
 === HAL USAGE
 
@@ -63,16 +71,35 @@ license "GPL";
 
 FUNCTION(_) {
 
-    double target_angle;
-    double latched_position;
     is_oriented = 0; // spindle.is-oriented inhibits spindle.orient
-    if (enable) {
-       if (enable ^ last_enable) {     // positive edge on enable
-           is_oriented = 0;
+
+    switch (state){
+        case 0: // waiting
+            if (enable) {
+            index_enable = 0;
+            if (enable ^ last_enable) {     // positive edge on enable
+                is_oriented = 0;
+                if (mode > 2){
+                    index_enable = 1;
+                    state = 3;
+                } else {
+                    state = 1;
+                }
+            }
+            }
+            break;
+
+        case 1: // normal orient - init
+            if (! enable) state = 0;
             debounce = 0;
             latched_position = position; // sample now
             target_angle =  angle/360.0;
-            switch (mode) {
+            state = 2;
+            break;
+
+        case 2: // orienting
+            if (! enable) state = 0;
+            switch (mode % 3) {
             case 0: // shortest move
                 command = floor(latched_position+0.5-target_angle) + target_angle;
                 break;
@@ -82,12 +109,45 @@ FUNCTION(_) {
             case 2: // always ccw
                 command = floor(latched_position-target_angle) + target_angle ;
                 break;
+            default:
+                rtapi_print_msg(RTAPI_MSG_ERR, "unhandled mode %i\n", mode % 3);
+        }
+        poserr = (position - command) * 360.0;
+        debounce += (fabs(poserr) < tolerance && debounce <=100);
+        is_oriented = (debounce > 100);
+        if (is_oriented) state = 0;
+        break;
+
+        case 3: // index search - init
+            if (! enable) state = 0;
+            latched_position = position; // sample now
+            state = 4;
+            break;
+
+        case 4: // waiting for spindle index
+            if (! enable) state = 0;
+            if (! index_enable){ // index has reset)
+                state = 1;
+                break;
+            }
+            switch (mode) {
+            case 3: // shortest move to index
+                if (0 > latched_position) {
+                command = position + 0.5;
+                } else {
+                command = position - 0.5;
+                }
+                break;
+            case 4: // always cw to index
+                command = position + 0.5;
+                break;
+            case 5: // always ccw to index
+                command = position - 0.5;
+                break;
+            default:
+                rtapi_print_msg(RTAPI_MSG_ERR, "unhandled mode %i\n", mode);
             }
-       }
-       poserr = (position - command) * 360.0;
-       debounce += (fabs(poserr) < tolerance && debounce <=100);
-       is_oriented = (debounce > 100);
+            break;
     }
     last_enable = enable;
 }
-