View Single Post
  #27   Spotlight this post!  
Unread 17-01-2012, 19:03
jwakeman jwakeman is offline
Registered User
FRC #0063 (Red Barons)
Team Role: Mentor
 
Join Date: Jan 2011
Rookie Year: 2010
Location: 16510
Posts: 182
jwakeman is just really nicejwakeman is just really nicejwakeman is just really nicejwakeman is just really nicejwakeman is just really nice
Re: New to 2CAN and Jaguar

I will be working with the Jags tomorrow night. I can try to see if I see the same issue as you.

Quote:
Originally Posted by mikets View Post
I am not familiar with the internal PID algorithm of the Jags...
The source code for the Jags is open. Here is the PID algorithm for fun:

Code:
//*****************************************************************************
//
// This function will execute another iteration of the PID algorithm.  In
// order to get reliable results from this, the sampled values passed in must
// be captured at fixed intervals (as close as possible).  Deviations from a
// fixed capture interval will result in errors in the control output.
//
//*****************************************************************************
long
PIDUpdate(tPIDState *psState, long lError)
{
    long long llOutput;
    long lOutput;

    //
    // Update the error integrator.
    //
    if((psState->lIntegrator & 0x80000000) == (lError & 0x80000000))
    {
        //
        // Add the error to the integrator.
        //
        psState->lIntegrator += lError;

        //
        // Since the sign of the integrator and error matched before the above
        // addition, if the signs no longer match it is because the integrator
        // rolled over.  In this case, saturate appropriately.
        //
        if((lError < 0) && (psState->lIntegrator > 0))
        {
            psState->lIntegrator = psState->lIntegMin;
        }
        if((lError > 0) && (psState->lIntegrator < 0))
        {
            psState->lIntegrator = psState->lIntegMax;
        }
    }
    else
    {
        //
        // Add the error to the integrator.
        //
        psState->lIntegrator += lError;
    }

    //
    // Saturate the integrator if necessary.
    //
    if(psState->lIntegrator > psState->lIntegMax)
    {
        psState->lIntegrator = psState->lIntegMax;
    }
    if(psState->lIntegrator < psState->lIntegMin)
    {
        psState->lIntegrator = psState->lIntegMin;
    }

    //
    // Compute the new control value.
    //
    llOutput = (((long long)psState->lPGain * (long long)lError) +
                ((long long)psState->lIGain *
                 (long long)psState->lIntegrator) +
                ((long long)psState->lDGain *
                 (long long)(lError - psState->lPrevError)));

    //
    // Clip the new control value as appropriate.
    //
    if(llOutput > (long long)0x7fffffffffff)
    {
        lOutput = 0x7fffffff;
    }
    else if(llOutput < (long long)0xffff800000000000)
    {
        lOutput = 0x80000000;
    }
    else
    {
        lOutput = (llOutput >> 16) & 0xffffffff;
    }

    //
    // Save the current error for computing the derivitive on the next
    // iteration.
    //
    psState->lPrevError = lError;

    //
    // Return the control value.
    //
    return(lOutput);
}
Reply With Quote