/**
* homing()
*
* set trajectory behavior for each joint during the homing process.
*/
void homing(struct DOF* _joint)
{
const float f_period[MAX_MECH*MAX_DOF_PER_MECH] = {1, 1, 1, 9999999, 1, 1, 1, 1,
1, 1, 1, 9999999, 1, 1, 1, 1};
const float f_magnitude[MAX_MECH*MAX_DOF_PER_MECH] = {-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD,
-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, -40 DEG2RAD, -40 DEG2RAD};
switch (_joint->state)
{
case jstate_wait:
break;
case jstate_not_ready:
// Initialize velocity trajectory
//log_msg("Starting homing on joint %d", _joint->type);
_joint->state = jstate_pos_unknown;
start_trajectory_mag(_joint, f_magnitude[_joint->type], f_period[_joint->type]);
break;
case jstate_pos_unknown:
// Set desired joint trajectory
update_linear_sinusoid_position_trajectory(_joint);
break;
case jstate_hard_stop:
// Wait for all joints. No trajectory here.
break;
case jstate_homing1:
start_trajectory( _joint , DOF_types[_joint->type].home_position, 2.5 );
_joint->state = jstate_homing2;
case jstate_homing2:
// Move to start position
// Update position trajectory
if ( !update_position_trajectory(_joint) )
{
_joint->state = jstate_ready;
log_msg("Joint %s ready", jointIndexAndArmName(_joint->type).c_str());
}
break;
default:
// not doing joint homing.
break;
} // switch
return;
}
/**
* homing()
*
* \param _joint The joint being controlled.
*
* \brief Set trajectory behavior for each joint during the homing process.
*
* \todo Explain why sinusoid is used for homing???
*
* \todo Homing limits should be Amps not DAC units
*
* \todo Change square vs. diamond to a config-file based runtime system instead of #ifdef
*
* \ingroup Control
*/
void homing(struct DOF* _joint)
{
// duration for homing of each joint
const float f_period[MAX_MECH*MAX_DOF_PER_MECH] = {1, 1, 1, 9999999, 1, 1, 1, 1,
1, 1, 1, 9999999, 1, 1, 1, 1};
// degrees for homing of each joint
#ifdef RAVEN_II_SQUARE
//roll is backwards because of the 'click' in the mechanism
const float f_magnitude[MAX_MECH*MAX_DOF_PER_MECH] = {-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, -80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD,
-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, -80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD};
#else
#ifdef DV_ADAPTER
const float f_magnitude[MAX_MECH*MAX_DOF_PER_MECH] = {-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD,
-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD};
#else //default
const float f_magnitude[MAX_MECH*MAX_DOF_PER_MECH] = {-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD,
-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD};
#endif
#endif
switch (_joint->state)
{
case jstate_wait:
break;
case jstate_not_ready:
// Initialize velocity trajectory
//log_msg("Starting homing on joint %d", _joint->type);
_joint->state = jstate_pos_unknown;
start_trajectory_mag(_joint, f_magnitude[_joint->type], f_period[_joint->type]);
break;
case jstate_pos_unknown:
// Set desired joint trajectory
update_linear_sinusoid_position_trajectory(_joint);
break;
case jstate_hard_stop:
// Wait for all joints. No trajectory here.
break;
case jstate_homing1:
start_trajectory( _joint , DOF_types[_joint->type].home_position, 2.5 );
_joint->state = jstate_homing2;
case jstate_homing2:
// Move to start position
// Update position trajectory
if ( !update_position_trajectory(_joint) )
{
_joint->state = jstate_ready;
log_msg("Joint %d ready", _joint->type);
}
break;
default:
// not doing joint homing.
break;
} // switch
return;
}
/**
* \fn void homing(struct DOF* _joint, tool a_tool)
*
* \brief Set trajectory behavior for each tool joint during the homing process.
*
* \param _joint The joint being controlled.
* \param a_tool The tool being controlled.
*
* \ingroup Control
*
* \return void
*/
void homing(struct DOF* _joint, tool a_tool)
{
// duration for homing of each joint
const float f_period[MAX_MECH*MAX_DOF_PER_MECH] = {1, 1, 1, 9999999, 1, 1, 1, 1,
1, 1, 1, 9999999, 1, 1, 1, 1};
// // degrees for homing of each joint
//#ifdef RAVEN_II_SQUARE
// //roll is backwards because of the 'click' in the mechanism
// const float f_magnitude[MAX_MECH*MAX_DOF_PER_MECH] = {-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, -80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD,
// -10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, -80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD};
//#else
//#ifdef DV_ADAPTER
// const float f_magnitude[MAX_MECH*MAX_DOF_PER_MECH] = {-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD,
// -10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD};
//#else //default
// const float f_magnitude[MAX_MECH*MAX_DOF_PER_MECH] = {-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD,
// -10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD};
//#endif
//#endif
//check if scissors
int scissor = ((a_tool.t_end == mopocu_scissor) || (a_tool.t_end == potts_scissor))? 1 : 0;
float f_magnitude[MAX_MECH*MAX_DOF_PER_MECH] = {-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD,
-10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999, 80 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD, 40 DEG2RAD};
if(a_tool.t_style == square_raven){
if(a_tool.mech_type == GOLD_ARM) f_magnitude[TOOL_ROT_GOLD] = -80 DEG2RAD;
else if(a_tool.mech_type == GREEN_ARM) f_magnitude[TOOL_ROT_GREEN] = -80 DEG2RAD;
}
if (scissor){
if(a_tool.mech_type == GOLD_ARM) f_magnitude[GRASP2_GOLD] = -40 DEG2RAD;
else if(a_tool.mech_type == GREEN_ARM) f_magnitude[GRASP2_GREEN] = -40 DEG2RAD;
}
switch (_joint->state)
{
case jstate_wait:
break;
case jstate_not_ready:
// Initialize velocity trajectory
//log_msg("Starting homing on joint %d", _joint->type);
_joint->state = jstate_pos_unknown;
start_trajectory_mag(_joint, f_magnitude[_joint->type], f_period[_joint->type]);
break;
case jstate_pos_unknown:
// Set desired joint trajectory
update_linear_sinusoid_position_trajectory(_joint);
break;
case jstate_hard_stop:
// Wait for all joints. No trajectory here.
break;
case jstate_homing1:
start_trajectory( _joint , DOF_types[_joint->type].home_position, 2.5 );
_joint->state = jstate_homing2;
break;
case jstate_homing2:
// Move to start position
// Update position trajectory
if ( !update_position_trajectory(_joint) )
{
_joint->state = jstate_ready;
log_msg("Joint %d ready", _joint->type);
}
break;
default:
// not doing joint homing.
break;
} // switch
return;
}
/**
* \brief This function runs pi_control on joint velocity
* \param device0 is robot_device struct defined in DS0.h
* \param currParams is param_pass struct defined in DS1.h
* \return 0
*
* This function:
* 1. if pedal is down or RL_INIT and SL_AUTO_INIT, it Loops over all the joints and all the mechanisim to initialize
* velocity trajectory by calling start_trajectory() which is in trajectory.cpp
* 2. calls update_linear_sinusoid_velocity_trajectory() to get the desired joint velocities
* 3. calls jvel_PI_control() to run PI control, which is in pid_control.cpp
* 4. calls TorqueToDac() to apply torque on DAC
* It sets all the joint torques to zero if pedal is not down or not (RL_INIT and SL_AUTO_INIT)
*
*/
int raven_joint_velocity_control(struct device *device0, struct param_pass *currParams)
{
static int controlStart;
static unsigned long int delay=0;
// Run velocity control
if ( currParams->runlevel == RL_PEDAL_DN ||
( currParams->runlevel == RL_INIT &&
currParams->sublevel == SL_AUTO_INIT ))
{
// delay the start of control for 300ms b/c the amps have to turn on.
if (gTime - delay < 800)
return 0;
for (int i=0; i < NUM_MECH; i++)
{
for (int j = 0; j < MAX_DOF_PER_MECH; j++)
{
struct DOF * _joint = &(device0->mech[i].joint[j]);
if (device0->mech[i].type == GOLD_ARM)
{
// initialize velocity trajectory
if (!controlStart)
start_trajectory(_joint);
// Get the desired joint velocities
update_linear_sinusoid_velocity_trajectory(_joint);
// Run PI control
jvel_PI_control(_joint, !controlStart);
}
else
{
_joint->tau_d = 0;
}
}
}
if (!controlStart)
controlStart = 1;
// Convert joint torque to DAC value.
TorqueToDAC(device0);
}
else
{
delay=gTime;
controlStart = 0;
for (int i=0; i < NUM_MECH; i++)
for (int j = 0; j < MAX_DOF_PER_MECH; j++)
{
device0->mech[i].joint[j].tau_d=0;
}
TorqueToDAC(device0);
}
return 0;
}
/**
* \brief This function applies a sinusoidal trajectory to all joints
* \param device0 is robot_device struct defined in DS0.h
* \param currParams is param_pass struct defined in DS1.h
* \return 0
*
* This function:
* 1. returns 0 if not in pedal down or init.init (do nothing)
* 2. it sets trajectory on all the joints
* 3. calls the invCableCoupling() to calculate inverse cable coupling
* 4. calls the mpos_PD_control() to run the PD control law
* 5. calls TorqueToDAC() to apply write torque value's on DAC
*
*/
int raven_sinusoidal_joint_motion(struct device *device0, struct param_pass *currParams){
static int controlStart = 0;
static unsigned long int delay=0;
// modify the period and magnitude / sep 15, xiao li
const float f_period[8] = {6, 7, 4, 9999999, 10, 5, 10, 6};
//const float f_period[8] = {6, 7, 10, 9999999, 5, 5, 10, 6};
// const float f_magnitude[8] = {10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999,
//30 DEG2RAD, 30 DEG2RAD, 30 DEG2RAD, 30 DEG2RAD};
const float f_magnitude[8] = {10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999,
0 DEG2RAD, 0 DEG2RAD, 0 DEG2RAD, 0 DEG2RAD};
// If we're not in pedal down or init.init then do nothing.
if (! ( currParams->runlevel == RL_INIT && currParams->sublevel == SL_AUTO_INIT ))
{
controlStart = 0;
delay = gTime;
// Set all joints to zero torque, and mpos_d = mpos
for (int i=0; i < NUM_MECH; i++)
{
for (int j = 0; j < MAX_DOF_PER_MECH; j++)
{
struct DOF* _joint = &(device0->mech[i].joint[j]);
_joint->mpos_d = _joint->mpos;
_joint->jpos_d = _joint->jpos;
_joint->tau_d = 0;
}
}
return 0;
}
// Wait for amplifiers to power up
if (gTime - delay < 800)
return 0;
// Set trajectory on all the joints
for (int i=0; i < NUM_MECH; i++)
{
for (int j = 0; j < MAX_DOF_PER_MECH; j++)
{
struct DOF * _joint = &(device0->mech[i].joint[j]);
int sgn = 1;
if (device0->mech[i].type == GREEN_ARM)
sgn = -1;
// initialize trajectory
if (!controlStart)
start_trajectory(_joint, (_joint->jpos + sgn*f_magnitude[j]), f_period[j]);
// Get trajectory update
update_sinusoid_position_trajectory(_joint);
}
}
//Inverse Cable Coupling
invCableCoupling(device0, currParams->runlevel);
// Do PD control on all the joints
for (int i=0; i < NUM_MECH; i++)
{
for (int j = 0; j < MAX_DOF_PER_MECH; j++)
{
struct DOF * _joint = &(device0->mech[i].joint[j]);
// Do PD control
mpos_PD_control(_joint);
// if (is_toolDOF(_joint))
// _joint->tau_d = 0;
}
}
TorqueToDAC(device0);
controlStart = 1;
return 0;
}
