Controlador de impulsión holonómico
El controlador de propulsión holonómico es un rastreador de trayectoria para robots con transmisiones holonómicas (por ejemplo, swerve, mecanum, etc.). Esto se puede usar para rastrear trayectorias con precisión con corrección para pequeñas perturbaciones.
Construcción de un controlador de impulsión holonómico
El controlador de accionamiento holonómico debe instanciarse con 2 controladores PID y 1 controlador PID perfilado.
Nota
Para obtener más información sobre el control PID, consulte Control PID en WPILib.
Los 2 controladores PID son controladores que deben corregir el error en las direcciones X e Y relativas al campo, respectivamente. Por ejemplo, si los primeros 2 argumentos son PIDController(1, 0, 0) y PIDController(1.2, 0, 0) respectivamente, el controlador holonómico agregará un metro adicional por segundo en la x dirección por cada metro de error en la dirección x y agregará 1,2 metros adicionales por segundo en la dirección y por cada metro de error en la dirección y.
El último parámetro es un ProfiledPIDController para la rotación del robot. Debido a que la dinámica de rotación de un tren motriz holonómico está desacoplada del movimiento en las direcciones x,y, los usuarios pueden establecer referencias de rumbo personalizadas mientras siguen una trayectoria. Estas referencias de encabezado se perfilan de acuerdo con los parámetros establecidos en el ProfiledPIDController.
var controller = new HolonomicDriveController(
new PIDController(1, 0, 0), new PIDController(1, 0, 0),
new ProfiledPIDController(1, 0, 0,
new TrapezoidProfile.Constraints(6.28, 3.14)));
// Here, our rotation profile constraints were a max velocity
// of 1 rotation per second and a max acceleration of 180 degrees
// per second squared.
frc::HolonomicDriveController controller{
frc::PIDController{1, 0, 0}, frc::PIDController{1, 0, 0},
frc::ProfiledPIDController<units::radian>{
1, 0, 0, frc::TrapezoidProfile<units::radian>::Constraints{
6.28_rad_per_s, 3.14_rad_per_s / 1_s}}};
// Here, our rotation profile constraints were a max velocity
// of 1 rotation per second and a max acceleration of 180 degrees
// per second squared.
from wpimath.controller import (
HolonomicDriveController,
PIDController,
ProfiledPIDControllerRadians,
)
from wpimath.trajectory import TrapezoidProfileRadians
controller = HolonomicDriveController(
PIDController(1, 0, 0),
PIDController(1, 0, 0),
ProfiledPIDControllerRadians(
1, 0, 0, TrapezoidProfileRadians.Constraints(6.28, 3.14)
),
)
# Here, our rotation profile constraints were a max velocity
# of 1 rotation per second and a max acceleration of 180 degrees
# per second squared.
Obtener velocidades ajustadas
El controlador de impulsión holonómico devuelve «velocidades ajustadas» de modo que cuando el robot sigue estas velocidades, alcanza con precisión el punto objetivo. El controlador debe actualizarse periódicamente con el nuevo objetivo. El objetivo se compone de una pose deseada, velocidad lineal y rumbo.
Nota
La «pose objetivo» representa la posición en la que debería estar el robot en un momento determinado al seguir la trayectoria. NO representa el punto final de la trayectoria.
El controlador puede ser actualizado usando el método Calculate (C++) / calculate (Java/Python). Existen 2 sobrecargas de este método. Ambas de estas sobrecargas aceptan la posición actual del robot como el primer parámetro, y la orientación deseada en el segundo parámetro. Para los parámetros de en medio, una sobrecarga acepta la posición deseada y la referencia de velocidad linear mientras que otra acepta un objeto Trajectory.State, el cual contiene información sobre la posición deseada. El ultimo método es preferible para rastrear trayectorias
// Sample the trajectory at 3.4 seconds from the beginning.
Trajectory.State goal = trajectory.sample(3.4);
// Get the adjusted speeds. Here, we want the robot to be facing
// 70 degrees (in the field-relative coordinate system).
ChassisSpeeds adjustedSpeeds = controller.calculate(
currentRobotPose, goal, Rotation2d.fromDegrees(70.0));
// Sample the trajectoty at 3.4 seconds from the beginning.
const auto goal = trajectory.Sample(3.4_s);
// Get the adjusted speeds. Here, we want the robot to be facing
// 70 degrees (in the field-relative coordinate system).
const auto adjustedSpeeds = controller.Calculate(
currentRobotPose, goal, 70_deg);
from wpimath.geometry import Rotation2d
# Sample the trajectory at 3.4 seconds from the beginning.
goal = trajectory.sample(3.4)
# Get the adjusted speeds. Here, we want the robot to be facing
# 70 degrees (in the field-relative coordinate system).
adjustedSpeeds = controller.calculate(
currentRobotPose, goal, Rotation2d.fromDegrees(70.0)
)
Usando velocidades ajustadas.
Las velocidades ajustadas son un tipo de ChassisSpeeds, que contiene un vx (velocidad lineal en dirección de avance), a vy (velocidad lineal en direcciones laterales), y omega (velocidad angular al rededor del centro del marco del robot).
La velocidad ajustada que nos da puede ser convertida en velocidad utilizable usando clases de cinemática para su chasis. En el código de ejemplo de abajo, podemos asumir un robot con swerve drive; de todas formas, el codigo cinemático es exactamente igual para un robot con mecanum drive usando MecanumDriveKinematics.
SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(adjustedSpeeds);
SwerveModuleState frontLeft = moduleStates[0];
SwerveModuleState frontRight = moduleStates[1];
SwerveModuleState backLeft = moduleStates[2];
SwerveModuleState backRight = moduleStates[3];
auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(adjustedSpeeds);
fl, fr, bl, br = kinematics.toSwerveModuleStates(adjustedSpeeds)
Porque estos módulos swerve son aún velocidades y ángulos, se necesitará usar controladores PID para marcar estos ángulos y velocidades.