Exemple #1
0
short
PidControllerUpdate( pidController *p, long sensor_value )
{
    if( p == NULL )
        return(0);

    if( p->enabled )
        {

        p->sensor_value = sensor_value;
        p->error = p->target_value - p->sensor_value;

        // force error to 0 if below threshold
        if( abs(p->error) < p->error_threshold )
            p->error = 0;

        // integral accumulation
        if( p->Ki != 0 )
            {
            p->integral += p->error;

            // limit to avoid windup
            if( abs( p->integral ) > p->integral_limit )
                p->integral = sgn(p->integral) * p->integral_limit;
            }
        else
            p->integral = 0;

        // derivative
        p->derivative = p->error - p->last_error;
        p->last_error = p->error;

        // calculate drive - no delta T in this version
        p->drive = (p->Kp * p->error) + (p->Ki * p->integral) + (p->Kd * p->derivative) + p->Kbias;

        // drive should be in the range +/- 1.0
        if( abs( p->drive ) > 1.0 )
            p->drive = sgn(p->drive);

        // final motor output
        p->drive_raw = p->drive * 127.0;
        }

    else
        {
        // Disabled - all 0
        p->error      = 0;
        p->last_error = 0;
        p->integral   = 0;
        p->derivative = 0;
        p->drive      = 0.0;
        p->drive_raw  = 0;
        }

    // linearize - be careful this is a macro
    p->drive_cmd = _LinearizeDrive( p->drive_raw );

    // return the thing we are really interested in
    return( p->drive_cmd );
}
Exemple #2
0
int16_t
PidControllerUpdate( pidController *p )
{
    if( p == NULL )
        return(0);

    if( p->enabled )
        {
        // check for sensor port
        // otherwise externally calculated error
        if( p->sensor_port >= 0 )
            {
            // Get raw position value, may be pot or encoder
            p->sensor_value = vexSensorValueGet( p->sensor_port );

            // A reversed sensor ?
            if( p->sensor_reverse )
                {
                if( vexSensorIsAnalog( p->sensor_port) )
                    // reverse pot
                    p->sensor_value = 4095 - p->sensor_value;
                else
                    // reverse encoder
                    p->sensor_value = -p->sensor_value;
                }

            p->error = p->target_value - p->sensor_value;
            }

        // force error to 0 if below threshold
        if( fabs(p->error) < p->error_threshold )
            p->error = 0;

        // integral accumulation
        if( p->Ki != 0 )
            {
            p->integral += p->error;

            // limit to avoid windup
            if( fabs( p->integral ) > p->integral_limit )
                p->integral = sgn(p->integral) * p->integral_limit;
            }
        else
            p->integral = 0;

        // derivative
        p->derivative = p->error - p->last_error;
        p->last_error = p->error;

        // calculate drive - no delta T in this version
        p->drive = (p->Kp * p->error) + (p->Ki * p->integral) + (p->Kd * p->derivative) + p->Kbias;

        // drive should be in the range +/- 1.0
        if( fabs( p->drive ) > 1.0 )
            p->drive = sgn(p->drive);

        // final motor output
        p->drive_raw = p->drive * 127.0;
        }

    else
        {
        // Disabled - all 0
        p->error      = 0;
        p->last_error = 0;
        p->integral   = 0;
        p->derivative = 0;
        p->drive      = 0.0;
        p->drive_raw  = 0;
        }

    // linearize - be careful this is a macro
    p->drive_cmd = _LinearizeDrive( p->drive_raw );

    // return the thing we are really interested in
    return( p->drive_cmd );
}