Orient component revised

// 100% sure someone can write this calc more cleaner...
       rev = position;                 // converting to int for extract full rev only for internal calc
       tempcalc = (position - rev) * 360.0;      // converting to degree in a -360 to 360 range 
                                             // converting to degree in 0 to 360 to 0 range
       if (tempcalc < 0) scaled_angle_out = 360 + tempcalc; else scaled_angle_out = tempcalc;
       angle_deviation = angle_scaled_out - angle;
// 100% sure someone can write this calc more cleaner...

                    if (angle_deviation < 0 && fabs angle_deviation > 180) cmd_to_pid_orient = -vel_speed; else cmd_to_pid_orient = vel_speed;
                    if (angle_deviation > 0 && fabs angle_deviation > 180) cmd_to_pid_orient = vel_speed; else cmd_to_pid_orient = vel_speed;

#####################################################################
# Spindle orient mode
#####################################################################
loadrt orientv4 names=orient.orient-spindle
loadrt pid      names=pid.orient-spindle

addf orient.orient-spindle                  servo-thread
addf pid.orient-spindle.do-pid-calcs        servo-thread

# setup Orient Spindle PID
setp pid.orient-spindle.Pgain        [ORIENT]P
setp pid.orient-spindle.Igain        [ORIENT]I
setp pid.orient-spindle.Dgain        [ORIENT]D
setp pid.orient-spindle.FF0          [ORIENT]FF0
setp pid.orient-spindle.FF1          [ORIENT]FF1
setp pid.orient-spindle.FF2          [ORIENT]FF2
setp pid.orient-spindle.bias         [ORIENT]BIAS
setp pid.orient-spindle.deadband     [ORIENT]DEADBAND
setp pid.orient-spindle.maxerror     [ORIENT]MAX_ERROR
setp pid.orient-spindle.maxerrorI    [ORIENT]MAX_ERROR_I
setp pid.orient-spindle.maxoutput    [ORIENT]MAX_OUTPUT

setp orient.orient-spindle.vel-speed [ORIENT]VEL_SPEED
setp orient.orient-spindle.tolerance [ORIENT]AT_ANGLE_TOL

# encoder connections feedback
net spindle-pos          => orient.orient-spindle.position
net spindle-pos-deg      <= orient.orient-spindle.scaled-angle-out

# Index connection
net spindle-index-enable <=> orient.orient-spindle.index-enable
net spindle-index-enable  => pid.orient-spindle.index-enable

# Spindle homing link for homing spindle with machine homing
net machinehomed <= joint.0.homed
net machinehomed => orient.orient-spindle.machine-is-homed

# Set asked mode from M19 P0/1/2
net spindle-orient-mode <= spindle.0.orient-mode
net spindle-orient-mode => orient.orient-spindle.mode

# If spindle is not homed report error and cancel orient process
net spindle-orient-error <= orient.orient-spindle.error-not-homed
net spindle-orient-error => spindle.0.orient-fault 

# Spindle orient enable control signals
net spindle-orient-enable     <= spindle.0.orient
net spindle-orient-enable     => orient.orient-spindle.enable

# Pid enable from orient component 
net spindle-orient-pid-ena <= orient.orient-spindle.ena-out-pid-muxed
net spindle-orient-pid-ena => pid.orient-spindle.enable

# enable out for controler from regular spindle or orient mode
net spindle-enable             => orient.orient-spindle.ena-from-velocity
net spindle-orient-enable-out  <= orient.orient-spindle.ena-out-muxed
# moved in the orient input/output but initially in spindle setup
net spindle-orient-enable-out  => [HMOT](CARD0).7i83.0.0.analogena0 

# Is oriented near signal + link position asked to component
net spindle-orient-degree-cmd <= spindle.0.orient-angle
net spindle-orient-degree-cmd => orient.orient-spindle.angle
net spindle-is-oriented <= orient.orient-spindle.is-oriented
net spindle-is-oriented => spindle.0.is-oriented

# Command to pid from component by velocity and velocity feedback
net spindle-orient-vel   <= orient.orient-spindle.cmd-to-pid-orient
net spindle-orient-vel   => pid.orient-spindle.command
net spindle-vel-rpm      => pid.orient-spindle.feedback                # feedback velocity mode

# Input speed velocity from normal mode inside component
# the component passthru the spindle velocity to the Card signal generator or send orient velocity
net spindle-orient-pid-in-out <= pid.orient-spindle.output
net spindle-orient-pid-in-out => orient.orient-spindle.cmd-from-pid-orient
net spindle-vel-cmd-out       => orient.orient-spindle.cmd-from-pid-velocity
net spindle-orient-ctrl-out   <= orient.orient-spindle.cmd-out-muxed
 # moved in the orient input/output but initially in spindle setup
net spindle-orient-ctrl-out   => [HMOT](CARD0).7i83.0.0.analogout0

                if (fabs(angle_deviation) > 180){
                    if (angle_deviation > 0){
                        cmd_to_pid_orient = vel_speed;
                    } else {
                        cmd_to_pid_orient = -vel_speed;
                    }
                } else {
                    if (angle_deviation > 0){
                        cmd_to_pid_orient = -vel_speed;
                    } else {
                        cmd_to_pid_orient = vel_speed;
                    }
                }

                if (angle_deviation > 180){
                        cmd_to_pid_orient = vel_speed;
                        ashortest_move = 1;
                        ashortest_move2 = angle_deviation;
                } else {
                     if (angle_deviation > 0){
                        cmd_to_pid_orient = -vel_speed;
                        ashortest_move = 2;
                        ashortest_move2 = angle_deviation;
                     } else { 
                          if (angle_deviation > -180){
                             cmd_to_pid_orient = vel_speed;
                        ashortest_move = 3;
                        ashortest_move2 = angle_deviation;
                          } else { 
                              if (angle_deviation < -180 ){
                                 cmd_to_pid_orient = -vel_speed;
                        ashortest_move = 4;
                        ashortest_move2 = angle_deviation;
                                     }                        
                                 }
                             }
                        }

if (angle_scaled_out <= angle+tolerance && angle_scaled_out >= angle-tolerance) is_oriented = 1;