التصويب والتحديد المدى مع 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());
  }