/** * \brief This function runs pd_control on motor position. * \param device0 robot_device struct defined in DS0.h * \param currParams param_pass struct defined in DS1.h * \return -1 if Pedal is up and 0 when torque is applied to DAC * * This function: * 1. calls the r2_inv_kin() to calculate the inverse kinematics * 2. call the invCableCoupling() to calculate the inverse cable coupling * 3. set all the joints to zero if pedal is not down otherwise it calls mpos_PD_control() to run the PD control law * 4. calls getGravityTorque() to calulate gravity torques on each joints. * 5. calls TorqueToDAC() to apply write torque value's on DAC * */ int raven_cartesian_space_command(struct device *device0, struct param_pass *currParams){ struct DOF *_joint = NULL; struct mechanism* _mech = NULL; int i=0,j=0; if (currParams->runlevel < RL_PEDAL_UP) { return -1; } else if (currParams->runlevel < RL_PEDAL_DN) { set_posd_to_pos(device0); updateMasterRelativeOrigin(device0); } parport_out(0x01); //Inverse kinematics r2_inv_kin(device0, currParams->runlevel); //Inverse Cable Coupling invCableCoupling(device0, currParams->runlevel); // Set all joints to zero torque _mech = NULL; _joint = NULL; while (loop_over_joints(device0, _mech, _joint, i,j) ) { if (currParams->runlevel != RL_PEDAL_DN) { _joint->tau_d=0; } else { mpos_PD_control(_joint); } } // Gravity compensation calculation getGravityTorque(*device0, *currParams); _mech = NULL; _joint = NULL; while ( loop_over_joints(device0, _mech, _joint, i,j) ) { _joint->tau_d += _joint->tau_g; // Add gravity torque } TorqueToDAC(device0); return 0; }
/** * 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; }
/**\ * \brief This function runs PD control on motor position * \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. checks to see if it's in pedal down mode, if not it sets all joints to zero torque and return 0 * 2. set trajectory on all joints * 3. calls invCableCoupling() to calculate inverse cable coupling * 4. calls mpos_PD_control() to perform PD control * 5. calls TorqueToDac() to apply torque on DAC */ int raven_motor_position_control(struct device *device0, struct param_pass *currParams) { static int controlStart = 0; static unsigned long int delay=0; struct DOF *_joint = NULL; struct mechanism* _mech = NULL; int i=0,j=0; // If we're not in pedal down or init.init then do nothing. if (! ( currParams->runlevel == RL_PEDAL_DN || ( currParams->runlevel == RL_INIT && currParams->sublevel == SL_AUTO_INIT )) ) { controlStart = 0; delay = gTime; // Set all joints to zero torque, and mpos_d = mpos _mech = NULL; _joint = NULL; while (loop_over_joints(device0, _mech, _joint, i,j) ) { _joint->mpos_d = _joint->mpos; _joint->tau_d = 0; } return 0; } if (gTime - delay < 800) return 0; // Set trajectory on all the joints _mech = NULL; _joint = NULL; while (loop_over_joints(device0, _mech, _joint, i,j) ) { if ( _joint->type == SHOULDER_GOLD || _joint->type == ELBOW_GOLD ) _joint->jpos_d = _joint->jpos; if (!controlStart) _joint->jpos_d = _joint->jpos; } //Inverse Cable Coupling invCableCoupling(device0, currParams->runlevel); // Do PD control on all the joints _mech = NULL; _joint = NULL; while (loop_over_joints(device0, _mech, _joint, i,j) ) { // Do PD control mpos_PD_control(_joint); if (_joint->type < Z_INS_GOLD) _joint->tau_d=0; else if (gTime % 500 == 0 && _joint->type == Z_INS_GOLD) log_msg("zp: %f, \t zp_d: %f, \t mp: %f, \t mp_d:%f", _joint->jpos, _joint->jpos_d, _joint->mpos, _joint->mpos_d); } TorqueToDAC(device0); controlStart = 1; 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; }