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 (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;

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

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;

if ( absf( angle_scaled_out - angle ) < tolerance ) is_oriented = 1;