TCP 5-axis kinematics

net :tool-offset motion.tooloffset.z xyzac-trt-kins.tool-offset
setp xyzac-trt-kins.y-offset -0.15
setp xyzac-trt-kins.z-offset -27.59
setp xyzac-trt-kins.x-rot-point 407.565
setp xyzac-trt-kins.y-rot-point 154.229
setp xyzac-trt-kins.z-rot-point -228.145

net x-axis-zero-pulse-zero-pulse x.zero.pulse.zero-pulse <=> joint.0.index-enable
net x-axis-zero-pulse-rot-pos    x.zero.pulse.rotary-pos <= lcec.0.EL6751-CanOpen.x-rot-position

component axiszeropulserotposition "generate axis zero pulse from axis rotary position";

//---------------------------------------------------------------

pin io bit zero_pulse "generated rotary zero pulse";
pin in  u32 rotary_pos "axis rotary position";
param rw u32 deadband=10000 "axis rotary position deadband";
variable u32 old_rot_pos=0;
variable int old_dir=0;
variable int pulse_started=0;

//---------------------------------------------------------------

function _;
license "GPL";

;;

FUNCTION(_)
{
  u32 new_rot_pos = rotary_pos;
  if (new_rot_pos != old_rot_pos) {
		int new_dir=0;
		int over_deadband=0;

		if (new_rot_pos<old_rot_pos) {
			new_dir=-1;
			over_deadband=(old_rot_pos-new_rot_pos)>deadband;
		} else if (new_rot_pos>old_rot_pos) {
			new_dir=1;
			over_deadband=(new_rot_pos-old_rot_pos)>deadband;
		}

		if (over_deadband) { // skip 'deadband' size jumps in any direction
      if (zero_pulse && old_dir && new_dir && old_dir!=new_dir)
         zero_pulse = 0;
	 	  if (new_dir) { // exclude same reading cases
	 	    old_dir = new_dir;
        old_rot_pos = new_rot_pos;
      }
    }
  }
}

case HOME_SET_INDEX_POSITION:
		/* This state is called when the encoder has been reset at
		   the index pulse position.  It sets the current joint 
		   position to 'home_offset', which is the location of the
		   index pulse in joint coordinates. */
		/* set the current position to 'home_offset' */
		joint->motor_offset = -joint->home_offset;
		joint->pos_fb = joint->motor_pos_fb -
		    (joint->backlash_filt + joint->motor_offset);
		joint->pos_cmd = joint->pos_fb;
		joint->free_tp.curr_pos = joint->pos_fb;


		// PL ++
		/* this moves the internal position but does not affect the
		   motor position */
		offset = joint->home_offset - joint->pos_fb;
		joint->pos_cmd += offset;
		joint->pos_fb += offset;
		joint->free_tp.curr_pos += offset;
		joint->motor_offset -= offset;
		// -- PL

		/* next state */
		joint->home_state = HOME_FINAL_MOVE_START;
		immediate_state = 1;
		break;