void do_homing_sequence(void)
{
static int home_sequence = -1;
int i;
int seen = 0;
emcmot_joint_t *joint;
/* first pass init */
if(home_sequence == -1) {
emcmotStatus->homingSequenceState = HOME_SEQUENCE_IDLE;
home_sequence = 0;
}
switch(emcmotStatus->homingSequenceState) {
case HOME_SEQUENCE_IDLE:
/* nothing to do */
break;
case HOME_SEQUENCE_START:
/* a request to home all joints */
for(i=0; i < num_joints; i++) {
joint = &joints[i];
if(joint->home_state != HOME_IDLE) {
/* a home is already in progress, abort the home-all */
emcmotStatus->homingSequenceState = HOME_SEQUENCE_IDLE;
return;
}
}
/* ok to start the sequence, start at zero */
home_sequence = 0;
/* tell the world we're on the job */
emcmotStatus->homing_active = 1;
/* and drop into next state */
case HOME_SEQUENCE_START_JOINTS:
/* start all joints whose sequence number matches home_sequence */
for(i=0; i < num_joints; i++) {
joint = &joints[i];
if(joint->home_sequence == home_sequence) {
/* start this joint */
joint->free_tp.enable = 0;
joint->home_state = HOME_START;
seen++;
}
}
if(seen) {
/* at least one joint is homing, wait for it */
emcmotStatus->homingSequenceState = HOME_SEQUENCE_WAIT_JOINTS;
} else {
/* no joints have this sequence number, we're done */
emcmotStatus->homingSequenceState = HOME_IDLE;
/* tell the world */
emcmotStatus->homing_active = 0;
}
break;
case HOME_SEQUENCE_WAIT_JOINTS:
for(i=0; i < num_joints; i++) {
joint = &joints[i];
if(joint->home_sequence != home_sequence) {
/* this joint is not at the current sequence number, ignore it */
continue;
}
if(joint->home_state != HOME_IDLE) {
/* still busy homing, keep waiting */
seen = 1;
continue;
}
if(!GET_JOINT_AT_HOME_FLAG(joint)) {
/* joint should have been homed at this step, it is no longer
homing, but its not at home - must have failed. bail out */
emcmotStatus->homingSequenceState = HOME_SEQUENCE_IDLE;
emcmotStatus->homing_active = 0;
return;
}
}
if(!seen) {
/* all joints at this step have finished homing, move on to next step */
home_sequence ++;
emcmotStatus->homingSequenceState = HOME_SEQUENCE_START_JOINTS;
}
break;
default:
/* should never get here */
reportError("unknown state '%d' during homing sequence",
emcmotStatus->homingSequenceState);
emcmotStatus->homingSequenceState = HOME_SEQUENCE_IDLE;
emcmotStatus->homing_active = 0;
break;
}
}
void do_homing_sequence(void)
{
int i,ii;
int special_case_sync_all;
int seen = 0;
emcmot_joint_t *joint;
/* first pass init */
if(home_sequence == -1) {
emcmotStatus->homingSequenceState = HOME_SEQUENCE_IDLE;
home_sequence = 0;
}
switch(emcmotStatus->homingSequenceState) {
case HOME_SEQUENCE_IDLE:
/* nothing to do */
break;
case HOME_SEQUENCE_START:
/* Request to home all joints
* A negative joint->home_sequence means sync final move
*
* Initializations
*/
home_sequence = 0;
for(i=0; i < MAX_HOME_SEQUENCES; i++) {
sync_final_move[i] = 0; //reset to allow a rehome
}
/*
* Force all joints having identical ABS(joint->home_sequence)
* to agree, e.g., if any one is negative make them all negative
*/
for(i=0; i < emcmotConfig->numJoints; i++) {
joint = &joints[i];
if ( (joint->home_flags & HOME_NO_REHOME)
&& GET_JOINT_HOMED_FLAG(joint)
) {
continue;
} else {
SET_JOINT_HOMED_FLAG(joint, 0);
}
if (joint->home_sequence < 0) {
// if a joint->home_sequence is neg, find all joints that
// have the same ABS sequence value and make them the same
emcmot_joint_t *jtmp;
for(ii=0; ii < emcmotConfig->numJoints; ii++) {
jtmp = &joints[ii];
if (jtmp->home_sequence == ABS(joint->home_sequence)) {
jtmp->home_sequence = joint->home_sequence;
}
}
}
}
/* special_case_sync_all: if home_sequence == -1 for all joints
* synchronize all joints final move
*/
special_case_sync_all = 1; // disprove
for(i=0; i < emcmotConfig->numJoints; i++) {
joint = &joints[i];
if (joint->home_sequence != -1) {special_case_sync_all = 0;}
}
for(i=0; i < emcmotConfig->numJoints; i++) {
joint = &joints[i];
if(joint->home_state != HOME_IDLE) {
/* a home is already in progress, abort the home-all */
emcmotStatus->homingSequenceState = HOME_SEQUENCE_IDLE;
return;
}
}
if (special_case_sync_all) {
home_sequence = 1;
} else {
/* ok to start the sequence, start at zero */
home_sequence = 0;
}
/* tell the world we're on the job */
emcmotStatus->homing_active = 1;
/* and drop into next state */
case HOME_SEQUENCE_START_JOINTS:
/* start all joints whose sequence number matches home_sequence */
for(i=0; i < emcmotConfig->numJoints; i++) {
joint = &joints[i];
// negative joint->home_sequence means sync final move
if(ABS(joint->home_sequence) == home_sequence) {
/* start this joint */
joint->free_tp.enable = 0;
joint->home_state = HOME_START;
seen++;
}
}
if(seen) {
/* at least one joint is homing, wait for it */
emcmotStatus->homingSequenceState = HOME_SEQUENCE_WAIT_JOINTS;
} else {
/* no joints have this sequence number, we're done */
emcmotStatus->homingSequenceState = HOME_SEQUENCE_IDLE;
/* tell the world */
emcmotStatus->homing_active = 0;
}
break;
case HOME_SEQUENCE_WAIT_JOINTS:
for(i=0; i < emcmotConfig->numJoints; i++) {
joint = &joints[i];
// negative joint->home_sequence means sync final move
if(ABS(joint->home_sequence) != home_sequence) {
/* this joint is not at the current sequence number, ignore it */
continue;
}
if(joint->home_state != HOME_IDLE) {
/* still busy homing, keep waiting */
seen = 1;
continue;
}
if(!GET_JOINT_AT_HOME_FLAG(joint)) {
/* joint should have been homed at this step, it is no longer
homing, but its not at home - must have failed. bail out */
emcmotStatus->homingSequenceState = HOME_SEQUENCE_IDLE;
emcmotStatus->homing_active = 0;
return;
}
}
if(!seen) {
/* all joints at this step have finished homing, move on to next step */
home_sequence ++;
emcmotStatus->homingSequenceState = HOME_SEQUENCE_START_JOINTS;
}
break;
default:
/* should never get here */
reportError(_("unknown state '%d' during homing sequence"),
emcmotStatus->homingSequenceState);
emcmotStatus->homingSequenceState = HOME_SEQUENCE_IDLE;
emcmotStatus->homing_active = 0;
break;
}
}
