Ensure Limit Axis Works Properly

lib/python/qtvcp/widgets/gcode_editor.py

@@ -624,6 +624,14 @@ def _hal_init(self):
             STATUS.connect('interp_idle', lambda w: self.set_line_number(0))
         self.markerDeleteHandle(self.currentHandle)
 
+    def focusOutEvent(self, event):
+        self.setColorBackground(QColor(211, 211, 211))
+        super().focusOutEvent(event)
+
+    def focusInEvent(self, event):
+        self.setColorBackground(QColor(255, 255, 255))
+        super().focusInEvent(event)
+
     def load_program(self, w, filename=None):
         if filename is None:
             filename = self._last_filename

src/Makefile

@@ -1141,6 +1141,7 @@ rosekins-objs := emc/kinematics/rosekins.o
 obj-m += scorbot-kins.o
 scorbot-kins-objs := emc/kinematics/scorbot-kins.o
 
+
 ifeq ($(origin userkfuncs), undefined)
   # use template:
   USERKFUNCS = emc/kinematics/userkfuncs.o
@@ -1211,6 +1212,14 @@ obj-m += 5axiskins.o
 5axiskins-objs += emc/kinematics/kins_util.o
 5axiskins-objs += emc/kinematics/switchkins.o
 5axiskins-objs += $(USERKFUNCS)
+
+obj-m += hot-kins.o
+hot-kins-objs := emc/kinematics/hot-kins.o
+hot-kins-objs += libnml/posemath/_posemath.o
+hot-kins-objs += libnml/posemath/sincos.o $(MATHSTUB)
+hot-kins-objs += emc/kinematics/kins_util.o
+hot-kins-objs += $(USERKFUNCS)
+
 #----------------------------------------------------------------
 
 obj-$(CONFIG_MOTMOD) += motmod.o
@@ -1343,6 +1352,7 @@ endif
 ../rtlib/rotarydeltakins$(MODULE_EXT): $(addprefix objects/rt,$(rotarydeltakins-objs))
 ../rtlib/rosekins$(MODULE_EXT): $(addprefix objects/rt,$(rosekins-objs))
 ../rtlib/scorbot-kins$(MODULE_EXT): $(addprefix objects/rt,$(scorbot-kins-objs))
+../rtlib/hot-kins$(MODULE_EXT): $(addprefix objects/rt,$(hot-kins-objs))
 ../rtlib/hal_gm$(MODULE_EXT): $(addprefix objects/rt,$(hal_gm-objs))
 ../rtlib/hostmot2$(MODULE_EXT): $(addprefix objects/rt,$(hostmot2-objs))
 ../rtlib/hm2_test$(MODULE_EXT): $(addprefix objects/rt,$(hm2_test-objs))

src/emc/kinematics/hot-kins.c

@@ -0,0 +1,281 @@
+/********************************************************************
+* Description: hotkins.c
+*
+*   Derived from a work by Fred Proctor & Will Shackleford
+*
+* Author: Chad Woitas
+* License: GPL Version 2
+* System: Linux
+*
+* Copyright (c) 2022 RMD Engineering Inc.
+*
+* Last change:
+********************************************************************/
+
+#include "motion.h"
+#include "hal.h"
+#include "rtapi_math.h"
+#include "rtapi_app.h"
+#include "kinematics.h"
+#include "posemath.h"
+#include "stdlib.h"
+#include "stdio.h"
+
+#define HW_JX 0
+#define HW_JY 1
+#define HW_JU 2
+#define HW_JV 3
+#define HW_JB 4
+
+enum kins_setting{
+    KINS_UNSYNCHRONIZED,
+    KINS_SYNCHRONIZED
+};
+
+static kparms kp; // kinematics parms (common all types)
+
+static struct hotwire_swdata {
+    hal_bit_t   *kinstype_is_sync;
+    hal_bit_t   *kinstype_is_unsync;
+    hal_float_t *u_offset;
+    hal_float_t *v_offset;
+
+    hal_u32_t   *switchkins_type;
+    hal_u32_t   *switchkins_type_old;
+    hal_bit_t   *switchkins_update;
+
+
+    hal_bit_t   *error;
+    hal_bit_t   *reset_error;
+} *hotwire_swdata;;
+
+#define HOTWIRE_MAX_TYPES KINS_SYNCHRONIZED
+static bool use_lastpose[HOTWIRE_MAX_TYPES] = {0};
+
+
+
+static int sync_KinematicsForward(const double *joints,
+                                  EmcPose * pos,
+                                  const KINEMATICS_FORWARD_FLAGS * fflags,
+                                  KINEMATICS_INVERSE_FLAGS * iflags){
+
+  pos->tran.x = joints[HW_JX];
+  pos->tran.y = joints[HW_JY];
+  pos->u = joints[HW_JU];
+  pos->v = joints[HW_JV];
+  pos->b = joints[HW_JB];
+
+  return 0;
+} // sync_KinematicsForward
+
+static int sync_KinematicsInverse(EmcPose * pos,
+                                  double *joints,
+                                  const KINEMATICS_INVERSE_FLAGS * iflags,
+                                  KINEMATICS_FORWARD_FLAGS * fflags){
+
+  if(use_lastpose[*hotwire_swdata->switchkins_type]){
+
+    *hotwire_swdata->u_offset = joints[HW_JX] - joints[HW_JU];
+    *hotwire_swdata->v_offset = joints[HW_JY] - joints[HW_JV];
+
+    use_lastpose[*hotwire_swdata->switchkins_type] = 0;
+  }
+
+  joints[HW_JX] = pos->tran.x;
+  joints[HW_JY] = pos->tran.y;
+  joints[HW_JU] = pos->tran.x - *hotwire_swdata->u_offset;
+  joints[HW_JV] = pos->tran.y - *hotwire_swdata->v_offset;
+
+  joints[HW_JB] = pos->b;
+
+  return 0;
+} // sync_KinematicsInverse
+
+
+static int unsync_KinematicsForward(const double *joints,
+                                    EmcPose * pos,
+                                    const KINEMATICS_FORWARD_FLAGS * fflags,
+                                    KINEMATICS_INVERSE_FLAGS * iflags){
+
+  pos->tran.x = joints[HW_JX];
+  pos->tran.y = joints[HW_JY];
+  pos->u = joints[HW_JU];
+  pos->v = joints[HW_JV];
+  pos->b = joints[HW_JB];
+
+  return 0;
+} //unsync_KinematicsForward
+
+static int unsync_KinematicsInverse(EmcPose * pos,
+                                    double *joints,
+                                    const KINEMATICS_INVERSE_FLAGS * iflags,
+                                    KINEMATICS_FORWARD_FLAGS * fflags){
+
+  if(use_lastpose[*hotwire_swdata->switchkins_type]){
+    use_lastpose[*hotwire_swdata->switchkins_type] = 0;
+    pos->u = joints[HW_JU];
+    pos->v = joints[HW_JV];
+    *hotwire_swdata->u_offset = 0;
+    *hotwire_swdata->v_offset = 0;
+  }
+
+  joints[HW_JX] = pos->tran.x;
+  joints[HW_JY] = pos->tran.y;
+  joints[HW_JU] = pos->u;
+  joints[HW_JV] = pos->v;
+  joints[HW_JB] = pos->b;
+
+  return 0;
+} //unsync_KinematicsInverse
+
+//*********************************************************************
+
+int kinematicsForward(const double *joint,
+                      EmcPose * pos,
+                      const KINEMATICS_FORWARD_FLAGS * fflags,
+                      KINEMATICS_INVERSE_FLAGS * iflags){
+
+  if(*hotwire_swdata->switchkins_type_old != *hotwire_swdata->switchkins_type){
+    use_lastpose[*hotwire_swdata->switchkins_type] = 1;
+    *hotwire_swdata->switchkins_update = !*hotwire_swdata->switchkins_update;
+  }
+
+  *hotwire_swdata->switchkins_type_old = *hotwire_swdata->switchkins_type;
+
+  if(*hotwire_swdata->switchkins_type == KINS_SYNCHRONIZED){
+    sync_KinematicsForward(joint, pos, fflags, iflags);
+  }
+  else{
+    unsync_KinematicsForward(joint, pos, fflags, iflags);
+  }
+
+  return 0;
+} // KinematicsForward()
+
+int kinematicsInverse(const EmcPose * pos,
+                      double *joint,
+                      const KINEMATICS_INVERSE_FLAGS * iflags,
+                      KINEMATICS_FORWARD_FLAGS * fflags){
+
+  if(*hotwire_swdata->switchkins_type_old != *hotwire_swdata->switchkins_type){
+    use_lastpose[*hotwire_swdata->switchkins_type] = 1;
+    *hotwire_swdata->switchkins_update = !*hotwire_swdata->switchkins_update;
+  }
+  *hotwire_swdata->switchkins_type_old = *hotwire_swdata->switchkins_type;
+
+  if(*hotwire_swdata->switchkins_type == KINS_SYNCHRONIZED){
+    sync_KinematicsInverse((EmcPose *) pos, joint, iflags, fflags);
+  }
+  else{
+    unsync_KinematicsInverse((EmcPose *) pos, joint, iflags, fflags);
+  }
+
+  return 0;
+} // kinematicsInverse()
+
+
+
+//*********************************************************************
+int kinematicsSwitchable() {return 1;}
+
+int kinematicsSwitch(int new_switchkins_type){
+
+  for (int k=0; k< HOTWIRE_MAX_TYPES; k++) { use_lastpose[k] = 0;}
+
+  rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE %d\n", *hotwire_swdata->switchkins_type);
+
+  *hotwire_swdata->switchkins_type = new_switchkins_type;
+
+  *hotwire_swdata->kinstype_is_sync = (*hotwire_swdata->switchkins_type == KINS_SYNCHRONIZED) ? 1 : 0;
+  *hotwire_swdata->kinstype_is_unsync = (*hotwire_swdata->switchkins_type == KINS_UNSYNCHRONIZED) ? 1 : 0;
+
+  if(( *hotwire_swdata->kinstype_is_sync + *hotwire_swdata->kinstype_is_unsync ) != 1){
+    rtapi_print_msg(RTAPI_MSG_ERR, "Kins switch invalid value: %d",   *hotwire_swdata->switchkins_type);
+    *hotwire_swdata->kinstype_is_sync = 0;
+    *hotwire_swdata->kinstype_is_unsync = 0;
+    return -1; // FAIL
+  }
+
+  use_lastpose[new_switchkins_type] = 1;
+
+  return 0; // 0==> no error
+} // kinematicsSwitch()
+
+KINEMATICS_TYPE kinematicsType()
+{
+  return KINEMATICS_BOTH;
+}
+
+
+//*********************************************************************
+
+EXPORT_SYMBOL(kinematicsSwitchable);
+EXPORT_SYMBOL(kinematicsSwitch);
+EXPORT_SYMBOL(kinematicsType);
+EXPORT_SYMBOL(kinematicsForward);
+EXPORT_SYMBOL(kinematicsInverse);
+MODULE_LICENSE("GPL");
+
+static int    comp_id;
+
+void rtapi_app_error( char* emsg) {
+
+  rtapi_print_msg(RTAPI_MSG_ERR,
+                  "\nhot Kins FAIL %s:<%s>\n",kp.kinsname,emsg);
+  hal_exit(comp_id);
+  exit(-1);
+}
+
+//*********************************************************************
+int rtapi_app_main(void)
+{
+
+  int res =0;
+
+  kp.kinsname    = "hot-kins"; // !!! must agree with filename
+  kp.halprefix   = "hot-kins"; // hal pin names
+  kp.required_coordinates = "XYUVB";
+  kp.allow_duplicates     = 1;
+  kp.max_joints           = EMCMOT_MAX_JOINTS;
+
+
+  if (kp.max_joints <= 0 || kp.max_joints > EMCMOT_MAX_JOINTS) {
+    rtapi_app_error("Invalid max_joints");
+  }
+
+
+  comp_id = hal_init(kp.kinsname);
+  if(comp_id < 0) rtapi_app_error("Hal Component Failed");
+
+  hotwire_swdata = hal_malloc(sizeof(struct hotwire_swdata));
+  if (!hotwire_swdata) rtapi_app_error("Hal Component Failed");;
+
+
+  rtapi_print_msg(RTAPI_MSG_ERR, "Loading pins setup\n");
+  res += hal_pin_bit_newf(HAL_OUT,    &(hotwire_swdata->kinstype_is_sync),    comp_id, "%s.is-sync",          kp.halprefix);
+  res += hal_pin_bit_newf(HAL_OUT,    &(hotwire_swdata->kinstype_is_unsync),  comp_id, "%s.is-unsync",        kp.halprefix);
+  res += hal_pin_float_newf(HAL_OUT,  &(hotwire_swdata->u_offset),            comp_id, "%s.u-offset",         kp.halprefix);
+  res += hal_pin_float_newf(HAL_OUT,  &(hotwire_swdata->v_offset),            comp_id, "%s.v-offset",         kp.halprefix);
+
+  res += hal_pin_u32_newf(HAL_IN,     &(hotwire_swdata->switchkins_type),     comp_id, "%s.switch-type",      kp.halprefix);
+  res += hal_pin_u32_newf(HAL_IN,     &(hotwire_swdata->switchkins_type_old), comp_id, "%s.switch-type-old",  kp.halprefix);
+  res += hal_pin_bit_newf(HAL_OUT,    &(hotwire_swdata->switchkins_update) ,  comp_id, "%s.switch-update",    kp.halprefix);
+  res += hal_pin_bit_newf(HAL_OUT,    &(hotwire_swdata->error),               comp_id, "%s.error",            kp.halprefix);
+  res += hal_pin_bit_newf(HAL_IN,     &(hotwire_swdata->reset_error),         comp_id, "%s.reset-error",      kp.halprefix);
+
+  if(res != 0){
+    return res;
+  }
+
+  *hotwire_swdata->switchkins_type = 1; // startup with default type
+  *hotwire_swdata->switchkins_type_old = 1;
+
+  kinematicsSwitch(KINS_SYNCHRONIZED);
+
+  hal_ready(comp_id);
+
+  return 0;
+
+} // rtapi_app_main()
+
+void rtapi_app_exit(void) { hal_exit(comp_id); }

src/emc/motion/control.c

@@ -41,6 +41,7 @@ static int    ext_offset_teleop_limit = 0;
 static int    ext_offset_coord_limit  = 0;
 static bool   coord_cubic_active = 0;
 static int    switchkins_type = 0;
+static bool   switchkins_update = 0;
 /* kinematics flags */
 KINEMATICS_FORWARD_FLAGS fflags = 0;
 KINEMATICS_INVERSE_FLAGS iflags = 0;
@@ -300,9 +301,12 @@ static void handle_kinematicsSwitch(void) {
 
     if (!kinematicsSwitchable()) return;
     hal_switchkins_type = (int)*emcmot_hal_data->switchkins_type;
-    if (switchkins_type == hal_switchkins_type) return;
+    bool update_request = (bool)*emcmot_hal_data->switchkins_update;
+
+    if (switchkins_type == hal_switchkins_type && switchkins_update == update_request) return;
 
     switchkins_type = hal_switchkins_type;
+    switchkins_update = update_request;
 
     emcmot_joint_t *jointKinsSwitch;
     double joint_posKinsSwitch[EMCMOT_MAX_JOINTS] = {0,};
@@ -341,6 +345,7 @@ static void handle_kinematicsSwitch(void) {
     }
 #endif
     tpSetPos(&emcmotInternal->coord_tp, &emcmotStatus->carte_pos_cmd);
+    axis_sync_teleop_tp_to_carte_pos(0, pcmd_p);
 } //handle_kinematicsSwitch()
 
 static void process_inputs(void)

src/emc/motion/mot_priv.h

@@ -198,6 +198,8 @@ typedef struct {
     hal_float_t *feed_mm_per_second; /* feed mm per second*/
 
     hal_float_t *switchkins_type;
+    hal_bit_t   *switchkins_update; // Force Kins update on edge
+
 } emcmot_hal_data_t;
 
 /***********************************************************************

src/emc/motion/motion.c

@@ -605,6 +605,7 @@ static int init_hal_io(void)
 
     if (kinematicsSwitchable()) {
         CALL_CHECK(hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->switchkins_type), mot_comp_id, "motion.switchkins-type"));
+        CALL_CHECK(hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->switchkins_update), mot_comp_id, "motion.switchkins-update"));
     }
     /* initialize machine wide pins and parameters */
     *(emcmot_hal_data->adaptive_feed) = 1.0;

src/emc/usr_intf/axis/extensions/emcmodule.cc

@@ -389,7 +389,7 @@ static PyMemberDef Stat_members[] = {
     {(char*)"adaptive_feed_enabled", T_BOOL, O(motion.traj.adaptive_feed_enabled), READONLY},
     {(char*)"feed_hold_enabled", T_BOOL, O(motion.traj.feed_hold_enabled), READONLY},
     {(char*)"num_extrajoints", T_INT, O(motion.numExtraJoints), READONLY},
-
+    {(char*)"jog_is_active", T_BOOL, O(motion.jogging_active), READONLY},
 
 // EMC_SPINDLE_STAT motion.spindle
     // MOVED TO THE "spindle" TUPLE OF DICTS

src/hal/components/limit_axis.comp

@@ -65,11 +65,11 @@ FUNCTION(_){
 
   // Check all ranges
   for (i = 0; i < personality; i++){
-    if (max_range(i) < min_range(i)){
-      if(! enable(i)){
-        continue;
-      }
+    if(! enable(i)){
+      continue;
+    }
 
+    if (max_range(i) < min_range(i)){
       // Throw Error and skip this range
       if (! error_range(i) ){
         rtapi_print_msg(RTAPI_MSG_ERR,