Paso 4: Creando y Siguiendo una trayectoria

Con nuestro subsistema de accionamiento escrito, es hora de generar una trayectoria y escribir un comando autónomo para seguirla.

As per the standard command-based project structure, we will do this in the getAutonomousCommand method of the RobotContainer class. The full method from the RamseteCommand Example Project (Java, C++) can be seen below. The rest of the article will break down the different parts of the method in more detail.

 74  /**
 75   * Use this to pass the autonomous command to the main {@link Robot} class.
 76   *
 77   * @return the command to run in autonomous
 78   */
 79  public Command getAutonomousCommand() {
 80    // Create a voltage constraint to ensure we don't accelerate too fast
 81    var autoVoltageConstraint =
 82        new DifferentialDriveVoltageConstraint(
 83            new SimpleMotorFeedforward(
 84                DriveConstants.ksVolts,
 85                DriveConstants.kvVoltSecondsPerMeter,
 86                DriveConstants.kaVoltSecondsSquaredPerMeter),
 87            DriveConstants.kDriveKinematics,
 88            10);
 89
 90    // Create config for trajectory
 91    TrajectoryConfig config =
 92        new TrajectoryConfig(
 93                AutoConstants.kMaxSpeedMetersPerSecond,
 94                AutoConstants.kMaxAccelerationMetersPerSecondSquared)
 95            // Add kinematics to ensure max speed is actually obeyed
 96            .setKinematics(DriveConstants.kDriveKinematics)
 97            // Apply the voltage constraint
 98            .addConstraint(autoVoltageConstraint);
 99
100    // An example trajectory to follow. All units in meters.
101    Trajectory exampleTrajectory =
102        TrajectoryGenerator.generateTrajectory(
103            // Start at the origin facing the +X direction
104            new Pose2d(0, 0, new Rotation2d(0)),
105            // Pass through these two interior waypoints, making an 's' curve path
106            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
107            // End 3 meters straight ahead of where we started, facing forward
108            new Pose2d(3, 0, new Rotation2d(0)),
109            // Pass config
110            config);
111
112    RamseteCommand ramseteCommand =
113        new RamseteCommand(
114            exampleTrajectory,
115            m_robotDrive::getPose,
116            new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
117            new SimpleMotorFeedforward(
118                DriveConstants.ksVolts,
119                DriveConstants.kvVoltSecondsPerMeter,
120                DriveConstants.kaVoltSecondsSquaredPerMeter),
121            DriveConstants.kDriveKinematics,
122            m_robotDrive::getWheelSpeeds,
123            new PIDController(DriveConstants.kPDriveVel, 0, 0),
124            new PIDController(DriveConstants.kPDriveVel, 0, 0),
125            // RamseteCommand passes volts to the callback
126            m_robotDrive::tankDriveVolts,
127            m_robotDrive);
128
129    // Reset odometry to the initial pose of the trajectory, run path following
130    // command, then stop at the end.
131    return Commands.runOnce(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose()))
132        .andThen(ramseteCommand)
133        .andThen(Commands.runOnce(() -> m_robotDrive.tankDriveVolts(0, 0)));
134  }
135}

45frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
46  // Create a voltage constraint to ensure we don't accelerate too fast
47  frc::DifferentialDriveVoltageConstraint autoVoltageConstraint{
48      frc::SimpleMotorFeedforward<units::meters>{
49          DriveConstants::ks, DriveConstants::kv, DriveConstants::ka},
50      DriveConstants::kDriveKinematics, 10_V};
51
52  // Set up config for trajectory
53  frc::TrajectoryConfig config{AutoConstants::kMaxSpeed,
54                               AutoConstants::kMaxAcceleration};
55  // Add kinematics to ensure max speed is actually obeyed
56  config.SetKinematics(DriveConstants::kDriveKinematics);
57  // Apply the voltage constraint
58  config.AddConstraint(autoVoltageConstraint);
59
60  // An example trajectory to follow.  All units in meters.
61  auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
62      // Start at the origin facing the +X direction
63      frc::Pose2d{0_m, 0_m, 0_deg},
64      // Pass through these two interior waypoints, making an 's' curve path
65      {frc::Translation2d{1_m, 1_m}, frc::Translation2d{2_m, -1_m}},
66      // End 3 meters straight ahead of where we started, facing forward
67      frc::Pose2d{3_m, 0_m, 0_deg},
68      // Pass the config
69      config);
70
71  frc2::CommandPtr ramseteCommand{frc2::RamseteCommand(
72      exampleTrajectory, [this] { return m_drive.GetPose(); },
73      frc::RamseteController{AutoConstants::kRamseteB,
74                             AutoConstants::kRamseteZeta},
75      frc::SimpleMotorFeedforward<units::meters>{
76          DriveConstants::ks, DriveConstants::kv, DriveConstants::ka},
77      DriveConstants::kDriveKinematics,
78      [this] { return m_drive.GetWheelSpeeds(); },
79      frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
80      frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
81      [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
82      {&m_drive})};
83
84  // Reset odometry to the initial pose of the trajectory, run path following
85  // command, then stop at the end.
86  return frc2::cmd::RunOnce(
87             [this, initialPose = exampleTrajectory.InitialPose()] {
88               m_drive.ResetOdometry(initialPose);
89             },
90             {})
91      .AndThen(std::move(ramseteCommand))
92      .AndThen(
93          frc2::cmd::RunOnce([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
94}

Configurando las Restricciones de Trayectoria

Primero, debemos establecer algunos parámetros de configuración para la trayectoria que aseguren que la trayectoria generada sea seguible.

Creando una Restricción de Voltaje

La primera pieza de configuración que necesitaremos es una restricción de voltaje. Esto asegurará que la trayectoria generada nunca ordene al robot ir más rápido de lo que es capaz de lograr con el suministro de voltaje dado:

80    // Create a voltage constraint to ensure we don't accelerate too fast
81    var autoVoltageConstraint =
82        new DifferentialDriveVoltageConstraint(
83            new SimpleMotorFeedforward(
84                DriveConstants.ksVolts,
85                DriveConstants.kvVoltSecondsPerMeter,
86                DriveConstants.kaVoltSecondsSquaredPerMeter),
87            DriveConstants.kDriveKinematics,
88            10);

46  // Create a voltage constraint to ensure we don't accelerate too fast
47  frc::DifferentialDriveVoltageConstraint autoVoltageConstraint{
48      frc::SimpleMotorFeedforward<units::meters>{
49          DriveConstants::ks, DriveConstants::kv, DriveConstants::ka},
50      DriveConstants::kDriveKinematics, 10_V};

Nótese que fijamos el votaje máximo a 10V, en lugar del voltaje nominal de la batería de 12V. Esto nos da un poco de «margen» para lidiar con la «caída de voltaje» durante la operación.

Creando la Configuración

Ahora que tenemos nuestra restricción de voltaje, podemos crear nuestra instancia TrajectoryConfig, que envuelve todas nuestras restricciones de camino:

90    // Create config for trajectory
91    TrajectoryConfig config =
92        new TrajectoryConfig(
93                AutoConstants.kMaxSpeedMetersPerSecond,
94                AutoConstants.kMaxAccelerationMetersPerSecondSquared)
95            // Add kinematics to ensure max speed is actually obeyed
96            .setKinematics(DriveConstants.kDriveKinematics)
97            // Apply the voltage constraint
98            .addConstraint(autoVoltageConstraint);

52  // Set up config for trajectory
53  frc::TrajectoryConfig config{AutoConstants::kMaxSpeed,
54                               AutoConstants::kMaxAcceleration};
55  // Add kinematics to ensure max speed is actually obeyed
56  config.SetKinematics(DriveConstants::kDriveKinematics);
57  // Apply the voltage constraint
58  config.AddConstraint(autoVoltageConstraint);

Generando la Trayectoria

Con la configuración de nuestra trayectoria en mano, estamos listos para generar nuestra trayectoria. Para este ejemplo, generaremos una trayectoria «clamped cubic» - esto significa que especificaremos las posiciones completas del robot en los puntos finales, y las posiciones sólo para los waypoints interiores (también conocidos como «knot points»). Como en cualquier otro lugar, todas las distancias están en metros.

100    // An example trajectory to follow. All units in meters.
101    Trajectory exampleTrajectory =
102        TrajectoryGenerator.generateTrajectory(
103            // Start at the origin facing the +X direction
104            new Pose2d(0, 0, new Rotation2d(0)),
105            // Pass through these two interior waypoints, making an 's' curve path
106            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
107            // End 3 meters straight ahead of where we started, facing forward
108            new Pose2d(3, 0, new Rotation2d(0)),
109            // Pass config
110            config);

60  // An example trajectory to follow.  All units in meters.
61  auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
62      // Start at the origin facing the +X direction
63      frc::Pose2d{0_m, 0_m, 0_deg},
64      // Pass through these two interior waypoints, making an 's' curve path
65      {frc::Translation2d{1_m, 1_m}, frc::Translation2d{2_m, -1_m}},
66      // End 3 meters straight ahead of where we started, facing forward
67      frc::Pose2d{3_m, 0_m, 0_deg},
68      // Pass the config
69      config);

Nota

Instead of generating the trajectory on the roboRIO as outlined above, one can also import a PathWeaver JSON.

Creando el RamseteCommand

Primero reajustaremos la posición de nuestro robot a la posición inicial de la trayectoria. Esto asegura que la posición del robot en el sistema de coordenadas y la posición inicial de la trayectoria sean las mismas.

129    // Reset odometry to the initial pose of the trajectory, run path following
130    // command, then stop at the end.
131    return Commands.runOnce(() -> m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose()))

84  // Reset odometry to the initial pose of the trajectory, run path following
85  // command, then stop at the end.
86  return frc2::cmd::RunOnce(

It is very important that the initial robot pose match the first pose in the trajectory. For the purposes of our example, the robot will be reliably starting at a position of (0,0) with a heading of 0. In actual use, however, it is probably not desirable to base your coordinate system on the robot position, and so the starting position for both the robot and the trajectory should be set to some other value. If you wish to use a trajectory that has been defined in robot-centric coordinates in such a situation, you can transform it to be relative to the robot’s current pose using the transformBy method (Java, C++). For more information about transforming trajectories, see Transformando trayectorias.

Now that we have a trajectory, we can create a command that, when executed, will follow that trajectory. To do this, we use the RamseteCommand class (Java, C++)

112    RamseteCommand ramseteCommand =
113        new RamseteCommand(
114            exampleTrajectory,
115            m_robotDrive::getPose,
116            new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
117            new SimpleMotorFeedforward(
118                DriveConstants.ksVolts,
119                DriveConstants.kvVoltSecondsPerMeter,
120                DriveConstants.kaVoltSecondsSquaredPerMeter),
121            DriveConstants.kDriveKinematics,
122            m_robotDrive::getWheelSpeeds,
123            new PIDController(DriveConstants.kPDriveVel, 0, 0),
124            new PIDController(DriveConstants.kPDriveVel, 0, 0),
125            // RamseteCommand passes volts to the callback
126            m_robotDrive::tankDriveVolts,
127            m_robotDrive);

71  frc2::CommandPtr ramseteCommand{frc2::RamseteCommand(
72      exampleTrajectory, [this] { return m_drive.GetPose(); },
73      frc::RamseteController{AutoConstants::kRamseteB,
74                             AutoConstants::kRamseteZeta},
75      frc::SimpleMotorFeedforward<units::meters>{
76          DriveConstants::ks, DriveConstants::kv, DriveConstants::ka},
77      DriveConstants::kDriveKinematics,
78      [this] { return m_drive.GetWheelSpeeds(); },
79      frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
80      frc::PIDController{DriveConstants::kPDriveVel, 0, 0},
81      [this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
82      {&m_drive})};

Esta declaración es bastante sustancial, así que la revisaremos argumento por argumento:

  1. La trayectoria: Esta es la trayectoria a seguir; en consecuencia, pasamos el comando de la trayectoria que acabamos de construir en nuestros pasos anteriores.

  2. The pose supplier: This is a method reference (or lambda) to the drive subsystem method that returns the pose. The RAMSETE controller needs the current pose measurement to determine the required wheel outputs.

  3. The RAMSETE controller: This is the RamseteController object (Java, C++) that will perform the path-following computation that translates the current measured pose and trajectory state into a chassis speed setpoint.

  4. The drive feedforward: This is a SimpleMotorFeedforward object (Java, C++) that will automatically perform the correct feedforward calculation with the feedforward gains (kS, kV, and kA) that we obtained from the drive identification tool.

  5. The drive kinematics: This is the DifferentialDriveKinematics object (Java, C++) that we constructed earlier in our constants file, and will be used to convert chassis speeds to wheel speeds.

  6. The wheel speed supplier: This is a method reference (or lambda) to the drive subsystem method that returns the wheel speeds

  7. The left-side PIDController: This is the PIDController object (Java, C++) that will track the left-side wheel speed setpoint, using the P gain that we obtained from the drive identification tool.

  8. The right-side PIDController: This is the PIDController object (Java, C++) that will track the right-side wheel speed setpoint, using the P gain that we obtained from the drive identification tool.

  9. The output consumer: This is a method reference (or lambda) to the drive subsystem method that passes the voltage outputs to the drive motors.

  10. El impulso del robot: Este es el subsistema de accionamiento en sí, incluido para asegurar que el comando no funcione en el accionamiento al mismo tiempo que cualquier otro comando que utilice el accionamiento.

Finalmente, note que añadimos un comando final de «stop» en secuencia después del comando de seguimiento de la trayectoria, para asegurar que el robot deje de moverse al final de la trayectoria.

Video

Si todo ha ido bien, la rutina autónoma de tu robot debería ser algo así: