דלג לתוכן הראשי

רובוט 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 = "ברירת מחדל";
  private static final String kCustomAuto = "האוטונומי שלי";
  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("אוטונומי ברירת מחדל", kDefaultAuto);
        m_chooser.addOption("האוטונומי שלי", kCustomAuto);
        SmartDashboard.putData("בחירות אוטונומי", 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;
  }
}