/** * 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; }