כיוון וטווח עם Swerve

ראה את הפרויקט המלא לדוגמה כאן

 // בקרת סיבוב פרופורציונלית פשוטה עם Limelight.
  // "בקרה פרופורציונלית" היא אלגוריתם בקרה שבו הפלט פרופורציונלי לשגיאה.
  // במקרה זה, אנחנו הולכים להחזיר מהירות זוויתית שהיא פרופורציונלית לערך
  // "tx" מה-Limelight.
  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;
  }

  // בקרת טווח פרופורציונלית פשוטה עם ערך "ty" של Limelight
  // זה עובד הכי טוב אם גובה ההרכבה של ה-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;

    // קבל את קצב הסיבוב הזוויתי. אנחנו הופכים את זה כי אנחנו רוצים
    // ערך חיובי כשאנחנו מושכים שמאלה (זכור, CCW הוא חיובי
    // במתמטיקה). בקרי 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());
  }