Étape 4 : Création et suivi d’une trajectoire

Avec notre sous-système de déplacement écrit, il est maintenant temps de générer une trajectoire et d’écrire une commande autonome pour la suivre.

Selon le projet structure standard d’un projet orienté commande, nous le ferons dans la méthode getAutonomousCommand de la classe RobotContainer. La méthode complète du projet d’exemple RamseteCommand (Java, C++) peut être vue ci-dessous. Le reste de l’article décomposera plus en détail les différentes parties de la méthode.

 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}

Configuration des contraintes de la trajectoire

Tout d’abord, nous devons définir certains paramètres de configuration pour la trajectoire qui garantiront que la trajectoire ainsi générée peut effectivement être suivie.

Création d’une contrainte de tension

La première pièce de configuration dont nous aurons besoin est une contrainte de tension. Cela permettra de s’assurer que la trajectoire générée ne contraigne jamais le robot d’aller plus vite qu’il n’est capable d’atteindre avec l’alimentation de tension disponible:

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

Notez que nous avons fixé la tension maximale à 10V, plutôt qu’à la tension nominale de la batterie de 12V. Cela nous laisse une certaine « marge » pour faire face à un éventuel « creux de tension » pendant le suivi de la trajectoire.

Création de la configuration

Maintenant que nous avons notre contrainte de tension, nous pouvons créer notre instance TrajectoryConfig, qui englobe toutes nos contraintes de trajectoire :

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

Génération de la trajectoire

Avec notre configuration de trajectoire en main, nous sommes maintenant prêts à générer notre trajectoire. Pour cet exemple, nous allons générer une trajectoire « clamped cubic » - cela signifie que nous allons spécifier au complet les poses de robots aux extrémités, et spécifier des positions uniquement pour les points de passage intérieurs (également connu sous le nom « points noeuds »). Comme ailleurs, toutes les distances sont en mètres.

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

Note

Au lieu de générer la trajectoire sur le roboRIO comme indiqué ci-dessus, on peut également importer un PathWeaver JSON.

Création de la commande Ramsete ou RamseteCommand

Nous allons d’abord réinitialiser la pose de notre robot à la pose de départ de la trajectoire. Cela garantit que l’emplacement du robot sur le système de coordonnées et la position de départ de la trajectoire sont identiques.

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 La transformation des trajectoires.

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

Cette déclaration est assez substantielle, nous allons donc la scruter paramètre par paramètre:

  1. La trajectoire : C’est la trajectoire à suivre; en conséquence, nous passons à la commande la trajectoire que nous venons de construire dans nos étapes antérieures.

  2. Le fournisseur de pose : il s’agit d’une référence de méthode (ou lambda) à la méthode du sous-système drive qui renvoie la pose. Le contrôleur RAMSETE a besoin de la mesure de pose actuelle pour déterminer les sorties de roue requises.

  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. Le fournisseur de vitesse de roue : il s’agit d’une référence de méthode (ou lambda) à la méthode du sous-système drive qui renvoie les vitesses de roue

  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. Le consommateur de sortie : il s’agit d’une référence de méthode (ou lambda) à la méthode du sous-système drive qui transmet les sorties de tension aux moteurs d’entraînement.

  10. Le sous-système d’entraînement du robot : Il s’agit du sous-système de déplacement lui-même, inclus ici pour s’assurer que la commande ne fonctionne pas sur la base pilotable en même temps que toute autre commande qui utilise cette même base pilotable.

Enfin, notez que nous ajoutons une commande finale « stop » dans l’ordre après la commande de suivi de la trajectoire, pour s’assurer que le robot s’immobilise à la fin de la trajectoire.

Vidéo

Si tout s’est bien passé, la routine autonome de votre robot devrait ressembler à ceci :