步骤4:创建和遵循轨迹

编写了驱动子系统后,现在是时候生成一个轨迹并编写一个自动阶段的指令来遵循它。

根据基于指令的标准项目结构<docs/software/commandbased/structuring-command-based-project:Structuring a Command-Based Robot Project>,我们将在“ RobotContainer”类的“ getAutonomousCommand”方法中执行此操作。可以在下面看到RamseteCommand示例项目的完整方法(Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand> __,C ++ <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibcExamples/src/main/cpp/examples/RamseteCommand> __)。本文的其余部分将更详细地介绍该方法的不同部分。

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

配置轨迹约束

首先,我们必须为轨迹设置一些配置参数,以确保生成的轨迹是可追踪的。

创建电压约束

我们需要的第一个配置是电压约束。这将确保生成的轨迹不会命令机器人以比给定电压供电下所能达到的速度更快的速度前进:

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

注意,我们将最大电压设置为10V,而不是标称电池电压12V。这给了我们一些“净空”来处理运行过程中的“电压跌落”。

创建配置

现在我们有了电压约束,我们可以创建“ TrajectoryConfig”实例,该实例将所有路径约束包装在一起:

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

产生轨迹

有了轨迹配置之后,我们现在就可以生成轨迹了。对于这个例子,我们将生成一个“三次样条”轨迹-这意味着我们将指定机器人在端点的完整姿态,并且只指定内部路径点的位置(也称为“结点”)。和其他地方一样,所有的距离都以米为单位。

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

注解

除了上面概述的在roboRIO上生成轨迹外,还可以用:ref:import a PathWeaver JSON <docs/software/wpilib-tools/pathweaver/integrating-robot-program:Importing a PathWeaver JSON>

创建Ramsete Command

首先,我们将机器人的姿态重置为轨迹的初始位姿。这保证了机器人在坐标系中的位置和轨迹的起始位置是相同的。

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

初始机器人姿态必须与轨迹中的第一个姿态相匹配,这一点非常重要。就我们的示例而言,机器人将可靠地从``(0,0)``的位置开始,并以``0’’的方向启动。但是,在实际使用中,可能不希望将坐标系建立在机器人的位置上,因此,机器人和轨迹的起始位置都应设置为其他值。如果您希望在这种情况下使用以机器人为中心的坐标定义的轨迹,可以使用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>`_)。有关变换轨迹的更多信息,请参阅:docs / software / advanced-controls / trajectories / transforming-trajectories:Transforming Trajectories。

现在我们有了一条轨迹,我们可以创建一个指令,该指令在执行时将遵循该轨迹。为此,我们使用``RamseteCommand``类(Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/RamseteCommand.html>`__,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
    );

这个声明是相当重要的,所以我们将逐个逐个地讨论它:

  1. 轨迹:这是要遵循的轨迹;相应地,我们将在前面的步骤中构造的轨迹传递给指令。

  2. “pose供应商”:这是一个 :ref:`drive subsystem method that returns the pose <docs/software/examples-tutorials/trajectory-tutorial/creating-drive-subsystem:Odometry Accessor Method>`的方法引用(或lambda)。横夯控制器需要当前位姿测量来确定所需的车轮输出。

  3. RAMSETE控制器:这是“ RamseteController”对象(Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/controller/RamseteController.html>`__,C ++),它将执行路径跟踪计算,该计算将当前测得的姿态和轨迹状态转换为底盘速度设定值。

  4. 驱动器前馈:这是一个``SimpleMotorFeedforward’’对象(Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.html>`__,C ++),将自动使用前馈增益(``kS’’,``kV’’执行正确的前馈计算,以及我们从驱动器表征工具获得的``kA’’)。

  5. 驱动器运动学:这是我们在常量文件中先前构建的“ DifferentialDriveKinematics”对象(JavaC ++),将用于将底盘速度转换为车轮速度。

  6. 车轮速度提供者:这是 :ref:`drive subsystem method that returns the wheel speeds <docs/software/examples-tutorials/trajectory-tutorial/creating-drive-subsystem:Encoder Accessor Method>`一个方法引用(或lambda)。

  7. 左侧PIDController:这是“ PIDController”对象(JavaC ++),它将使用从驱动器表征获得的P增益跟踪左侧车轮速度设定值。工具。

  8. 右侧PIDController:这是“ PIDController”对象(JavaC ++),这将跟踪右侧车轮速度设定值,使用我们从驱动特性工具获得的P增益。

  9. 输出消费者:这是 :ref:`drive subsystem method that passes the voltage outputs to the drive motors <docs/software/examples-tutorials/trajectory-tutorial/creating-drive-subsystem:Voltage-Based Drive Method>`的一个方法引用(或lambda)。

  10. 机器人驱动器:这是驱动器子系统本身,包含它是为了确保指令不会与使用驱动器的任何其他指令同时在驱动器上操作。

最后需要注意的是,我们在路径跟随指令之后依次添加最后一个“stop”指令,以确保机器人在轨迹结束时停止移动

视频

如果一切顺利,你的机器人的自主程序应该看起来像这样: