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.

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

The «goal pose» represents the position that the robot should be at a particular timestamp when tracking the trajectory. It does NOT represent the trajectory’s endpoint.

El controlador puede ser actualizado usando el método Calculate (C++) / calculate (Java). Existen 2 sobrecargas de este método. Ambas de estas sobrecargas aceptan la posiciones actuales como el primer parámetro y el nombre deseado para el último parámetro. Para los parámetros del medio, una sobre carga acepta la pose deseada y la referencia de velocidad lineal mientras que la otra acepta el objeto Trajectory.State, que contiene información sobre la pose meta. El último método es preferible para las trayectorias de rastreo.

// 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);

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);

Porque estos módulos swerve son aún velocidades y ángulos, se necesitará usar controladores PID para marcar estos ángulos y velocidades.