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