スワーブドライブでのエイミングと距離調整

完全なサンプルプロジェクトはこちらをご覧ください。

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