使用 Swerve 进行瞄准和测距

查看完整示例项目 点击这里

 // 使用 Limelight 的简单比例转向控制。
  // "比例控制"是一种控制算法，其中输出与误差成正比。
  // 在这种情况下，我们将返回一个与 Limelight 的 "tx" 值成正比的角速度。
  double limelight_aim_proportional()
  {    
    // kP（比例常数）
    // 这是一个手动调整的数值，用于确定比例控制回路的响应强度
    // 如果太高，机器人会来回振荡。
    // 如果太低，机器人将永远无法到达目标
    // 如果机器人从不朝正确方向转动，应该反转 kP。
    double kP = .035;

    // tx 的范围是从 (-hfov/2) 到 (hfov/2) 度。如果你的目标在
    // limelight 3 画面的最右边缘，tx 应该返回大约 31 度。
    double targetingAngularVelocity = LimelightHelpers.getTX("limelight") * kP;

    // 转换为弧度每秒以供我们的驱动方法使用
    targetingAngularVelocity *= Drivetrain.kMaxAngularSpeed;

    // 反转，因为当目标在准星右侧时 tx 为正值
    targetingAngularVelocity *= -1.0;

    return targetingAngularVelocity;
  }

  // 使用 Limelight 的 "ty" 值进行简单的比例测距控制
  // 当你的 Limelight 安装高度和目标安装高度不同时，这种方法效果最好。
  // 如果你的 limelight 和目标安装在相同或相似的高度，请使用 "ta"（面积）而不是 "ty" 进行目标测距
  double limelight_range_proportional()
  {    
    double kP = .1;
    double targetingForwardSpeed = LimelightHelpers.getTY("limelight") * kP;
    targetingForwardSpeed *= Drivetrain.kMaxSpeed;
    targetingForwardSpeed *= -1.0;
    return targetingForwardSpeed;
  }

  private void drive(boolean fieldRelative) {
    // 获取 x 速度。我们反转这个值是因为 Xbox 控制器在我们向前推时
    // 返回负值。
    var xSpeed =
        -m_xspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftY(), 0.02))
            * Drivetrain.kMaxSpeed;

    // 获取 y 速度或横向/平移速度。我们反转这个值是因为
    // 我们希望向左拉时得到正值。Xbox 控制器默认在你向右拉时
    // 返回正值。
    var ySpeed =
        -m_yspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftX(), 0.02))
            * Drivetrain.kMaxSpeed;

    // 获取角旋转速率。我们反转这个值是因为我们希望向左拉时
    // 得到正值（记住，在数学中逆时针为正）。
    // Xbox 控制器默认在你向右拉时返回正值。
    var rot =
        -m_rotLimiter.calculate(MathUtil.applyDeadband(m_controller.getRightX(), 0.02))
            * Drivetrain.kMaxAngularSpeed;

    // 当按住 A 键时，用我们的 limelight 方法的输出覆盖一些驾驶值
    if(m_controller.getAButton())
    {
        final var rot_limelight = limelight_aim_proportional();
        rot = rot_limelight;

        final var forward_limelight = limelight_range_proportional();
        xSpeed = forward_limelight;

        // 使用 Limelight 时，关闭场地相对驾驶。
        fieldRelative = false;
    }

    m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod());
  }