/**
* set_joints_known_pos()
*
* Set all the mechanism joints to known reference angles.
* Propogate the joint angle to motor position and encoder offset.
*/
int set_joints_known_pos(struct mechanism* _mech, int tool_only)
{
struct DOF* _joint=NULL;
int j=0;
/// Set joint position reference for just tools, or all DOFS
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
if ( tool_only && ! is_toolDOF( _joint->type)) {
// Set jpos_d to the joint limit value.
_joint->jpos_d = DOF_types[ _joint->type ].home_position;
} else if (!tool_only && is_toolDOF(_joint->type) ) {
_joint->jpos_d = _joint->jpos;
} else {
// Set jpos_d to the joint limit value.
_joint->jpos_d = DOF_types[ _joint->type ].max_position;
// Initialize a trajectory to operating angle
_joint->state = jstate_homing1;
}
}
/// Inverse cable coupling: jpos_d ---> mpos_d
invMechCableCoupling(_mech, 1);
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
// Reset the state-estimate filter
_joint->mpos = _joint->mpos_d;
resetFilter( _joint );
// Convert the motor position to an encoder offset.
// mpos = k * (enc_val - enc_offset) ---> enc_offset = enc_val - mpos/k
float f_enc_val = _joint->enc_val;
// Encoder values on Gold arm are reversed. See also state_machine.cpp
if ( _mech->type == GOLD_ARM)
f_enc_val *= -1.0;
/// Set the joint offset in encoder space.
float cc = ENC_CNTS_PER_REV / (2*M_PI);
_joint->enc_offset = f_enc_val - (_joint->mpos_d * cc);
getStateLPF(_joint);
}
fwdMechCableCoupling(_mech);
return 0;
}
/**
* jointVelControl()
* Move joints at constant rate.
*/
float jvel_PI_control(struct DOF *_joint, int resetI){
// Set gains. Gains have been "empirically" tuned.
// TODO: move this to a permanent place.
float ki;
float kv[MAX_MECH * MAX_DOF_PER_MECH] = {0};
kv[SHOULDER_GOLD] = kv[SHOULDER_GREEN] = (0.528/(15 DEG2RAD));
kv[ELBOW_GOLD] = kv[ELBOW_GREEN] = (0.528/(15 DEG2RAD));
kv[Z_INS_GOLD] = kv[Z_INS_GREEN] = (0.400/0.1);
kv[TOOL_ROT_GOLD] = kv[TOOL_ROT_GREEN] = (0.005/(15 DEG2RAD));
kv[WRIST_GOLD] = kv[WRIST_GREEN] = 0;
kv[GRASP1_GOLD] = kv[GRASP1_GREEN] = 0;
kv[GRASP2_GREEN] = kv[GRASP2_GREEN] = 0;
static float jVelIntErr[MAX_MECH * MAX_DOF_PER_MECH]; // Integral of velocity error
// Reset integral term
if (resetI){
jVelIntErr[_joint->type] = 0;
return 0;
}
float jVelErr;
// Calculate joint velocity error
if ( is_toolDOF(_joint) )
jVelErr = _joint->mvel_d - _joint->mvel;
else
jVelErr = _joint->jvel_d - _joint->jvel;
// Integrate error over 1ms
jVelIntErr[_joint->type] += jVelErr * ONE_MS;
ki = kv[_joint->type] * 0.1 * 0;
// Calculate PI velocity control
_joint->tau_d = ( kv[_joint->type]*jVelErr + ki*jVelIntErr[_joint->type] );
return jVelIntErr[_joint->type];
}
/**
* raven_homing()
*
* This function is called 1000 times per second during INIT mode
*
* \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
*
* \brief Move to hard stops in controled way, "zero" the joint value, and then move to "home" position
*
* 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.
* \todo Homing limits should be Amps not DAC units (see homing() ).
* \todo Eliminate or comment out Enders Game code!!
*/
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; // 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) || 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;
}
/**
* set_joints_known_pos()
*
* \param _mech which mechanism (gold/green)
* \param tool_only flag which initializes only the tool/wrist joints
*
* \brief Set joint angles to known values after hard stops are reached.
*
* Set all the mechanism joints to known reference angles.
* Propagate the joint angle to motor position and encoder offset.
*
* \todo Rationalize the sign changes on GREEN_ARM vs GOLD_ARM (see IFDEF below).
* \todo This MAYBE needs to be changed to support device specific parameter changes read from a config file or ROS service.
* \ingroup Control
*/
int set_joints_known_pos(struct mechanism* _mech, int tool_only)
{
struct DOF* _joint=NULL;
int j=0;
// int offset = 0;
// if (_mech->type == GREEN_ARM) offset = 8;
/// Set joint position reference for just tools, or all DOFS
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
// when tool joints finish, set positioning joints to neutral
if ( tool_only && ! is_toolDOF( _joint->type))
{
// Set jpos_d to the joint limit value.
_joint->jpos_d = DOF_types[ _joint->type ].home_position; // keep non tool joints from moving
}
// when positioning joints finish, set tool joints to nothing special
else if (!tool_only && is_toolDOF(_joint->type) )
{
_joint->jpos_d = _joint->jpos;
}
// when tool or positioning joints finish, set them to max_angle
else
{
// Set jpos_d to the joint limit value.
_joint->jpos_d = DOF_types[ _joint->type ].max_position;
// Initialize a trajectory to operating angle
_joint->state = jstate_homing1;
}
}
/// Inverse cable coupling: jpos_d ---> mpos_d
// use_actual flag triggered for insertion axis
invMechCableCoupling(_mech, 1);
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
// Reset the state-estimate filter
_joint->mpos = _joint->mpos_d;
resetFilter( _joint );
// Convert the motor position to an encoder offset.
// mpos = k * (enc_val - enc_offset) ---> enc_offset = enc_val - mpos/k
float f_enc_val = _joint->enc_val;
// Encoder values on Gold arm are reversed. See also state_machine.cpp
switch (_mech->tool_type)
{
case dv_adapter:
if ( _mech->type == GOLD_ARM && !is_toolDOF(_joint))
f_enc_val *= -1.0;
break;
case RII_square_type:
//Green arm tools are also reversed with square pattern
if (( _mech->type == GOLD_ARM && !is_toolDOF(_joint) ) ||
( _mech->type == GREEN_ARM && is_toolDOF(_joint) ))
f_enc_val *= -1.0;
break;
default:
if ( _mech->type == GOLD_ARM || is_toolDOF(_joint) )
f_enc_val *= -1.0;
break;
//Needs a true default/error state
}
/// Set the joint offset in encoder space.
float cc = ENC_CNTS_PER_REV / (2*M_PI);
_joint->enc_offset = f_enc_val - (_joint->mpos_d * cc);
getStateLPF(_joint, _mech->tool_type);
}
fwdMechCableCoupling(_mech);
return 0;
}
int is_toolDOF(struct DOF *_joint){
return is_toolDOF(_joint->type);
}
/**
* \fn int set_joints_known_pos(struct mechanism* _mech, int tool_only)
*
* \brief Set joint angles to known values after hard stops are reached.
*
* \desc Set all the mechanism joints to known reference angles.
* Propagate the joint angle to motor position and encoder offset.
*
* \param _mech which mechanism (gold/green)
* \param tool_only flag which initializes only the tool/wrist joints
*
* \ingroup Control
*
* \return 0
*
* \todo Rationalize the sign changes on GREEN_ARM vs GOLD_ARM (see IFDEF below).
* \todo This MAYBE needs to be changed to support device specific parameter changes read from a config file or ROS service.
*/
int set_joints_known_pos(struct mechanism* _mech, int tool_only)
{
struct DOF* _joint=NULL;
int j=0;
int scissor = ((_mech->mech_tool.t_end == mopocu_scissor) || (_mech->mech_tool.t_end == potts_scissor))? 1 : 0;
// int offset = 0;
// if (_mech->type == GREEN_ARM) offset = 8;
/// Set joint position reference for just tools, or all DOFS
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
// when tool joints finish, set positioning joints to neutral
if ( tool_only && ! is_toolDOF( _joint->type))
{
// Set jpos_d to the joint limit value.
_joint->jpos_d = DOF_types[ _joint->type ].home_position; // keep non tool joints from moving
}
// when positioning joints finish, set tool joints to nothing special
else if (!tool_only && is_toolDOF(_joint->type) )
{
_joint->jpos_d = _joint->jpos;
}
// when tool or positioning joints finish, set them to max_angle
else {
// Set jpos_d to the joint limit value.
if (scissor && ((_joint->type == GRASP2_GREEN) || (_joint->type == GRASP2_GOLD))){
_joint->jpos_d = _mech->mech_tool.grasp2_min_angle;
log_msg("setting grasp 2 to arm %d, joint %d to value %f", _mech->mech_tool.mech_type, _joint->type, _joint->jpos_d);
}
else
_joint->jpos_d = DOF_types[_joint->type].max_position;
// Initialize a trajectory to operating angle
_joint->state = jstate_homing1;
}
}
/// Inverse cable coupling: jpos_d ---> mpos_d
// use_actual flag triggered for insertion axis
invMechCableCoupling(_mech, 1);
_joint = NULL;
while ( loop_over_joints(_mech, _joint ,j) )
{
// Reset the state-estimate filter
_joint->mpos = _joint->mpos_d;
resetFilter( _joint );
// Convert the motor position to an encoder offset.
// mpos = k * (enc_val - enc_offset) ---> enc_offset = enc_val - mpos/k
float f_enc_val = _joint->enc_val;
// Encoder values on Gold arm are reversed. See also state_machine.cpp
switch (_mech->mech_tool.t_style){
case dv:
if ( _mech->type == GOLD_ARM && !is_toolDOF(_joint))
f_enc_val *= -1.0;
break;
case square_raven:
if ( ( _mech->type == GOLD_ARM && !is_toolDOF(_joint) )
||
( _mech->type == GREEN_ARM && is_toolDOF(_joint) ) //Green arm tools are also reversed with square pattern
)
f_enc_val *= -1.0;
break;
default:
if ( _mech->type == GOLD_ARM || is_toolDOF(_joint) )
f_enc_val *= -1.0;
break;
}
#ifdef OPPOSE_GRIP
if (j == GRASP1){
f_enc_val *= -1;// switch encoder value for opposed grasp
// static int twice = 0;
// if (twice < 2){
// log_msg("homing enc_val swapped");
// twice++;
}
#endif
/// Set the joint offset in encoder space.
float cc = ENC_CNTS_PER_REV / (2*M_PI);
_joint->enc_offset = f_enc_val - (_joint->mpos_d * cc);
getStateLPF(_joint, _mech->tool_type);
//getStateLPF(_joint, _mech->mech_tool.t_style);
}
fwdMechCableCoupling(_mech);
return 0;
}
