Skip to content

Orient.comp: Add modes with preceding homing to index #4343

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Sigma1912 wants to merge 1 commit into
base: master
Choose a base branch
from
+80 −24
Open

Orient.comp: Add modes with preceding homing to index #4343

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/emc/rs274ngc/interp_check.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -338,8 +338,8 @@ int Interp::check_other_codes(block_pointer block) //!< pointer to a block
CHKS(((motion == G_2 || motion == G_3 || (block->m_modes[7] == 19)) &&
fabs(p_value - block->p_number) > 0.001),
_("P value not an integer with M19 G2 or G3"));
CHKS((block->m_modes[7] == 19) && ((p_value > 2) || p_value < 0),
_("P value must be 0,1,or 2 with M19"));
CHKS((block->m_modes[7] == 19) && ((p_value > 5) || p_value < 0),
_("P value must be 0,1,2,3,4 or 5 with M19"));
CHKS(((motion == G_2 || motion == G_3) && round_to_int(block->p_number) < 1),
_("P value should be 1 or greater with G2 or G3"));
}
Expand Down
100 changes: 78 additions & 22 deletions src/hal/components/orient.comp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,16 +1,21 @@
component orient "Provide a PID command input for orientation mode based on current spindle position, target angle and orient mode";

pin in bool enable "enable angular output for orientation mode";
pin in si32 mode "0: rotate - shortest move; 1: always rotate clockwise; 2: always rotate counterclockwise";
pin in si32 mode "sets rotation direction";
pin in real position "spindle position input, unit 1 rev";
pin in real angle "orient target position in degrees, 0 ≤ angle < 360";
pin out real command "target spindle position, input to PID command";
pin out real poserr "in degrees - aid for PID tuning";
pin out bool is-oriented "This pin goes high when poserr < tolerance. Use to drive spindle.N.is-oriented";
pin in real tolerance = 0.5 "The tolerance in degrees for considering the align completed";
pin in real tolerance = 0.5 "The tolerance in degrees for considering the align completed";
pin io bool index-enable "Connect to the spindle encoder counter to reset to index before orienting";

param r ui32 state "state machine for index homing";

variable int last_enable = 0;
variable int debounce = 0; // to prevent the in-position triggering with the spindle moving
variable rtapi_real target_angle = 0;
variable rtapi_real latched_position = 0;

option period no;

Expand All @@ -36,6 +41,9 @@ The *mode* pin is interpreted as follows:
which may be clockwise or counterclockwise.
* 1: the spindle rotates always rotates clockwise to the new angle.
* 2: the spindle rotates always rotates counterclockwise to the new angle.
* 3: same as mode 0 but with homing to index before orienting to the new angle.
* 4: same as mode 1 but with homing to index before orienting to the new angle.
* 5: same as mode 2 but with homing to index before orienting to the new angle.

=== HAL USAGE

Expand All @@ -62,31 +70,79 @@ license "GPL";

FUNCTION(_) {

rtapi_real target_angle;
rtapi_real latched_position;
rtapi_bool en = enable;

if (!en) state_set(0);
is_oriented_set(0); // spindle.is-oriented inhibits spindle.orient
if (enable) {
if (enable ^ last_enable) { // positive edge on enable
is_oriented_set(0);

switch (state){
case 0: // waiting
if (en && !last_enable) { // positive edge on enable
if (mode > 2){
index_enable_set(1);
state_set(3);
} else {
state_set(1);
}
}
break;
Comment thread
Sigma1912 marked this conversation as resolved.

case 1: // normal orient - init
debounce = 0;
latched_position = position; // sample now
target_angle = angle/360.0;
switch (mode) {
case 0: // shortest move
command_set(floor(latched_position+0.5-target_angle) + target_angle);
break;
case 1: // always cw
command_set(ceil(latched_position-target_angle) + target_angle);
break;
case 2: // always ccw
command_set(floor(latched_position-target_angle) + target_angle );
state_set(2);
break;

case 2: // orienting
switch (mode % 3) {
case 0: // shortest move
command_set(floor(latched_position+0.5-target_angle) + target_angle);
break;
case 1: // always cw
command_set(ceil(latched_position-target_angle) + target_angle);
break;
case 2: // always ccw
command_set(floor(latched_position-target_angle) + target_angle);
break;
default:
rtapi_print_msg(RTAPI_MSG_ERR, "unhandled mode %i\n", mode % 3);
}
poserr_set((position - command) * 360.0);
debounce += (fabs(poserr) < tolerance && debounce <=100);
if (is_oriented_set(debounce > 100)) {
state_set(0);
}
break;

case 3: // index search - init
latched_position = position; // sample now
state_set(4);
break;

case 4: // waiting for spindle index
if (! index_enable){ // index has reset)
state_set(1);
break;
}
}
poserr_set((position - command) * 360.0);
debounce += (fabs(poserr) < tolerance && debounce <=100);
is_oriented_set((debounce > 100));
switch (mode) {
case 3: // shortest move to index
if (0 > latched_position) {
command_set(position + 0.5);
} else {
command_set(position - 0.5);
}
break;
case 4: // always cw to index
command_set(position + 0.5);
break;
case 5: // always ccw to index
command_set(position - 0.5);
break;
default:
rtapi_print_msg(RTAPI_MSG_ERR, "unhandled mode %i\n", mode);
}
break;
}
last_enable = enable;
last_enable = en;
}

Loading