void do_homing(void)
{
int joint_num;
emcmot_joint_t *joint;
double offset, tmp;
int home_sw_active, homing_flag;
homing_flag = 0;
if (emcmotStatus->motion_state != EMCMOT_MOTION_FREE) {
/* can't home unless in free mode */
return;
}
/* loop thru joints, treat each one individually */
for (joint_num = 0; joint_num < emcmotConfig->numJoints; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
if (!GET_JOINT_ACTIVE_FLAG(joint)) {
/* if joint is not active, skip it */
continue;
}
home_sw_active = GET_JOINT_HOME_SWITCH_FLAG(joint);
if (joint->home_state != HOME_IDLE) {
homing_flag = 1; /* at least one joint is homing */
}
/* when an joint is homing, 'check_for_faults()' ignores its limit
switches, so that this code can do the right thing with them. Once
the homing process is finished, the 'check_for_faults()' resumes
checking */
/* homing state machine */
/* Some portions of the homing sequence can run thru two or more
states during a single servo period. This is done using
'immediate_state'. If a state transition sets it true (non-zero),
this 'do-while' will loop executing switch(home_state) immediately
to run the new state code. Otherwise, the loop will fall thru, and
switch(home_state) runs only once per servo period. Do _not_ set
'immediate_state' true unless you also change 'home_state', unless
you want an infinite loop! */
do {
immediate_state = 0;
switch (joint->home_state) {
case HOME_IDLE:
/* nothing to do */
break;
case HOME_START:
/* This state is responsible for getting the homing process
started. It doesn't actually do anything, it simply
determines what state is next */
if (joint->home_flags & HOME_IS_SHARED && home_sw_active) {
reportError(
_("Cannot home while shared home switch is closed %d"),joint_num);
joint->home_state = HOME_IDLE;
break;
}
/* set flags that communicate with the rest of EMC */
if ( (joint->home_flags & HOME_NO_REHOME)
&& GET_JOINT_HOMED_FLAG(joint)
) {
joint->home_state = HOME_IDLE;
break; //no rehome allowed if absolute_enoder
} else {
SET_JOINT_HOMING_FLAG(joint, 1);
SET_JOINT_HOMED_FLAG(joint, 0);
}
SET_JOINT_AT_HOME_FLAG(joint, 0);
/* stop any existing motion */
joint->free_tp.enable = 0;
/* reset delay counter */
joint->home_pause_timer = 0;
/* figure out exactly what homing sequence is needed */
if (joint->home_flags & HOME_ABSOLUTE_ENCODER) {
joint->home_flags &= ~HOME_IS_SHARED; // shared not applicable
joint->home_state = HOME_SET_SWITCH_POSITION;
immediate_state = 1;
break;
}
if (joint->home_flags & HOME_UNLOCK_FIRST) {
joint->home_state = HOME_UNLOCK;
} else {
joint->home_state = HOME_UNLOCK_WAIT;
immediate_state = 1;
}
break;
case HOME_UNLOCK:
// unlock now
emcmotSetRotaryUnlock(joint_num, 1);
joint->home_state = HOME_UNLOCK_WAIT;
break;
case HOME_UNLOCK_WAIT:
// if not yet unlocked, continue waiting
if ((joint->home_flags & HOME_UNLOCK_FIRST) &&
!emcmotGetRotaryIsUnlocked(joint_num)) break;
// either we got here without an unlock needed, or the
// unlock is now complete.
if (joint->home_search_vel == 0.0) {
if (joint->home_latch_vel == 0.0) {
/* both vels == 0 means home at current position */
joint->home_state = HOME_SET_SWITCH_POSITION;
immediate_state = 1;
} else if (joint->home_flags & HOME_USE_INDEX) {
/* home using index pulse only */
joint->home_state = HOME_INDEX_ONLY_START;
immediate_state = 1;
} else {
reportError(_("invalid homing config: non-zero LATCH_VEL needs either SEARCH_VEL or USE_INDEX"));
joint->home_state = HOME_IDLE;
}
} else {
if (joint->home_latch_vel != 0.0) {
/* need to find home switch */
joint->home_state = HOME_INITIAL_SEARCH_START;
immediate_state = 1;
} else {
reportError(_("invalid homing config: non-zero SEARCH_VEL needs LATCH_VEL"));
joint->home_state = HOME_IDLE;
}
}
break;
case HOME_INITIAL_BACKOFF_START:
/* This state is called if the homing sequence starts at a
location where the home switch is already tripped. It
starts a move away from the switch. */
/* is the joint still moving? */
if (joint->free_tp.active) {
/* yes, reset delay, wait until joint stops */
joint->home_pause_timer = 0;
break;
}
/* has delay timed out? */
if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
/* no, update timer and wait some more */
joint->home_pause_timer++;
break;
}
joint->home_pause_timer = 0;
/* set up a move at '-search_vel' to back off of switch */
home_start_move(joint, -joint->home_search_vel);
/* next state */
joint->home_state = HOME_INITIAL_BACKOFF_WAIT;
break;
case HOME_INITIAL_BACKOFF_WAIT:
/* This state is called while the machine is moving off of
the home switch. It terminates when the switch is cleared
successfully. If the move ends or hits a limit before it
clears the switch, the home is aborted. */
/* are we off home switch yet? */
if (! home_sw_active) {
/* yes, stop motion */
joint->free_tp.enable = 0;
/* begin initial search */
joint->home_state = HOME_INITIAL_SEARCH_START;
immediate_state = 1;
break;
}
home_do_moving_checks(joint);
break;
case HOME_INITIAL_SEARCH_START:
/* This state is responsible for starting a move toward the
home switch. This move is at 'search_vel', which can be
fairly fast, because once the switch is found another
slower move will be used to set the exact home position. */
/* is the joint already moving? */
if (joint->free_tp.active) {
/* yes, reset delay, wait until joint stops */
joint->home_pause_timer = 0;
break;
}
/* has delay timed out? */
if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
/* no, update timer and wait some more */
joint->home_pause_timer++;
break;
}
joint->home_pause_timer = 0;
/* make sure we aren't already on home switch */
if (home_sw_active) {
/* already on switch, need to back off it first */
joint->home_state = HOME_INITIAL_BACKOFF_START;
immediate_state = 1;
break;
}
/* set up a move at 'search_vel' to find switch */
home_start_move(joint, joint->home_search_vel);
/* next state */
joint->home_state = HOME_INITIAL_SEARCH_WAIT;
break;
case HOME_INITIAL_SEARCH_WAIT:
/* This state is called while the machine is looking for the
home switch. It terminates when the switch is found. If
the move ends or hits a limit before it finds the switch,
the home is aborted. */
/* have we hit home switch yet? */
if (home_sw_active) {
/* yes, stop motion */
joint->free_tp.enable = 0;
/* go to next step */
joint->home_state = HOME_SET_COARSE_POSITION;
immediate_state = 1;
break;
}
home_do_moving_checks(joint);
break;
case HOME_SET_COARSE_POSITION:
/* This state is called after the first time the switch is
found. At this point, we are approximately home. Although
we will do another slower pass to get the exact home
location, we reset the joint coordinates now so that screw
error comp will be appropriate for this portion of the
screw (previously we didn't know where we were at all). */
/* set the current position to 'home_offset' */
offset = joint->home_offset - joint->pos_fb;
/* this moves the internal position but does not affect the
motor position */
joint->pos_cmd += offset;
joint->pos_fb += offset;
joint->free_tp.curr_pos += offset;
joint->motor_offset -= offset;
/* The next state depends on the signs of 'search_vel' and
'latch_vel'. If they are the same, that means we must
back up, then do the final homing moving the same
direction as the initial search, on a rising edge of the
switch. If they are opposite, it means that the final
homing will take place on a falling edge as the machine
moves off of the switch. */
tmp = joint->home_search_vel * joint->home_latch_vel;
if (tmp > 0.0) {
/* search and latch vel are same direction */
joint->home_state = HOME_FINAL_BACKOFF_START;
} else {
/* search and latch vel are opposite directions */
joint->home_state = HOME_FALL_SEARCH_START;
}
immediate_state = 1;
break;
case HOME_FINAL_BACKOFF_START:
/* This state is called once the approximate location of the
switch has been found. It is responsible for starting a
move that will back off of the switch in preparation for a
final slow move that captures the exact switch location. */
/* is the joint already moving? */
if (joint->free_tp.active) {
/* yes, reset delay, wait until joint stops */
joint->home_pause_timer = 0;
break;
}
/* has delay timed out? */
if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
/* no, update timer and wait some more */
joint->home_pause_timer++;
break;
}
joint->home_pause_timer = 0;
/* we should still be on the switch */
if (! home_sw_active) {
reportError(
_("Home switch inactive before start of backoff move"));
joint->home_state = HOME_IDLE;
break;
}
/* set up a move at '-search_vel' to back off of switch */
home_start_move(joint, -joint->home_search_vel);
/* next state */
joint->home_state = HOME_FINAL_BACKOFF_WAIT;
break;
case HOME_FINAL_BACKOFF_WAIT:
/* This state is called while the machine is moving off of
the home switch after finding its approximate location.
It terminates when the switch is cleared successfully. If
the move ends or hits a limit before it clears the switch,
the home is aborted. */
/* are we off home switch yet? */
if (! home_sw_active) {
/* yes, stop motion */
joint->free_tp.enable = 0;
/* begin final search */
joint->home_state = HOME_RISE_SEARCH_START;
immediate_state = 1;
break;
}
home_do_moving_checks(joint);
break;
case HOME_RISE_SEARCH_START:
/* This state is called to start the final search for the
point where the home switch trips. It moves at
'latch_vel' and looks for a rising edge on the switch */
/* is the joint already moving? */
if (joint->free_tp.active) {
/* yes, reset delay, wait until joint stops */
joint->home_pause_timer = 0;
break;
}
/* has delay timed out? */
if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
/* no, update timer and wait some more */
joint->home_pause_timer++;
break;
}
joint->home_pause_timer = 0;
/* we should still be off of the switch */
if (home_sw_active) {
reportError(
_("Home switch active before start of latch move"));
joint->home_state = HOME_IDLE;
break;
}
/* set up a move at 'latch_vel' to locate the switch */
home_start_move(joint, joint->home_latch_vel);
/* next state */
joint->home_state = HOME_RISE_SEARCH_WAIT;
break;
case HOME_RISE_SEARCH_WAIT:
/* This state is called while the machine is moving towards
the home switch on its final, low speed pass. It
terminates when the switch is detected. If the move ends
or hits a limit before it hits the switch, the home is
aborted. */
/* have we hit the home switch yet? */
if (home_sw_active) {
/* yes, where do we go next? */
if (joint->home_flags & HOME_USE_INDEX) {
/* look for index pulse */
joint->home_state = HOME_INDEX_SEARCH_START;
immediate_state = 1;
break;
} else {
/* no index pulse, stop motion */
joint->free_tp.enable = 0;
/* go to next step */
joint->home_state = HOME_SET_SWITCH_POSITION;
immediate_state = 1;
break;
}
}
home_do_moving_checks(joint);
break;
case HOME_FALL_SEARCH_START:
/* This state is called to start the final search for the
point where the home switch releases. It moves at
'latch_vel' and looks for a falling edge on the switch */
/* is the joint already moving? */
if (joint->free_tp.active) {
/* yes, reset delay, wait until joint stops */
joint->home_pause_timer = 0;
break;
}
/* has delay timed out? */
if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
/* no, update timer and wait some more */
joint->home_pause_timer++;
break;
}
joint->home_pause_timer = 0;
/* we should still be on the switch */
if (!home_sw_active) {
reportError(
_("Home switch inactive before start of latch move"));
joint->home_state = HOME_IDLE;
break;
}
/* set up a move at 'latch_vel' to locate the switch */
home_start_move(joint, joint->home_latch_vel);
/* next state */
joint->home_state = HOME_FALL_SEARCH_WAIT;
break;
case HOME_FALL_SEARCH_WAIT:
/* This state is called while the machine is moving away from
the home switch on its final, low speed pass. It
terminates when the switch is cleared. If the move ends or
hits a limit before it clears the switch, the home is
aborted. */
/* have we cleared the home switch yet? */
if (!home_sw_active) {
/* yes, where do we go next? */
if (joint->home_flags & HOME_USE_INDEX) {
/* look for index pulse */
joint->home_state = HOME_INDEX_SEARCH_START;
immediate_state = 1;
break;
} else {
/* no index pulse, stop motion */
joint->free_tp.enable = 0;
/* go to next step */
joint->home_state = HOME_SET_SWITCH_POSITION;
immediate_state = 1;
break;
}
}
home_do_moving_checks(joint);
break;
case HOME_SET_SWITCH_POSITION:
/* This state is called when the machine has determined the
switch position as accurately as possible. It sets the
current joint position to 'home_offset', which is the
location of the home switch in joint coordinates. */
/* set the current position to 'home_offset' */
if (joint->home_flags & HOME_ABSOLUTE_ENCODER) {
offset = joint->home_offset;
} else {
offset = joint->home_offset - joint->pos_fb;
}
/* this moves the internal position but does not affect the
motor position */
joint->pos_cmd += offset;
joint->pos_fb += offset;
joint->free_tp.curr_pos += offset;
joint->motor_offset -= offset;
if (joint->home_flags & HOME_ABSOLUTE_ENCODER) {
if (joint->home_flags & HOME_NO_FINAL_MOVE) {
joint->home_state = HOME_FINISHED;
immediate_state = 1;
break;
}
}
/* next state */
joint->home_state = HOME_FINAL_MOVE_START;
immediate_state = 1;
break;
case HOME_INDEX_ONLY_START:
/* This state is used if the machine has been pre-positioned
near the home position, and simply needs to find the
next index pulse. It starts a move at latch_vel, and
sets index-enable, which tells the encoder driver to
reset its counter to zero and clear the enable when the
next index pulse arrives. */
/* is the joint already moving? */
if (joint->free_tp.active) {
/* yes, reset delay, wait until joint stops */
joint->home_pause_timer = 0;
break;
}
/* has delay timed out? */
if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
/* no, update timer and wait some more */
joint->home_pause_timer++;
break;
}
joint->home_pause_timer = 0;
/* Although we don't know the exact home position yet, we
we reset the joint coordinates now so that screw error
comp will be appropriate for this portion of the screw
(previously we didn't know where we were at all). */
/* set the current position to 'home_offset' */
offset = joint->home_offset - joint->pos_fb;
/* this moves the internal position but does not affect the
motor position */
joint->pos_cmd += offset;
joint->pos_fb += offset;
joint->free_tp.curr_pos += offset;
joint->motor_offset -= offset;
/* set the index enable */
joint->index_enable = 1;
/* set up a move at 'latch_vel' to find the index pulse */
home_start_move(joint, joint->home_latch_vel);
/* next state */
joint->home_state = HOME_INDEX_SEARCH_WAIT;
break;
case HOME_INDEX_SEARCH_START:
/* This state is called after the machine has made a low
speed pass to determine the limit switch location. It
sets index-enable, which tells the encoder driver to
reset its counter to zero and clear the enable when the
next index pulse arrives. */
/* set the index enable */
joint->index_enable = 1;
/* and move right into the waiting state */
joint->home_state = HOME_INDEX_SEARCH_WAIT;
immediate_state = 1;
home_do_moving_checks(joint);
break;
case HOME_INDEX_SEARCH_WAIT:
/* This state is called after the machine has found the
home switch and "armed" the encoder counter to reset on
the next index pulse. It continues at low speed until
an index pulse is detected, at which point it sets the
final home position. If the move ends or hits a limit
before an index pulse occurs, the home is aborted. */
/* has an index pulse arrived yet? encoder driver clears
enable when it does */
if ( joint->index_enable == 0 ) {
/* yes, stop motion */
joint->free_tp.enable = 0;
/* go to next step */
joint->home_state = HOME_SET_INDEX_POSITION;
immediate_state = 1;
break;
}
home_do_moving_checks(joint);
break;
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;
/* next state */
joint->home_state = HOME_FINAL_MOVE_START;
immediate_state = 1;
break;
case HOME_FINAL_MOVE_START:
/* This state is called once the joint coordinate system is
set properly. It moves to the actual 'home' position,
which is not neccessarily the position of the home switch
or index pulse. */
/* is the joint already moving? */
if (joint->free_tp.active) {
/* yes, reset delay, wait until joint stops */
joint->home_pause_timer = 0;
break;
}
/* has delay timed out? */
if (joint->home_pause_timer < (HOME_DELAY * servo_freq)) {
/* no, update timer and wait some more */
joint->home_pause_timer++;
if (joint->home_sequence<0) {
if (!sync_final_move[home_sequence]) break;
} else {
break;
}
}
// negative joint->home_sequence means sync final move
// defer final move until all joints in sequence are ready
if ( (joint->home_sequence < 0)
&& ( ABS(joint->home_sequence) == home_sequence)
) {
if (!sync_final_move[home_sequence]) {
int jno;
emcmot_joint_t *jtmp;
sync_final_move[home_sequence] = 1; //disprove
for (jno = 0; jno < emcmotConfig->numJoints; jno++) {
jtmp = &joints[jno];
if (ABS(jtmp->home_sequence) != home_sequence) {continue;}
if (jtmp->home_flags & HOME_ABSOLUTE_ENCODER) {continue;}
if ( (jtmp->home_state != HOME_FINAL_MOVE_START)
||
(jtmp->free_tp.active)
) {
sync_final_move[home_sequence] = 0;
break;
}
}
if (!sync_final_move[home_sequence]) break;
}
}
joint->home_pause_timer = 0;
/* plan a move to home position */
joint->free_tp.pos_cmd = joint->home;
/* if home_vel is set (>0) then we use that, otherwise we rapid there */
if (joint->home_final_vel > 0) {
joint->free_tp.max_vel = fabs(joint->home_final_vel);
/* clamp on max vel for this joint */
if (joint->free_tp.max_vel > joint->vel_limit)
joint->free_tp.max_vel = joint->vel_limit;
} else {
joint->free_tp.max_vel = joint->vel_limit;
}
/* start the move */
joint->free_tp.enable = 1;
joint->home_state = HOME_FINAL_MOVE_WAIT;
break;
case HOME_FINAL_MOVE_WAIT:
/* This state is called while the machine makes its final
move to the home position. It terminates when the machine
arrives at the final location. If the move hits a limit
before it arrives, the home is aborted. */
/* have we arrived (and stopped) at home? */
if (!joint->free_tp.active) {
/* yes, stop motion */
joint->free_tp.enable = 0;
/* we're finally done */
joint->home_state = HOME_LOCK;
immediate_state = 1;
break;
}
if (joint->on_pos_limit || joint->on_neg_limit) {
/* on limit, check to see if we should trip */
if (!(joint->home_flags & HOME_IGNORE_LIMITS)) {
/* not ignoring limits, time to quit */
reportError(_("hit limit in home state"));
joint->home_state = HOME_ABORT;
immediate_state = 1;
break;
}
}
break;
case HOME_LOCK:
if (joint->home_flags & HOME_UNLOCK_FIRST) {
emcmotSetRotaryUnlock(joint_num, 0);
} else {
immediate_state = 1;
}
joint->home_state = HOME_LOCK_WAIT;
break;
case HOME_LOCK_WAIT:
// if not yet locked, continue waiting
if ((joint->home_flags & HOME_UNLOCK_FIRST) &&
emcmotGetRotaryIsUnlocked(joint_num)) break;
// either we got here without a lock needed, or the
// lock is now complete.
joint->home_state = HOME_FINISHED;
immediate_state = 1;
break;
case HOME_FINISHED:
SET_JOINT_HOMING_FLAG(joint, 0);
SET_JOINT_HOMED_FLAG(joint, 1);
SET_JOINT_AT_HOME_FLAG(joint, 1);
joint->home_state = HOME_IDLE;
immediate_state = 1;
// This joint just finished homing. See if this is the
// final one and all joints are now homed, and switch to
// Teleop mode if so.
if (checkAllHomed()) {
switch_to_teleop_mode();
homing_flag = 0;
}
break;
case HOME_ABORT:
SET_JOINT_HOMING_FLAG(joint, 0);
SET_JOINT_HOMED_FLAG(joint, 0);
SET_JOINT_AT_HOME_FLAG(joint, 0);
joint->free_tp.enable = 0;
joint->home_state = HOME_IDLE;
joint->index_enable = 0;
immediate_state = 1;
break;
default:
/* should never get here */
reportError(_("unknown state '%d' during homing"),
joint->home_state);
joint->home_state = HOME_ABORT;
immediate_state = 1;
break;
} /* end of switch(joint->home_state) */
} while (immediate_state);
} /* end of loop through all joints */
if ( homing_flag ) {
/* at least one joint is homing, set global flag */
emcmotStatus->homing_active = 1;
} else {
/* is a homing sequence in progress? */
if (emcmotStatus->homingSequenceState == HOME_SEQUENCE_IDLE) {
/* no, single joint only, we're done */
emcmotStatus->homing_active = 0;
}
}
}
/*
emcmotCommandHandler() is called each main cycle to read the
shared memory buffer
*/
void emcmotCommandHandler(void *arg, long period)
{
int joint_num;
emcmot_joint_t *joint;
double tmp1;
emcmot_comp_entry_t *comp_entry;
check_stuff ( "before command_handler()" );
/* check for split read */
if (emcmotCommand->head != emcmotCommand->tail) {
emcmotDebug->split++;
return; /* not really an error */
}
if (emcmotCommand->commandNum != emcmotStatus->commandNumEcho) {
/* increment head count-- we'll be modifying emcmotStatus */
emcmotStatus->head++;
emcmotDebug->head++;
/* got a new command-- echo command and number... */
emcmotStatus->commandEcho = emcmotCommand->command;
emcmotStatus->commandNumEcho = emcmotCommand->commandNum;
/* clear status value by default */
emcmotStatus->commandStatus = EMCMOT_COMMAND_OK;
/* ...and process command */
/* Many commands uses "command->axis" to indicate which joint they
wish to operate on. This code eliminates the need to copy
command->axis to "joint_num", limit check it, and then set "joint"
to point to the joint data. All the individual commands need to do
is verify that "joint" is non-zero. */
joint_num = emcmotCommand->axis;
if (joint_num >= 0 && joint_num < num_joints) {
/* valid joint, point to it's data */
joint = &joints[joint_num];
} else {
/* bad joint number */
joint = 0;
}
/* printing of commands for troubleshooting */
rtapi_print_msg(RTAPI_MSG_DBG, "%d: CMD %d, code %3d ", emcmotStatus->heartbeat,
emcmotCommand->commandNum, emcmotCommand->command);
switch (emcmotCommand->command) {
case EMCMOT_ABORT:
/* abort motion */
/* can happen at any time */
/* this command attempts to stop all machine motion. it looks at
the current mode and acts accordingly, if in teleop mode, it
sets the desired velocities to zero, if in coordinated mode,
it calls the traj planner abort function (don't know what that
does yet), and if in free mode, it disables the free mode traj
planners which stops axis motion */
rtapi_print_msg(RTAPI_MSG_DBG, "ABORT");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", emcmotCommand->axis);
/* check for coord or free space motion active */
if (GET_MOTION_TELEOP_FLAG()) {
emcmotDebug->teleop_data.desiredVel.tran.x = 0.0;
emcmotDebug->teleop_data.desiredVel.tran.y = 0.0;
emcmotDebug->teleop_data.desiredVel.tran.z = 0.0;
emcmotDebug->teleop_data.desiredVel.a = 0.0;
emcmotDebug->teleop_data.desiredVel.b = 0.0;
emcmotDebug->teleop_data.desiredVel.c = 0.0;
} else if (GET_MOTION_COORD_FLAG()) {
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(0);
} else {
for (joint_num = 0; joint_num < num_joints; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* tell joint planner to stop */
joint->free_tp_enable = 0;
/* stop homing if in progress */
if ( joint->home_state != HOME_IDLE ) {
joint->home_state = HOME_ABORT;
}
}
}
/* clear axis errors (regardless of mode */
for (joint_num = 0; joint_num < num_joints; joint_num++) {
/* point to joint struct */
joint = &joints[joint_num];
/* update status flags */
SET_JOINT_ERROR_FLAG(joint, 0);
SET_JOINT_FAULT_FLAG(joint, 0);
}
break;
case EMCMOT_AXIS_ABORT:
/* abort one axis */
/* can happen at any time */
/* this command stops a single axis. It is only usefull
in free mode, so in coord or teleop mode it does
nothing. */
rtapi_print_msg(RTAPI_MSG_DBG, "AXIS_ABORT");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", emcmotCommand->axis);
if (GET_MOTION_TELEOP_FLAG()) {
/* do nothing in teleop mode */
} else if (GET_MOTION_COORD_FLAG()) {
/* do nothing in coord mode */
} else {
/* validate joint */
if (joint == 0) {
break;
}
/* tell joint planner to stop */
joint->free_tp_enable = 0;
/* stop homing if in progress */
if ( joint->home_state != HOME_IDLE ) {
joint->home_state = HOME_ABORT;
}
/* update status flags */
SET_JOINT_ERROR_FLAG(joint, 0);
}
break;
case EMCMOT_FREE:
/* change the mode to free axis motion */
/* can be done at any time */
/* this code doesn't actually make the transition, it merely
requests the transition by clearing a couple of flags */
/* reset the emcmotDebug->coordinating flag to defer transition
to controller cycle */
rtapi_print_msg(RTAPI_MSG_DBG, "FREE");
emcmotDebug->coordinating = 0;
emcmotDebug->teleoperating = 0;
break;
case EMCMOT_COORD:
/* change the mode to coordinated axis motion */
/* can be done at any time */
/* this code doesn't actually make the transition, it merely
tests a condition and then sets a flag requesting the
transition */
/* set the emcmotDebug->coordinating flag to defer transition to
controller cycle */
rtapi_print_msg(RTAPI_MSG_DBG, "COORD");
emcmotDebug->coordinating = 1;
emcmotDebug->teleoperating = 0;
if (kinType != KINEMATICS_IDENTITY) {
if (!checkAllHomed()) {
reportError
("all axes must be homed before going into coordinated mode");
emcmotDebug->coordinating = 0;
break;
}
}
break;
case EMCMOT_TELEOP:
/* change the mode to teleop motion */
/* can be done at any time */
/* this code doesn't actually make the transition, it merely
tests a condition and then sets a flag requesting the
transition */
/* set the emcmotDebug->teleoperating flag to defer transition to
controller cycle */
rtapi_print_msg(RTAPI_MSG_DBG, "TELEOP");
emcmotDebug->teleoperating = 1;
if (kinType != KINEMATICS_IDENTITY) {
if (!checkAllHomed()) {
reportError
("all axes must be homed before going into teleop mode");
emcmotDebug->teleoperating = 0;
break;
}
}
break;
case EMCMOT_SET_NUM_AXES:
/* set the global NUM_AXES, which must be between 1 and
EMCMOT_MAX_AXIS, inclusive */
/* this sets a global - I hate globals - hopefully this can be
moved into the config structure, or dispensed with completely */
rtapi_print_msg(RTAPI_MSG_DBG, "SET_NUM_AXES");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", emcmotCommand->axis);
if (( emcmotCommand->axis <= 0 ) ||
( emcmotCommand->axis > EMCMOT_MAX_AXIS )) {
break;
}
num_axes = emcmotCommand->axis;
emcmotConfig->numAxes = num_axes;
break;
case EMCMOT_SET_WORLD_HOME:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_WORLD_HOME");
emcmotStatus->world_home = emcmotCommand->pos;
break;
case EMCMOT_SET_HOMING_PARAMS:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_HOMING_PARAMS");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
emcmot_config_change();
if (joint == 0) {
break;
}
joint->home_offset = emcmotCommand->offset;
joint->home = emcmotCommand->home;
joint->home_search_vel = emcmotCommand->search_vel;
joint->home_latch_vel = emcmotCommand->latch_vel;
joint->home_flags = emcmotCommand->flags;
joint->home_sequence = emcmotCommand->home_sequence;
break;
case EMCMOT_OVERRIDE_LIMITS:
/* this command can be issued with axix < 0 to re-enable
limits, but they are automatically re-enabled at the
end of the next jog */
rtapi_print_msg(RTAPI_MSG_DBG, "OVERRIDE_LIMITS");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", emcmotCommand->axis);
if (emcmotCommand->axis < 0) {
/* don't override limits */
rtapi_print_msg(RTAPI_MSG_DBG, "override off");
emcmotStatus->overrideLimitMask = 0;
} else {
rtapi_print_msg(RTAPI_MSG_DBG, "override on");
emcmotStatus->overrideLimitMask = 0;
for (joint_num = 0; joint_num < num_joints; joint_num++) {
/* point at joint data */
joint = &joints[joint_num];
#if 0 // Original code commented by KSU to allow tripped axis move
/* only override limits that are currently tripped */
if ( GET_JOINT_NHL_FLAG(joint) ) {
emcmotStatus->overrideLimitMask |= (1 << (joint_num*2));
}
if ( GET_JOINT_PHL_FLAG(joint) ) {
emcmotStatus->overrideLimitMask |= (2 << (joint_num*2));
}
#else
emcmotStatus->overrideLimitMask |= (1 << (joint_num*2));
emcmotStatus->overrideLimitMask |= (2 << (joint_num*2));
#endif
}
}
emcmotDebug->overriding = 0;
for (joint_num = 0; joint_num < num_joints; joint_num++) {
/* point at joint data */
joint = &joints[joint_num];
/* clear joint errors */
SET_JOINT_ERROR_FLAG(joint, 0);
}
break;
case EMCMOT_SET_MOTOR_OFFSET:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_MOTOR_OFFSET");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", emcmotCommand->axis);
if(joint == 0) {
break;
}
joint->motor_offset = emcmotCommand->motor_offset;
break;
case EMCMOT_SET_POSITION_LIMITS:
/* sets soft limits for an axis */
rtapi_print_msg(RTAPI_MSG_DBG, "SET_POSITION_LIMITS");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
emcmot_config_change();
/* set the position limits for the axis */
/* can be done at any time */
if (joint == 0) {
break;
}
joint->min_pos_limit = emcmotCommand->minLimit;
joint->max_pos_limit = emcmotCommand->maxLimit;
break;
case EMCMOT_SET_BACKLASH:
/* sets backlash for an axis */
rtapi_print_msg(RTAPI_MSG_DBG, "SET_BACKLASH");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
emcmot_config_change();
/* set the backlash for the axis */
/* can be done at any time */
if (joint == 0) {
break;
}
joint->backlash = emcmotCommand->backlash;
break;
/*
Max and min ferror work like this: limiting ferror is
determined by slope of ferror line, = maxFerror/limitVel ->
limiting ferror = maxFerror/limitVel * vel. If ferror <
minFerror then OK else if ferror < limiting ferror then OK
else ERROR */
case EMCMOT_SET_MAX_FERROR:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_MAX_FERROR");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
emcmot_config_change();
if (joint == 0 || emcmotCommand->maxFerror < 0.0) {
break;
}
joint->max_ferror = emcmotCommand->maxFerror;
break;
case EMCMOT_SET_MIN_FERROR:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_MIN_FERROR");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
emcmot_config_change();
if (joint == 0 || emcmotCommand->minFerror < 0.0) {
break;
}
joint->min_ferror = emcmotCommand->minFerror;
break;
case EMCMOT_JOG_CONT:
/* do a continuous jog, implemented as an incremental jog to the
limit. When the user lets go of the button an abort will
stop the jog. */
rtapi_print_msg(RTAPI_MSG_DBG, "JOG_CONT");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
/* check axis range */
if (joint == 0) {
break;
}
/* must be in free mode and enabled */
if (GET_MOTION_COORD_FLAG()) {
reportError("Can't jog axis in coordinated mode.");
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
if (!GET_MOTION_ENABLE_FLAG()) {
reportError("Can't jog axis when not enabled.");
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
if (emcmotStatus->homing_active) {
reportError("Can't jog any axis while homing.");
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
if (joint->wheel_jog_active) {
/* can't do two kinds of jog at once */
break;
}
if (emcmotStatus->net_feed_scale < 0.0001 ) {
/* don't jog if feedhold is on or if feed override is zero */
break;
}
/* don't jog further onto limits */
if (!jog_ok(joint_num, emcmotCommand->vel)) {
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
/* set destination of jog */
refresh_jog_limits(joint);
if (emcmotCommand->vel > 0.0) {
joint->free_pos_cmd = joint->max_jog_limit;
} else {
joint->free_pos_cmd = joint->min_jog_limit;
}
/* set velocity of jog */
joint->free_vel_lim = fabs(emcmotCommand->vel);
/* lock out other jog sources */
joint->kb_jog_active = 1;
/* and let it go */
joint->free_tp_enable = 1;
/*! \todo FIXME - should we really be clearing errors here? */
SET_JOINT_ERROR_FLAG(joint, 0);
/* clear axis homed flag(s) if we don't have forward kins.
Otherwise, a transition into coordinated mode will incorrectly
assume the homed position. Do all if they've all been moved
since homing, otherwise just do this one */
clearHomes(joint_num);
break;
case EMCMOT_JOG_INCR:
/* do an incremental jog */
/* check axis range */
rtapi_print_msg(RTAPI_MSG_DBG, "JOG_INCR");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
if (joint == 0) {
break;
}
/* must be in free mode and enabled */
if (GET_MOTION_COORD_FLAG()) {
reportError("Can't jog axis in coordinated mode.");
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
if (!GET_MOTION_ENABLE_FLAG()) {
reportError("Can't jog axis when not enabled.");
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
if (emcmotStatus->homing_active) {
reportError("Can't jog any axis while homing.");
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
if (joint->wheel_jog_active) {
/* can't do two kinds of jog at once */
break;
}
if (emcmotStatus->net_feed_scale < 0.0001 ) {
/* don't jog if feedhold is on or if feed override is zero */
break;
}
/* don't jog further onto limits */
if (!jog_ok(joint_num, emcmotCommand->vel)) {
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
/* set target position for jog */
if (emcmotCommand->vel > 0.0) {
tmp1 = joint->free_pos_cmd + emcmotCommand->offset;
} else {
tmp1 = joint->free_pos_cmd - emcmotCommand->offset;
}
/* don't jog past limits */
refresh_jog_limits(joint);
if (tmp1 > joint->max_jog_limit) {
break;
}
if (tmp1 < joint->min_jog_limit) {
break;
}
/* set target position */
joint->free_pos_cmd = tmp1;
/* set velocity of jog */
joint->free_vel_lim = fabs(emcmotCommand->vel);
/* lock out other jog sources */
joint->kb_jog_active = 1;
/* and let it go */
joint->free_tp_enable = 1;
SET_JOINT_ERROR_FLAG(joint, 0);
/* clear axis homed flag(s) if we don't have forward kins.
Otherwise, a transition into coordinated mode will incorrectly
assume the homed position. Do all if they've all been moved
since homing, otherwise just do this one */
clearHomes(joint_num);
break;
case EMCMOT_JOG_ABS:
/* do an absolute jog */
/* check axis range */
rtapi_print_msg(RTAPI_MSG_DBG, "JOG_ABS");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
if (joint == 0) {
break;
}
/* must be in free mode and enabled */
if (GET_MOTION_COORD_FLAG()) {
reportError("Can't jog axis in coordinated mode.");
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
if (!GET_MOTION_ENABLE_FLAG()) {
reportError("Can't jog axis when not enabled.");
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
if (emcmotStatus->homing_active) {
reportError("Can't jog any axis while homing.");
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
if (joint->wheel_jog_active) {
/* can't do two kinds of jog at once */
break;
}
if (emcmotStatus->net_feed_scale < 0.0001 ) {
/* don't jog if feedhold is on or if feed override is zero */
break;
}
/* don't jog further onto limits */
if (!jog_ok(joint_num, emcmotCommand->vel)) {
SET_JOINT_ERROR_FLAG(joint, 1);
break;
}
/*! \todo FIXME-- use 'goal' instead */
joint->free_pos_cmd = emcmotCommand->offset;
/* don't jog past limits */
refresh_jog_limits(joint);
if (joint->free_pos_cmd > joint->max_jog_limit) {
joint->free_pos_cmd = joint->max_jog_limit;
}
if (joint->free_pos_cmd < joint->min_jog_limit) {
joint->free_pos_cmd = joint->min_jog_limit;
}
/* set velocity of jog */
joint->free_vel_lim = fabs(emcmotCommand->vel);
/* lock out other jog sources */
joint->kb_jog_active = 1;
/* and let it go */
joint->free_tp_enable = 1;
SET_JOINT_ERROR_FLAG(joint, 0);
/* clear axis homed flag(s) if we don't have forward kins.
Otherwise, a transition into coordinated mode will incorrectly
assume the homed position. Do all if they've all been moved
since homing, otherwise just do this one */
clearHomes(joint_num);
break;
case EMCMOT_SET_TERM_COND:
/* sets termination condition for motion emcmotDebug->queue */
rtapi_print_msg(RTAPI_MSG_DBG, "SET_TERM_COND");
tpSetTermCond(&emcmotDebug->queue, emcmotCommand->termCond, emcmotCommand->tolerance);
break;
case EMCMOT_SET_SPINDLESYNC:
tpSetSpindleSync(&emcmotDebug->queue, emcmotCommand->spindlesync, emcmotCommand->flags);
break;
case EMCMOT_SET_LINE:
/* emcmotDebug->queue up a linear move */
/* requires coordinated mode, enable off, not on limits */
rtapi_print_msg(RTAPI_MSG_DBG, "SET_LINE");
if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) {
reportError
("need to be enabled, in coord mode for linear move");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
SET_MOTION_ERROR_FLAG(1);
break;
} else if (!inRange(emcmotCommand->pos)) {
if(emcmotCommand->id > 0)
reportError("linear move on line %d would exceed limits",
emcmotCommand->id);
else
reportError("linear move in MDI would exceed limits");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
} else if (!limits_ok()) {
reportError("can't do linear move with limits exceeded");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
}
/* append it to the emcmotDebug->queue */
tpSetId(&emcmotDebug->queue, emcmotCommand->id);
if (-1 == tpAddLine(&emcmotDebug->queue, emcmotCommand->pos, emcmotCommand->motion_type, emcmotCommand->vel, emcmotCommand->ini_maxvel, emcmotCommand->acc, emcmotStatus->enables_new)) {
reportError("can't add linear move");
emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
} else {
SET_MOTION_ERROR_FLAG(0);
/* set flag that indicates all axes need rehoming, if any
axis is moved in joint mode, for machines with no forward
kins */
rehomeAll = 1;
}
break;
case EMCMOT_SET_CIRCLE:
/* emcmotDebug->queue up a circular move */
/* requires coordinated mode, enable on, not on limits */
rtapi_print_msg(RTAPI_MSG_DBG, "SET_CIRCLE");
if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) {
reportError
("need to be enabled, in coord mode for circular move");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
SET_MOTION_ERROR_FLAG(1);
break;
} else if (!inRange(emcmotCommand->pos)) {
if(emcmotCommand->id > 0)
reportError("circular move on line %d would exceed limits",
emcmotCommand->id);
else
reportError("circular move in MDI would exceed limits");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
} else if (!limits_ok()) {
reportError("can't do circular move with limits exceeded");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
}
/* append it to the emcmotDebug->queue */
tpSetId(&emcmotDebug->queue, emcmotCommand->id);
if (-1 ==
tpAddCircle(&emcmotDebug->queue, emcmotCommand->pos,
emcmotCommand->center, emcmotCommand->normal,
emcmotCommand->turn, emcmotCommand->motion_type,
emcmotCommand->vel, emcmotCommand->ini_maxvel,
emcmotCommand->acc, emcmotStatus->enables_new)) {
reportError("can't add circular move");
emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
} else {
SET_MOTION_ERROR_FLAG(0);
/* set flag that indicates all axes need rehoming, if any
axis is moved in joint mode, for machines with no forward
kins */
rehomeAll = 1;
}
break;
case EMCMOT_SET_VEL:
/* set the velocity for subsequent moves */
/* can do it at any time */
rtapi_print_msg(RTAPI_MSG_DBG, "SET_VEL");
emcmotStatus->vel = emcmotCommand->vel;
tpSetVmax(&emcmotDebug->queue, emcmotStatus->vel,
emcmotCommand->ini_maxvel);
break;
case EMCMOT_SET_VEL_LIMIT:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_VEL_LIMIT");
emcmot_config_change();
/* set the absolute max velocity for all subsequent moves */
/* can do it at any time */
emcmotConfig->limitVel = emcmotCommand->vel;
tpSetVlimit(&emcmotDebug->queue, emcmotConfig->limitVel);
break;
case EMCMOT_SET_JOINT_VEL_LIMIT:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_JOINT_VEL_LIMIT");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
emcmot_config_change();
/* check axis range */
if (joint == 0) {
break;
}
joint->vel_limit = emcmotCommand->vel;
joint->big_vel = 10 * emcmotCommand->vel;
break;
case EMCMOT_SET_JOINT_ACC_LIMIT:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_JOINT_ACC_LIMIT");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
emcmot_config_change();
/* check axis range */
if (joint == 0) {
break;
}
joint->acc_limit = emcmotCommand->acc;
break;
case EMCMOT_SET_ACC:
/* set the max acceleration */
/* can do it at any time */
rtapi_print_msg(RTAPI_MSG_DBG, "SET_ACCEL");
emcmotStatus->acc = emcmotCommand->acc;
tpSetAmax(&emcmotDebug->queue, emcmotStatus->acc);
break;
case EMCMOT_PAUSE:
/* pause the motion */
/* can happen at any time */
rtapi_print_msg(RTAPI_MSG_DBG, "PAUSE");
tpPause(&emcmotDebug->queue);
emcmotStatus->paused = 1;
break;
case EMCMOT_RESUME:
/* resume paused motion */
/* can happen at any time */
rtapi_print_msg(RTAPI_MSG_DBG, "RESUME");
emcmotDebug->stepping = 0;
tpResume(&emcmotDebug->queue);
emcmotStatus->paused = 0;
break;
case EMCMOT_STEP:
/* resume paused motion until id changes */
/* can happen at any time */
rtapi_print_msg(RTAPI_MSG_DBG, "STEP");
if(emcmotStatus->paused) {
emcmotDebug->idForStep = emcmotStatus->id;
emcmotDebug->stepping = 1;
tpResume(&emcmotDebug->queue);
emcmotStatus->paused = 1;
} else {
reportError("MOTION: can't STEP while already executing");
}
break;
case EMCMOT_FEED_SCALE:
/* override speed */
/* can happen at any time */
rtapi_print_msg(RTAPI_MSG_DBG, "FEED SCALE");
if (emcmotCommand->scale < 0.0) {
emcmotCommand->scale = 0.0; /* clamp it */
}
emcmotStatus->feed_scale = emcmotCommand->scale;
break;
case EMCMOT_FS_ENABLE:
/* enable/disable overriding speed */
/* can happen at any time */
if ( emcmotCommand->mode != 0 ) {
rtapi_print_msg(RTAPI_MSG_DBG, "FEED SCALE: ON");
emcmotStatus->enables_new |= FS_ENABLED;
} else {
rtapi_print_msg(RTAPI_MSG_DBG, "FEED SCALE: OFF");
emcmotStatus->enables_new &= ~FS_ENABLED;
}
break;
case EMCMOT_FH_ENABLE:
/* enable/disable feed hold */
/* can happen at any time */
if ( emcmotCommand->mode != 0 ) {
rtapi_print_msg(RTAPI_MSG_DBG, "FEED HOLD: ENABLED");
emcmotStatus->enables_new |= FH_ENABLED;
} else {
rtapi_print_msg(RTAPI_MSG_DBG, "FEED HOLD: DISABLED");
emcmotStatus->enables_new &= ~FH_ENABLED;
}
break;
case EMCMOT_SPINDLE_SCALE:
/* override spindle speed */
/* can happen at any time */
rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE SCALE");
if (emcmotCommand->scale < 0.0) {
emcmotCommand->scale = 0.0; /* clamp it */
}
emcmotStatus->spindle_scale = emcmotCommand->scale;
break;
case EMCMOT_SS_ENABLE:
/* enable/disable overriding spindle speed */
/* can happen at any time */
if ( emcmotCommand->mode != 0 ) {
rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE SCALE: ON");
emcmotStatus->enables_new |= SS_ENABLED;
} else {
rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE SCALE: OFF");
emcmotStatus->enables_new &= ~SS_ENABLED;
}
break;
case EMCMOT_AF_ENABLE:
/* enable/disable adaptive feedrate override from HAL pin */
/* can happen at any time */
if ( emcmotCommand->flags != 0 ) {
rtapi_print_msg(RTAPI_MSG_DBG, "ADAPTIVE FEED: ON");
emcmotStatus->enables_new |= AF_ENABLED;
} else {
rtapi_print_msg(RTAPI_MSG_DBG, "ADAPTIVE FEED: OFF");
emcmotStatus->enables_new &= ~AF_ENABLED;
}
break;
case EMCMOT_DISABLE:
/* go into disable */
/* can happen at any time */
/* reset the emcmotDebug->enabling flag to defer disable until
controller cycle (it *will* be honored) */
rtapi_print_msg(RTAPI_MSG_DBG, "DISABLE");
emcmotDebug->enabling = 0;
if (kinType == KINEMATICS_INVERSE_ONLY) {
emcmotDebug->teleoperating = 0;
emcmotDebug->coordinating = 0;
}
break;
case EMCMOT_ENABLE:
/* come out of disable */
/* can happen at any time */
/* set the emcmotDebug->enabling flag to defer enable until
controller cycle */
rtapi_print_msg(RTAPI_MSG_DBG, "ENABLE");
if ( *(emcmot_hal_data->enable) == 0 ) {
reportError("can't enable motion, enable input is false");
} else {
emcmotDebug->enabling = 1;
if (kinType == KINEMATICS_INVERSE_ONLY) {
emcmotDebug->teleoperating = 0;
emcmotDebug->coordinating = 0;
}
}
break;
case EMCMOT_ACTIVATE_JOINT:
/* make axis active, so that amps will be enabled when system is
enabled or disabled */
/* can be done at any time */
rtapi_print_msg(RTAPI_MSG_DBG, "ACTIVATE_JOINT");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
if (joint == 0) {
break;
}
SET_JOINT_ACTIVE_FLAG(joint, 1);
break;
case EMCMOT_DEACTIVATE_JOINT:
/* make axis inactive, so that amps won't be affected when system
is enabled or disabled */
/* can be done at any time */
rtapi_print_msg(RTAPI_MSG_DBG, "DEACTIVATE_AXIS");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
if (joint == 0) {
break;
}
SET_JOINT_ACTIVE_FLAG(joint, 0);
break;
/*! \todo FIXME - need to replace the ext function */
case EMCMOT_ENABLE_AMPLIFIER:
/* enable the amplifier directly, but don't enable calculations */
/* can be done at any time */
rtapi_print_msg(RTAPI_MSG_DBG, "ENABLE_AMP");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
if (joint == 0) {
break;
}
/*! \todo Another #if 0 */
#if 0
extAmpEnable(axis, 1);
#endif
break;
case EMCMOT_DISABLE_AMPLIFIER:
/* disable the axis calculations and amplifier, but don't disable
calculations */
/* can be done at any time */
rtapi_print_msg(RTAPI_MSG_DBG, "DISABLE_AMP");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
if (joint == 0) {
break;
}
/*! \todo Another #if 0 */
#if 0
extAmpEnable(axis, 0);
#endif
break;
case EMCMOT_HOME:
/* home the specified axis */
/* need to be in free mode, enable on */
/* this just sets the initial state, then the state machine in
control.c does the rest */
rtapi_print_msg(RTAPI_MSG_DBG, "HOME");
rtapi_print_msg(RTAPI_MSG_DBG, " %d", joint_num);
if (emcmotStatus->motion_state != EMCMOT_MOTION_FREE) {
/* can't home unless in free mode */
reportError("must be in joint mode to home");
return;
}
if (!GET_MOTION_ENABLE_FLAG()) {
break;
}
if(joint_num == -1) {
if(emcmotStatus->homingSequenceState == HOME_SEQUENCE_IDLE)
emcmotStatus->homingSequenceState = HOME_SEQUENCE_START;
else
reportError("homing sequence already in progress");
break;
}
if (joint == NULL) {
break;
}
if(joint->home_state != HOME_IDLE) {
reportError("homing already in progress");
} else if(emcmotStatus->homingSequenceState != HOME_SEQUENCE_IDLE) {
reportError("homing sequence already in progress");
} else {
/* abort any movement (jog, etc) that is in progress */
joint->free_tp_enable = 0;
/* prime the homing state machine */
joint->home_state = HOME_START;
}
break;
case EMCMOT_ENABLE_WATCHDOG:
rtapi_print_msg(RTAPI_MSG_DBG, "ENABLE_WATCHDOG");
/*! \todo Another #if 0 */
#if 0
emcmotDebug->wdEnabling = 1;
emcmotDebug->wdWait = emcmotCommand->wdWait;
if (emcmotDebug->wdWait < 0) {
emcmotDebug->wdWait = 0;
}
#endif
break;
case EMCMOT_DISABLE_WATCHDOG:
rtapi_print_msg(RTAPI_MSG_DBG, "DISABLE_WATCHDOG");
/*! \todo Another #if 0 */
#if 0
emcmotDebug->wdEnabling = 0;
#endif
break;
case EMCMOT_CLEAR_PROBE_FLAGS:
rtapi_print_msg(RTAPI_MSG_DBG, "CLEAR_PROBE_FLAGS");
emcmotStatus->probing = 0;
break;
case EMCMOT_PROBE:
/* most of this is taken from EMCMOT_SET_LINE */
/* emcmotDebug->queue up a linear move */
/* requires coordinated mode, enable off, not on limits */
rtapi_print_msg(RTAPI_MSG_DBG, "PROBE");
if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) {
reportError
("need to be enabled, in coord mode for probe move");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
SET_MOTION_ERROR_FLAG(1);
break;
} else if (!inRange(emcmotCommand->pos)) {
if(emcmotCommand->id > 0)
reportError("probe move on line %d would exceed limits",
emcmotCommand->id);
else
reportError("probe move in MDI would exceed limits");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
} else if (!limits_ok()) {
reportError("can't do probe move with limits exceeded");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
}
/* append it to the emcmotDebug->queue */
tpSetId(&emcmotDebug->queue, emcmotCommand->id);
if (-1 == tpAddLine(&emcmotDebug->queue, emcmotCommand->pos, emcmotCommand->motion_type, emcmotCommand->vel, emcmotCommand->ini_maxvel, emcmotCommand->acc, emcmotStatus->enables_new)) {
reportError("can't add probe move");
emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
} else {
emcmotStatus->probing = 1;
SET_MOTION_ERROR_FLAG(0);
/* set flag that indicates all axes need rehoming, if any
axis is moved in joint mode, for machines with no forward
kins */
rehomeAll = 1;
}
break;
case EMCMOT_RIGID_TAP:
/* most of this is taken from EMCMOT_SET_LINE */
/* emcmotDebug->queue up a linear move */
/* requires coordinated mode, enable off, not on limits */
rtapi_print_msg(RTAPI_MSG_DBG, "RIGID_TAP");
if (!GET_MOTION_COORD_FLAG() || !GET_MOTION_ENABLE_FLAG()) {
reportError
("need to be enabled, in coord mode for rigid tap move");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
SET_MOTION_ERROR_FLAG(1);
break;
} else if (!inRange(emcmotCommand->pos)) {
if(emcmotCommand->id > 0)
reportError("rigid tap move on line %d would exceed limits",
emcmotCommand->id);
else
reportError("rigid tap move in MDI would exceed limits");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
} else if (!limits_ok()) {
reportError("can't do rigid tap move with limits exceeded");
emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
}
/* append it to the emcmotDebug->queue */
tpSetId(&emcmotDebug->queue, emcmotCommand->id);
if (-1 == tpAddRigidTap(&emcmotDebug->queue, emcmotCommand->pos, emcmotCommand->vel, emcmotCommand->ini_maxvel, emcmotCommand->acc, emcmotStatus->enables_new)) {
reportError("can't add rigid tap move");
emcmotStatus->commandStatus = EMCMOT_COMMAND_BAD_EXEC;
tpAbort(&emcmotDebug->queue);
SET_MOTION_ERROR_FLAG(1);
break;
} else {
SET_MOTION_ERROR_FLAG(0);
}
break;
case EMCMOT_SET_TELEOP_VECTOR:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_TELEOP_VECTOR");
if (!GET_MOTION_TELEOP_FLAG() || !GET_MOTION_ENABLE_FLAG()) {
reportError
("need to be enabled, in teleop mode for teleop move");
} else {
double velmag;
emcmotDebug->teleop_data.desiredVel = emcmotCommand->pos;
pmCartMag(emcmotDebug->teleop_data.desiredVel.tran, &velmag);
if (emcmotDebug->teleop_data.desiredVel.a > velmag) {
velmag = emcmotDebug->teleop_data.desiredVel.a;
}
if (emcmotDebug->teleop_data.desiredVel.b > velmag) {
velmag = emcmotDebug->teleop_data.desiredVel.b;
}
if (emcmotDebug->teleop_data.desiredVel.c > velmag) {
velmag = emcmotDebug->teleop_data.desiredVel.c;
}
if (velmag > emcmotConfig->limitVel) {
pmCartScalMult(emcmotDebug->teleop_data.desiredVel.tran,
emcmotConfig->limitVel / velmag,
&emcmotDebug->teleop_data.desiredVel.tran);
emcmotDebug->teleop_data.desiredVel.a *=
emcmotConfig->limitVel / velmag;
emcmotDebug->teleop_data.desiredVel.b *=
emcmotConfig->limitVel / velmag;
emcmotDebug->teleop_data.desiredVel.c *=
emcmotConfig->limitVel / velmag;
}
/* flag that all joints need to be homed, if any joint is
jogged individually later */
rehomeAll = 1;
}
break;
case EMCMOT_SET_DEBUG:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_DEBUG");
emcmotConfig->debug = emcmotCommand->debug;
emcmot_config_change();
break;
/* needed for synchronous I/O */
case EMCMOT_SET_AOUT:
if (emcmotCommand->now) { //we set it right away
emcmotAioWrite(emcmotCommand->out, emcmotCommand->minLimit);
} else { // we put it on the TP queue, warning: only room for one in there, any new ones will overwrite
tpSetAout(&emcmotDebug->queue, emcmotCommand->out,
emcmotCommand->start, emcmotCommand->end);
}
break;
case EMCMOT_SET_DOUT:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_DOUT");
if (emcmotCommand->now) { //we set it right away
emcmotDioWrite(emcmotCommand->out, emcmotCommand->start);
} else { // we put it on the TP queue, warning: only room for one in there, any new ones will overwrite
tpSetDout(&emcmotDebug->queue, emcmotCommand->out,
emcmotCommand->start, emcmotCommand->end);
}
break;
case EMCMOT_SET_SPINDLE_VEL:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_SPINDLE_VEL");
emcmotStatus->spindle.speed = emcmotCommand->vel;
break;
case EMCMOT_SPINDLE_ON:
rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_ON");
emcmotStatus->spindle.speed = emcmotCommand->vel;
emcmotStatus->spindle.css_factor = emcmotCommand->ini_maxvel;
emcmotStatus->spindle.xoffset = emcmotCommand->acc;
if (emcmotCommand->vel >= 0) {
emcmotStatus->spindle.direction = 1;
} else {
emcmotStatus->spindle.direction = -1;
}
emcmotStatus->spindle.brake = 0; //disengage brake
break;
case EMCMOT_SPINDLE_OFF:
rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_OFF");
emcmotStatus->spindle.speed = 0;
emcmotStatus->spindle.direction = 0;
emcmotStatus->spindle.brake = 1; // engage brake
break;
case EMCMOT_SPINDLE_INCREASE:
rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_INCREASE");
if (emcmotStatus->spindle.speed > 0) {
emcmotStatus->spindle.speed += 100; //FIXME - make the step a HAL parameter
} else if (emcmotStatus->spindle.speed < 0) {
emcmotStatus->spindle.speed -= 100;
}
break;
case EMCMOT_SPINDLE_DECREASE:
rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_DECREASE");
if (emcmotStatus->spindle.speed > 100) {
emcmotStatus->spindle.speed -= 100; //FIXME - make the step a HAL parameter
} else if (emcmotStatus->spindle.speed < -100) {
emcmotStatus->spindle.speed += 100;
}
break;
case EMCMOT_SPINDLE_BRAKE_ENGAGE:
rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_BRAKE_ENGAGE");
emcmotStatus->spindle.speed = 0;
emcmotStatus->spindle.direction = 0;
emcmotStatus->spindle.brake = 1;
break;
case EMCMOT_SPINDLE_BRAKE_RELEASE:
rtapi_print_msg(RTAPI_MSG_DBG, "SPINDLE_BRAKE_RELEASE");
emcmotStatus->spindle.brake = 0;
break;
case EMCMOT_SET_JOINT_COMP:
rtapi_print_msg(RTAPI_MSG_DBG, "SET_JOINT_COMP for joint %d", joint_num);
if (joint == 0) {
break;
}
if (joint->comp.entries >= EMCMOT_COMP_SIZE) {
reportError("joint %d: too many compensation entries", joint_num);
break;
}
/* point to last entry */
comp_entry = &(joint->comp.array[joint->comp.entries]);
if (emcmotCommand->comp_nominal <= comp_entry[0].nominal) {
reportError("joint %d: compensation values must increase", joint_num);
break;
}
/* store data to new entry */
comp_entry[1].nominal = emcmotCommand->comp_nominal;
comp_entry[1].fwd_trim = emcmotCommand->comp_forward;
comp_entry[1].rev_trim = emcmotCommand->comp_reverse;
/* calculate slopes from previous entry to the new one */
if ( comp_entry[0].nominal != -HUGE_VAL ) {
/* but only if the previous entry is "real" */
tmp1 = comp_entry[1].nominal - comp_entry[0].nominal;
comp_entry[0].fwd_slope =
(comp_entry[1].fwd_trim - comp_entry[0].fwd_trim) / tmp1;
comp_entry[0].rev_slope =
(comp_entry[1].rev_trim - comp_entry[0].rev_trim) / tmp1;
} else {
/* previous entry is at minus infinity, slopes are zero */
comp_entry[0].fwd_trim = comp_entry[1].fwd_trim;
comp_entry[0].rev_trim = comp_entry[1].rev_trim;
}
joint->comp.entries++;
break;
default:
rtapi_print_msg(RTAPI_MSG_DBG, "UNKNOWN");
reportError("unrecognized command %d", emcmotCommand->command);
emcmotStatus->commandStatus = EMCMOT_COMMAND_UNKNOWN_COMMAND;
break;
} /* end of: command switch */
if (emcmotStatus->commandStatus != EMCMOT_COMMAND_OK) {
rtapi_print_msg(RTAPI_MSG_DBG, "ERRROR: %d",
emcmotStatus->commandStatus);
}
rtapi_print_msg(RTAPI_MSG_DBG, "\n");
/* synch tail count */
emcmotStatus->tail = emcmotStatus->head;
emcmotConfig->tail = emcmotConfig->head;
emcmotDebug->tail = emcmotDebug->head;
}
/* end of: if-new-command */
check_stuff ( "after command_handler()" );
return;
}
