/**
* \fn int raven_homing(struct device *device0, struct param_pass *currParams, int begin_homing)
*
* \brief Move to hard stops in controlled way, "zero" the joint value, and then move to "home" position
*
* \desc This function is called 1000 times per second during INIT mode
* This function operates in two phases:
* -# Discover joint position by running to hard stop. Using PD control (I term zero'd) we move the
* joint at a smooth rate until current increases which indicates hitting hard mechanical stop.
* -# Move joints to "home" position. In this phase the robot moves from the joint limits to a
* designated pose in the center of the workspace.
*
* \param device0 Which (top level) device to home (usually only one device per system)
* \param currParams Current parameters (for Run Level)
* \param begin_homing Flag to start the homing process
*
* \ingroup Control
*
* \return 0
*
* \todo Homing limits should be Amps not DAC units (see homing()).
*/
int raven_homing(struct device *device0, struct param_pass *currParams, int begin_homing)
{
static int homing_inited = 0;
static unsigned long int delay, delay2;
struct DOF *_joint = NULL;
struct mechanism* _mech = NULL;
int i=0,j=0;
#ifdef RICKS_TOOLS // Refers to Enders Game Prop manager!!
// these were wooden dummy tools for the movie.
_mech = NULL; _joint = NULL;
while (loop_over_joints(device0, _mech, _joint, i,j) )
{
if (is_toolDOF(_joint))
{
_joint->state = jstate_ready;
}
}
#endif
// Only run in init mode
if ( ! (currParams->runlevel == RL_INIT && currParams->sublevel == SL_AUTO_INIT ))
{
homing_inited = 0;
delay = gTime;
return 0; // return if we are in the WRONG run level (PLC state)
}
// Wait a short time for amps to turn on
if (gTime - delay < 1000)
{
return 0;
}
// Initialize the homing sequence.
if (begin_homing || !homing_inited) // only do this the first time through
{
// Zero out joint torques, and control inputs. Set joint.state=not_ready.
_mech = NULL; _joint = NULL;
while (loop_over_joints(device0, _mech, _joint, i,j) ) // foreach (joint)
{
_joint->tau_d = 0;
_joint->mpos_d = _joint->mpos;
_joint->jpos_d = _joint->jpos;
_joint->jvel_d = 0;
_joint->state = jstate_not_ready;
if (is_toolDOF(_joint))
jvel_PI_control(_joint, 1); // reset PI control integral term
homing_inited = 1;
}
log_msg("Homing sequence initialized");
}
// Specify motion commands
_mech = NULL; _joint = NULL;
while ( loop_over_joints(device0, _mech, _joint, i,j) )
{
// Initialize tools first.
if ( is_toolDOF(_joint)){
homing(_joint, device0->mech[i].mech_tool);
}
else if(tools_ready( &(device0->mech[i]))){
homing(_joint);
}
}
//Inverse Cable Coupling
invCableCoupling(device0, currParams->runlevel);
// Do PD control on all joints
_mech = NULL; _joint = NULL;
while ( loop_over_joints(device0, _mech, _joint, i,j) )
{
mpos_PD_control( _joint );
}
// Calculate output DAC values
TorqueToDAC(device0);
// Check homing conditions and set joint angles appropriately.
_mech = NULL; _joint = NULL;
while ( loop_over_joints(device0, _mech, _joint, i,j) )
{
struct DOF * _joint = &(_mech->joint[j]); ///\todo is this line necessary?
// Check to see if we've reached the joint limit.
if( check_homing_condition(_joint) )
{
log_msg("Found limit on joint %d cmd: %d \t", _joint->type, _joint->current_cmd, DOF_types[_joint->type].DAC_max);
_joint->state = jstate_hard_stop;
_joint->current_cmd = 0;
stop_trajectory(_joint);
log_msg("joint %d checked ",j);
}
// For each mechanism, check to see if the mech is finished homing.
if ( j == (MAX_DOF_PER_MECH-1) )
{
/// if we're homing tools, wait for tools to be finished
if (( !tools_ready(_mech) &&
_mech->joint[TOOL_ROT].state==jstate_hard_stop &&
_mech->joint[WRIST ].state==jstate_hard_stop &&
_mech->joint[GRASP1 ].state==jstate_hard_stop )
||
( tools_ready( _mech ) &&
_mech->joint[SHOULDER].state==jstate_hard_stop &&
_mech->joint[ELBOW ].state==jstate_hard_stop &&
_mech->joint[Z_INS ].state==jstate_hard_stop ))
{
if (delay2==0)
delay2=gTime;
if (gTime > delay2 + 200) // wait 200 ticks for cables to settle down
{
set_joints_known_pos(_mech, !tools_ready(_mech) ); // perform second phase
delay2 = 0;
}
}
}
}
return 0;
}
/*
* raven_homing()
* 1- Discover joint position by running to hard stop
* 2- Move joints to "home" position.
*
*/
int raven_homing(struct device *device0, struct param_pass *currParams, int begin_homing)
{
static int homing_inited = 0;
static unsigned long int delay, delay2;
struct DOF *_joint = NULL;
struct mechanism* _mech = NULL;
int i=0,j=0;
// Only run in init mode
if ( ! (currParams->runlevel == RL_INIT && currParams->sublevel == SL_AUTO_INIT ))
{
homing_inited = 0;
delay = gTime;
return 0;
}
// Wait a short time for amps to turn on
if (gTime - delay < 1000)
{
return 0;
}
// Initialize the homing sequence.
if (begin_homing || !homing_inited)
{
// Zero out joint torques, and control inputs. Set joint.state=not_ready.
_mech = NULL; _joint = NULL;
while (loop_over_joints(device0, _mech, _joint, i,j) )
{
_joint->tau_d = 0;
_joint->mpos_d = _joint->mpos;
_joint->jpos_d = _joint->jpos;
_joint->jvel_d = 0;
_joint->state = jstate_not_ready;
if (is_toolDOF(_joint))
jvel_PI_control(_joint, 1); // reset PI control integral term
homing_inited = 1;
}
}
// Specify motion commands
_mech = NULL; _joint = NULL;
while ( loop_over_joints(device0, _mech, _joint, i,j) )
{
// Initialize tools first.
if ( is_toolDOF(_joint) || tools_ready( &(device0->mech[i]) ) )
{
homing(_joint);
}
}
//Inverse Cable Coupling
invCableCoupling(device0, currParams->runlevel);
// Do PD control on all joints
_mech = NULL; _joint = NULL;
while ( loop_over_joints(device0, _mech, _joint, i,j) )
{
mpos_PD_control( _joint );
}
// Calculate output DAC values
TorqueToDAC(device0);
// Check homing conditions and set joint angles appropriately.
_mech = NULL; _joint = NULL;
while ( loop_over_joints(device0, _mech, _joint, i,j) )
{
struct DOF * _joint = &(_mech->joint[j]);
// Check to see if we've reached the joint limit.
if( check_homing_condition(_joint) )
{
log_msg("Found limit on joint %s", jointIndexAndArmName(_joint->type).c_str(), _joint->current_cmd, DOF_types[_joint->type].DAC_max);
_joint->state = jstate_hard_stop;
_joint->current_cmd = 0;
stop_trajectory(_joint);
}
// For each mechanism, check to see if the mech is finished homing.
if ( j == (MAX_DOF_PER_MECH-1) )
{
/// if we're homing tools, wait for tools to be finished
bool ready = ( !tools_ready(_mech) &&
_mech->joint[TOOL_ROT].state==jstate_hard_stop &&
_mech->joint[WRIST ].state==jstate_hard_stop &&
_mech->joint[GRASP1 ].state==jstate_hard_stop )
||
( tools_ready( _mech ) &&
_mech->joint[SHOULDER].state==jstate_hard_stop &&
_mech->joint[ELBOW ].state==jstate_hard_stop &&
_mech->joint[Z_INS ].state==jstate_hard_stop );
if (ready) {
if (delay2==0) {
delay2=gTime;
}
if (gTime > delay2 + 200) {
set_joints_known_pos(_mech, !tools_ready(_mech) );
delay2 = 0;
}
}
}
}
saveOffsets(*device0);
return 0;
}
