Exemplo n.º 1
0
/**
*  \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;
}
Exemplo n.º 2
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;
}
Exemplo n.º 3
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;
}
Exemplo n.º 4
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;
}