انتقل إلى المحتوى الرئيسي

روبوت Deep Space 2019

كانت لعبة FRC لعام 2019 Deep Space تحتوي على أهداف رؤية فوق العديد من الأهداف. يمكنك العثور أدناه على برامج مثال كاملة بلغتي Java و Labview تنفذ طريقة بسيطة للقيادة التلقائية نحو هدف في Deep Space.

هذه برامج بسيطة جدًا ومخصصة فقط لإظهار مفهوم استخدام بيانات تتبع limelight للتحكم في روبوتك. في كل برنامج، يمكنك قيادة روبوتك باستخدام لوحة الألعاب. إذا ضغطت باستمرار على الزر 'A'، ورأى limelight هدفًا صالحًا (اعتمادًا على الإعدادات في خط الأنابيب الخاص بك) فسيقود الروبوت تلقائيًا نحو الهدف. احرص على ضبط الثوابت المختلفة في الكود لروبوتك الخاص. بعض الروبوتات تدور أو تقود بسهولة أكبر من غيرها، لذا يجب إجراء ضبط ثوابت التحكم التناسبي على أساس كل حالة على حدة. تأكد من أن الروبوت يقود بشكل صحيح باستخدام وحدة التحكم في لوحة الألعاب قبل تمكين سلوك البحث عن limelight.

package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.VictorSP;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.networktables.*;


public class Robot extends TimedRobot {

  private static final String kDefaultAuto = "Default";
  private static final String kCustomAuto = "My Auto";
  private String m_autoSelected;
  private final SendableChooser<String> m_chooser = new SendableChooser<>();

  private VictorSP m_Left0 = new VictorSP(0);
  private VictorSP m_Left1 = new VictorSP(1);
  private VictorSP m_Right0 = new VictorSP(2);
  private VictorSP m_Right1 = new VictorSP(3);
  private SpeedControllerGroup m_LeftMotors = new SpeedControllerGroup(m_Left0,m_Left1);
  private SpeedControllerGroup m_RightMotors = new SpeedControllerGroup(m_Right0,m_Right1);
  private DifferentialDrive m_Drive = new DifferentialDrive(m_LeftMotors,m_RightMotors);

  private XboxController m_Controller = new XboxController(0);

  private boolean m_LimelightHasValidTarget = false;
  private double m_LimelightDriveCommand = 0.0;
  private double m_LimelightSteerCommand = 0.0;

  @Override
  public void robotInit() {
        m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
        m_chooser.addOption("My Auto", kCustomAuto);
        SmartDashboard.putData("Auto choices", m_chooser);
  }

  @Override
  public void robotPeriodic() {
  }

  @Override
  public void autonomousInit() {
        m_autoSelected = m_chooser.getSelected();
  }

  @Override
  public void autonomousPeriodic() {
  }

  @Override
  public void teleopPeriodic() {

        Update_Limelight_Tracking();

        double steer = m_Controller.getX(Hand.kRight);
        double drive = -m_Controller.getY(Hand.kLeft);
        boolean auto = m_Controller.getAButton();

        steer *= 0.70;
        drive *= 0.70;

        if (auto)
        {
          if (m_LimelightHasValidTarget)
          {
                m_Drive.arcadeDrive(m_LimelightDriveCommand,m_LimelightSteerCommand);
          }
          else
          {
                m_Drive.arcadeDrive(0.0,0.0);
          }
        }
        else
        {
          m_Drive.arcadeDrive(drive,steer);
        }
  }

  @Override
  public void testPeriodic() {
  }

  /**
   * تنفذ هذه الوظيفة طريقة بسيطة لتوليد أوامر القيادة والتوجيه
   * بناءً على بيانات التتبع من كاميرا limelight.
   */
  public void Update_Limelight_Tracking()
  {
        // يجب ضبط هذه الأرقام لروبوتك! كن حذرًا!
        final double STEER_K = 0.03;                    // مدى قوة الدوران نحو الهدف
        final double DRIVE_K = 0.26;                    // مدى قوة القيادة للأمام نحو الهدف
        final double DESIRED_TARGET_AREA = 13.0;        // مساحة الهدف عندما يصل الروبوت إلى الجدار
        final double MAX_DRIVE = 0.7;                   // حد سرعة بسيط حتى لا نقود بسرعة كبيرة جدًا

        double tv = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0);
        double tx = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0);
        double ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0);
        double ta = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0);

        if (tv < 1.0)
        {
          m_LimelightHasValidTarget = false;
          m_LimelightDriveCommand = 0.0;
          m_LimelightSteerCommand = 0.0;
          return;
        }

        m_LimelightHasValidTarget = true;

        // ابدأ بالتوجيه التناسبي
        double steer_cmd = tx * STEER_K;
        m_LimelightSteerCommand = steer_cmd;

        // حاول القيادة للأمام حتى تصل مساحة الهدف إلى المساحة المطلوبة
        double drive_cmd = (DESIRED_TARGET_AREA - ta) * DRIVE_K;

        // لا تدع الروبوت يقود بسرعة كبيرة جدًا نحو الهدف
        if (drive_cmd > MAX_DRIVE)
        {
          drive_cmd = MAX_DRIVE;
        }
        m_LimelightDriveCommand = drive_cmd;
  }
}