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.

Según la :ref:` estructura estandar de un proyecto a base de comandos <docs/software/commandbased/structuring-command-based-project:Structuring a Command-Based Robot Project>`, haremos esto en el método de la clase RobotContainer. El método completo del proyecto RamseteCommand Example (Java, C++) puede verse debajo. El resto del articulo se separará en diferentes partes del método en más detalle.

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

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:

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

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:

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

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.

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

Nota

En lugar de generar la trayectoria en el roboRIO como se indica arriba, también se puede importar un 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.

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

Es muy importante que la pose inicial del robot coincida con la primera pose en la trayectoria. Para los propósitos de nuestro ejemplo, el robot comenzará de manera confiable en una posición de (0,0) con un encabezado de 0. Sin embargo, en el uso real, probablemente no sea deseable basar su sistema de coordenadas en la posición del robot, por lo que la posición inicial tanto para el robot como para la trayectoria debe establecerse en algún otro valor. Si desea utilizar una trayectoria que ha sido definida en coordenadas centradas en el robot en tal situación, puede transformarla para que sea relativa a la pose actual del robot utilizando el método transformBy (Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/trajectory/Trajectory.html#transformBy(edu.wpi.first.wpilibj.geometry.Transform2d)> _, C ++ <https://first.wpi.edu/wpilib/allwpilib/docs/release/cpp/classfrc_1_1Trajectory.html#a8edfbd82347bbf32ddfb092679336cd8> `_). Para obtener más información sobre la transformación de trayectorias, consulte :ref:`docs/software/advanced-controls/trajectories/transforming-trajectories:Transforming Trajectories.

Ahora que tenemos una trayectoria, podemos crear un comando que, cuando se ejecute, seguirá esa trayectoria. Para hacer esto, usamos la clase 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
    );

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. El proveedor de poses: Este es un método de referencia (o lambda) al método de subsistema de la unidad que regresa la pose. El controlador RAMSETE necesita la medición de la pose actual para determinar las salidas de rueda requeridas.

  3. El controlador RAMSETE: Este es el objeto RamseteController (Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/controller/RamseteController.html> __, C ++ <https://first.wpi.edu/wpilib/allwpilib/docs/release/cpp/classfrc_1_1RamseteController.html> __) que realizará el cálculo de seguimiento de ruta que traduce la pose actual medida y el estado de trayectoria en un punto de ajuste de velocidad del chasis.

  4. El feedforward del subsistema drive: Este es un objeto SimpleMotorFeedforward (Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.html> __, C ++ <https://first.wpi.edu/wpilib/allwpilib/docs/release/cpp/classfrc_1_1SimpleMotorFeedforward.html> __) que realizará automáticamente el cálculo de feedforward correcto con las ganancias de feedforward (kS, kV, and kA) que obtuvimos de la herramienta de caracterización de variadores.

  5. La cinemática de la unidad: Este es el objeto DifferentialDriveKinematics (Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.html> __, C ++ <https://first.wpi.edu/wpilib/allwpilib/docs/release/cpp/classfrc_1_1DifferentialDriveKinematics.html> __) que construimos anteriormente en nuestro archivo de constantes, y se utilizará para convertir las velocidades del chasis a velocidades de rueda.

  6. El proveedor de la velocidad de la rueda: Este es un método de referencia (o lambda) al método del subsistema de conducción que regresa las velocidades de la llanta

  7. El PIDController del lado izquierdo: Este es el objeto PIDController (Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/controller/PIDController.html> __, C ++ <https://first.wpi.edu/wpilib/allwpilib/docs/release/cpp/classfrc2_1_1PIDController.html> __) que rastreará el punto de ajuste de velocidad de la rueda del lado izquierdo, usando la ganancia P que obtuvimos de la caracterización del variador herramienta.

  8. El PIDController del lado derecho: este es el objeto PIDController (Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/controller/PIDController.html> __, C ++ <https://first.wpi.edu/wpilib/allwpilib/docs/release/cpp/classfrc2_1_1PIDController.html> __) que rastreará el punto de ajuste de velocidad de la rueda del lado derecho, utilizando la ganancia P que obtuvimos de la caracterización del variador herramienta.

  9. El consumidor de la producción: Este es un método de referencia (o lambda) al método del subsistema de accionamiento que pasa las salidas de tensión a los motores de accionamiento.

  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í: