スウェルブでの照準と距離測定

完全なサンプルプロジェクトはこちらでご覧いただけます。

 // 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とターゲットが同じまたは類似の高さに取り付けられている場合、距離測定には "ty" の代わりに "ta"（面積）を使用してください。
  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());
  }