É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.

 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
  /**
   * Use this to pass the autonomous command to the main {@link Robot} class.
   *
   * @return the command to run in autonomous
   */
  public Command getAutonomousCommand() {

    // Create a voltage constraint to ensure we don't accelerate too fast
    var autoVoltageConstraint =
        new DifferentialDriveVoltageConstraint(
            new SimpleMotorFeedforward(DriveConstants.ksVolts,
                                       DriveConstants.kvVoltSecondsPerMeter,
                                       DriveConstants.kaVoltSecondsSquaredPerMeter),
            DriveConstants.kDriveKinematics,
            10);

    // Create config for trajectory
    TrajectoryConfig config =
        new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
                             AutoConstants.kMaxAccelerationMetersPerSecondSquared)
            // Add kinematics to ensure max speed is actually obeyed
            .setKinematics(DriveConstants.kDriveKinematics)
            // Apply the voltage constraint
            .addConstraint(autoVoltageConstraint);

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

    RamseteCommand ramseteCommand = new RamseteCommand(
        exampleTrajectory,
        m_robotDrive::getPose,
        new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
        new SimpleMotorFeedforward(DriveConstants.ksVolts,
                                   DriveConstants.kvVoltSecondsPerMeter,
                                   DriveConstants.kaVoltSecondsSquaredPerMeter),
        DriveConstants.kDriveKinematics,
        m_robotDrive::getWheelSpeeds,
        new PIDController(DriveConstants.kPDriveVel, 0, 0),
        new PIDController(DriveConstants.kPDriveVel, 0, 0),
        // RamseteCommand passes volts to the callback
        m_robotDrive::tankDriveVolts,
        m_robotDrive
    );

    // Reset odometry to the starting pose of the trajectory.
    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());

    // Run path following command, then stop at the end.
    return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
  }

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:

89
90
91
92
93
94
95
96
    // Create a voltage constraint to ensure we don't accelerate too fast
    var autoVoltageConstraint =
        new DifferentialDriveVoltageConstraint(
            new SimpleMotorFeedforward(DriveConstants.ksVolts,
                                       DriveConstants.kvVoltSecondsPerMeter,
                                       DriveConstants.kaVoltSecondsSquaredPerMeter),
            DriveConstants.kDriveKinematics,
            10);

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 :

 98
 99
100
101
102
103
104
105
    // Create config for trajectory
    TrajectoryConfig config =
        new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
                             AutoConstants.kMaxAccelerationMetersPerSecondSquared)
            // Add kinematics to ensure max speed is actually obeyed
            .setKinematics(DriveConstants.kDriveKinematics)
            // Apply the voltage constraint
            .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.

108
109
110
111
112
113
114
115
116
117
118
119
120
    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
        // Start at the origin facing the +X direction
        new Pose2d(0, 0, new Rotation2d(0)),
        // Pass through these two interior waypoints, making an 's' curve path
        List.of(
            new Translation2d(1, 1),
            new Translation2d(2, -1)
        ),
        // End 3 meters straight ahead of where we started, facing forward
        new Pose2d(3, 0, new Rotation2d(0)),
        // Pass config
        config
    );

Note

Au lieu de générer la trajectoire sur le roboRIO comme indiqué ci-dessus, on peut également importe un fichier 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.

137
138
    // Reset odometry to the starting pose of the trajectory.
    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());

Il est très important que la pose initiale du robot corresponde à la première pose de la trajectoire. Pour les besoins de notre exemple, le robot aura un départ fiable en partant de la position (0,0) avec une orientation de 0. Dans un fonctionnement réel, cependant, il n’est probablement pas souhaitable de baser votre système de coordonnées sur la position du robot, et donc la position de départ du le robot et celle de la trajectoire doit être définie à une autre valeur. Si vous souhaitez utiliser une trajectoire qui a été définie dans les coordonnées relatives au robot dans une telle situation, vous pouvez la transformer de manière à être relative à la pose actuelle du robot en utilisant la méthode transformBy (Java, C++). Pour plus d’informations sur la transformation des trajectoires, voir La transformation des trajectoires.

Maintenant que nous avons une trajectoire, nous pouvons créer une commande qui, une fois exécutée, suivra cette trajectoire. Pour ce faire, nous nous servons de la classe RamseteCommand (Java, C++)

122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
    RamseteCommand ramseteCommand = new RamseteCommand(
        exampleTrajectory,
        m_robotDrive::getPose,
        new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
        new SimpleMotorFeedforward(DriveConstants.ksVolts,
                                   DriveConstants.kvVoltSecondsPerMeter,
                                   DriveConstants.kaVoltSecondsSquaredPerMeter),
        DriveConstants.kDriveKinematics,
        m_robotDrive::getWheelSpeeds,
        new PIDController(DriveConstants.kPDriveVel, 0, 0),
        new PIDController(DriveConstants.kPDriveVel, 0, 0),
        // RamseteCommand passes volts to the callback
        m_robotDrive::tankDriveVolts,
        m_robotDrive
    );

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 pose supplier: Il s’agit d’une référence de méthode (ou lambda) à la méthode du sous-système de déplacement qui retourne la pose. Le contrôleur RAMSETE a besoin de la valeur de la pose actuelle pour déterminer les sorties requises des roues .

  3. Le contrôleur RAMSETE : Il s’agit de l’objet RamseteController (Java, C++) qui effectuera les calculs relatifs au suivi de la trajectoire qui traduisent la pose actuelle mesurée et l’état de la trajectoire en un point de consigne de vitesse du châssis.

  4. L’anticipation de l’entraînement : Il s’agit d’un objet SimpleMotorFeedforward (Java, C++) qui effectuera automatiquement le bon calcul d’anticipation avec les gains feedforward (kS, kV, and kA) du système d’entraînement que nous avons obtenus à l’aide de l’outil caractérisation.

  5. La cinématique du système d’entraînement: Il s’agit de l’objet DifferentialDriveKinematics (Java, C++) que nous avons construit plus tôt dans notre fichier réservé aux constantes, et sera utilisé pour convertir les vitesses du châssis en vitesses de roue.

  6. Le supplier de vitesse de la roue: Il s’agit d’une méthode de référence (ou lambda) à la méthode du sous-système de déplacement qui retourne les vitesses des roues

  7. Le PIDController du côté gauche : Il s’agit de l’objet PIDController (Java, C++) qui permettra de suivre le point de consigne de vitesse de la roue gauche, en utilisant le gain P que nous avons obtenu de l’outil de caractérisation pour caractériser le système d’entraînement.

  8. Le PIDController du côté droit : Il s’agit de l’objet PIDController (Java, C++) qui permettra de suivre le point de consigne de vitesse de la roue gauche, en utilisant le gain P que nous avons obtenu de l’outil de caractérisation pour caractériser le système d’entraînement.

  9. Le consumer de la sortie: Il s’agit d’une méthode de référence (ou lambda) à la la méthode du sous-système base pilotable qui passe les tensions de sortie aux moteurs de déplacement.

  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 :