スワーブドライブでの照準と距離調整

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

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