スウェーブドライブを使用した照準と距離調整

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

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