Mori Seiki TV30 tool changer help needed

int current_pocket = round(motor_pos_fb) % 10;
if (current_pocket < 0) current_pocket += 10; // make it 0..9
int move_dist = new_pocket - current_pocket;
if (move_dist > 5)
	move_dist -= 10; // go back instead
else if (move_dist < -5)
	move_dist += 10; // go forward
else if (move_dist == 5 or move_dist == -5) // can move either way
	move_dist = motor_pos_fb > 0 : -5 : 5; // go towards the origin

double new_motor_pos = round(motor_pos_fb) + move_dist;

pin in int pocket;
pin out bit tool_changed;

component utc;
pin in signed pocket;
pin in float tc-pos-fb;
pin out float tc-pos-cmd;
pin out bit ready;
function _;
;;
tc_pos_cmd = pocket * 10.0; // e.g. 10 units between pockets
ready = fabs(tc_pos_cmd - tc_pos_fb) < 0.01;

loadrt utc
loadrt limit3
addf utc.0 servo-thread
addf limit3.0 servo-thread

net tc-pos-fb utc.0.tc-pos-fb <= nyx.0.servo-03.pos-fb
net tc-pos-cmd utc.0.tc-pos-cmd => limit3.0.in
net tc-pos-cmd-limited limit3.0.out => nyx.0.servo-03.pos-cmd
setp limit3.maxv 10.0
setp limit3.maxa 0.1

component tv30atc;
pin in signed req_pocket;
pin in float tc_pos_fb;
pin out float tc_pos_cmd;
pin out float ready;
license "GPL"; // indicates GPL v2 or later
function _;
;;
#include <math.h>
int move_dist;
int current_pocket = round(tc_pos_fb); // current pocket is rounded position of motor
if (current_pocket < 10) {
	current_pocket += 10; // make it 0..9
	move_dist = req_pocket - current_pocket;
}
if (current_pocket >= 10) {
	current_pocket -= 10;
	move_dist = req_pocket - current_pocket;
}
if (move_dist > 5) {
	move_dist -= 10; // go back instead
}
else if (move_dist < -5) {
	move_dist += 10; // go forward
}
else if (move_dist == 5 && tc_pos_fb > 0) { // can move either way
	move_dist = tc_pos_fb -5; // go towards the origin
}
else if (move_dist == -5 && tc_pos_fb > 0) {
	move_dist = tc_pos_fb +5;
}
tc_pos_cmd = tc_pos_fb + move_dist;
ready = fabs(current_pocket - tc_pos_fb) < 0.01; // when tc position near motor feedback position then signal ready