Control PID en WPILib

Nota

This article focuses on in-code implementation of PID control in WPILib. For a conceptual explanation of the working of a PIDController, see Introducción a PID

Nota

Para una guía en implementar controles PID a través de un marco de referencia base comandos, ver Control PID a través de subsistemas PID y comandos PID.

WPILib supports PID control of mechanisms through the PIDController class (Java, C++, Python). This class handles the feedback loop calculation for the user, as well as offering methods for returning the error, setting tolerances, and checking if the control loop has reached its setpoint within the specified tolerances.

Usando la Clase PIDController

Construyendo un PIDController

Nota

Mientras que PIDController puede ser usado de manera asincronica, este no provee ninguna función de seguridad de hilos - el asegurar una operación segura para hilos se deja completamente al usuario, y de este modo el uso asincronico es recomendado solamente para equipos avanzados.

Para usar las funcionalidades de control PID de WPILib, los usuarios deben primero construir un objeto PIDController con las ganancias deseadas:

// Creates a PIDController with gains kP, kI, and kD
PIDController pid = new PIDController(kP, kI, kD);

// Creates a PIDController with gains kP, kI, and kD
frc::PIDController pid{kP, kI, kD};

from wpimath.controller import PIDController

# Creates a PIDController with gains kP, kI, and kD
pid = PIDController(kP, kI, kD)

Un cuarto parámetro opcional puede proveerse al constructor, especificando el periodo en el cual el controlador será ejecutado. El objeto PIDController está echo principalmente para uso sincrónico desde el loop principal del robot, y este valor está puesto por default a 20ms.

Usando la salida del loop de retroalimentación.

Nota

El PIDController asume que el método calculate() es llamado regularmente en un intervalo consistente con el periodo configurado. La falla en esto resultará un resultado no deseado en el comportamiento del loop.

Usar un construido PIDController es simple: Solo llame el método calculate() desde el loop principal del robot (ejemplo: El método autonomousPeriodic() del robot)

// Calculates the output of the PID algorithm based on the sensor reading
// and sends it to a motor
motor.set(pid.calculate(encoder.getDistance(), setpoint));

// Calculates the output of the PID algorithm based on the sensor reading
// and sends it to a motor
motor.Set(pid.Calculate(encoder.GetDistance(), setpoint));

# Calculates the output of the PID algorithm based on the sensor reading
# and sends it to a motor
motor.set(pid.calculate(encoder.getDistance(), setpoint))

Checando errores

Nota

getPositionError() y getVelocityError() son nombrados asumiendo que el loop está controlando una posición - para un loop que está controlando velocidad, estos regresan el error de velocidad y el error de la aceleración, respectivamente.

El error actual del proceso medido se regresa por medio de la función getPositionError(), mientras que su derivada regresa por medio de la función getVelocityError():

Especificando y checando tolerancias

Nota

Si solo una tolerancia de posición es especificada, la tolerancia de velocidad se pone como default a infinito.

Nota

Tal como arriba, «position» se refiere al proceso de medición variable, y «velocity» a su derivada - de esta forma, para un loop de velocidad, estos son, de hecho, velocidad y aceleración respectivamente.

Ocasionalmente, es útil saber si un controlador ha rastreado el setpoint a una tolerancia dada - por ejemplo, para determinar si un comando debería ser finalizado, o (mientras siguiendo un perfil de movimiento) si el movimiento está siendo impedido y necesita ser planeado otra vez.

Para hacer esto, primero necesitamos especificar las tolerancias con el método setTolerance(); entonces, podemos checarlo con el método atSetpoint().

// Sets the error tolerance to 5, and the error derivative tolerance to 10 per second
pid.setTolerance(5, 10);

// Returns true if the error is less than 5 units, and the
// error derivative is less than 10 units
pid.atSetpoint();

// Sets the error tolerance to 5, and the error derivative tolerance to 10 per second
pid.SetTolerance(5, 10);

// Returns true if the error is less than 5 units, and the
// error derivative is less than 10 units
pid.AtSetpoint();

# Sets the error tolerance to 5, and the error derivative tolerance to 10 per second
pid.setTolerance(5, 10)

# Returns true if the error is less than 5 units, and the
# error derivative is less than 10 units
pid.atSetpoint()

Reiniciando el controlador

Algunas veces es deseable aclarar el estado interno (más importante, el acumulador integral) de un PIDController, teniendo en cuenta que puede que ya no sea valido (ejemplo: cuando el PIDController ha sido desactivado y luego reactivado). Esto puede lograrse llamando el método reset().

Ajustando un valor integrador máximo.

Nota

Los integradores introducen inestabilidad e histéresis al sistema de retroalimentación del loop. Se recomienda fuertemente que los equipos eviten usar ganancias integrales a menos de que ninguna otra opción sirva - muy a menudo, los problemas que pueden ser resueltos con un integrador pueden ser resueltos por medio del uso de un feedforward más preciso.

A typical problem encountered when using integral feedback is excessive «wind-up» causing the system to wildly overshoot the setpoint. This can be alleviated in a number of ways - the WPILib PIDController class enforces an integrator range limiter to help teams overcome this issue.

By default, the total output contribution from the integral gain is limited to be between -1.0 and 1.0.

The range limits may be increased or decreased using the setIntegratorRange() method.

// The integral gain term will never add or subtract more than 0.5 from
// the total loop output
pid.setIntegratorRange(-0.5, 0.5);

// The integral gain term will never add or subtract more than 0.5 from
// the total loop output
pid.SetIntegratorRange(-0.5, 0.5);

# The integral gain term will never add or subtract more than 0.5 from
# the total loop output
pid.setIntegratorRange(-0.5, 0.5)

Disabling Integral Gain if the Error is Too High

Another way integral «wind-up» can be alleviated is by limiting the error range where integral gain is active. This can be achieved by setting IZone. If the error is more than IZone, the total accumulated error is reset, disabling integral gain. When the error is equal to or less than IZone, integral gain is enabled.

By default, IZone is disabled.

IZone may be set using the setIZone() method. To disable it, set it to infinity.

// Disable IZone
pid.setIZone(Double.POSITIVE_INFINITY);

// Integral gain will not be applied if the absolute value of the error is
// more than 2
pid.setIZone(2);

// Disable IZone
pid.SetIZone(std::numeric_limits<double>::infinity());

// Integral gain will not be applied if the absolute value of the error is
// more than 2
pid.SetIZone(2);

# Disable IZone
pid.setIZone(math.inf)

# Integral gain will not be applied if the absolute value of the error is
# more than 2
pid.setIZone(2)

Ajustando entradas continuas

Advertencia

Si su mecanismo no es capaz de usar un movimiento de rotación continua completo (ejemplo: una torreta con cables que se giran mientras rota), no active la entrada continua a menos de que tenga implementada una característica de seguridad adicional para prevenir que el mecanismo se mueva más allá de sus limites.

Advertencia

La función de entrada continua no envuelve automáticamente sus valores de entrada - asegúrese que sus valores de entrada, mientras use esta función, nunca estén fuera del rango establecido.

Some process variables (such as the angle of a turret) are measured on a circular scale, rather than a linear one - that is, each «end» of the process variable range corresponds to the same point in reality (e.g. 360 degrees and 0 degrees). In such a configuration, there are two possible values for any given error, corresponding to which way around the circle the error is measured. It is usually best to use the smaller of these errors.

Para configurar un PIDController para automáticamente hacer esto, use el método enableContinuousInput():

// Enables continuous input on a range from -180 to 180
pid.enableContinuousInput(-180, 180);

// Enables continuous input on a range from -180 to 180
pid.EnableContinuousInput(-180, 180);

# Enables continuous input on a range from -180 to 180
pid.enableContinuousInput(-180, 180)

Salida del controlador de sujeción

// Clamps the controller output to between -0.5 and 0.5
MathUtil.clamp(pid.calculate(encoder.getDistance(), setpoint), -0.5, 0.5);

// Clamps the controller output to between -0.5 and 0.5
std::clamp(pid.Calculate(encoder.GetDistance(), setpoint), -0.5, 0.5);

# Python doesn't have a builtin clamp function
def clamp(v, minval, maxval):
    return max(min(v, maxval), minval)

# Clamps the controller output to between -0.5 and 0.5
clamp(pid.calculate(encoder.getDistance(), setpoint), -0.5, 0.5)