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

רובוט Deep Space 2019

משחק FRC 2019 Deep Space כלל מטרות ראייה מעל רבים מהשערים. למטה תוכלו למצוא תוכניות דוגמה מלאות ב-Java וב-Labview שמיישמות שיטה פשוטה לנהיגה אוטומטית לכיוון שער ב-Deep Space.

אלו תוכניות פשוטות מאוד ונועדו רק להראות את הרעיון של שימוש בנתוני מעקב של limelight לשליטה ברובוט שלכם. בכל תוכנית, תוכלו לנהוג ברובוט שלכם עם בקר משחק. אם תחזיקו את כפתור 'A' לחוץ, וה-limelight רואה מטרה תקפה (בהתאם להגדרות ב-pipeline שלכם) אז הרובוט ינהג אוטומטית לכיוון המטרה. היזהרו לכוונן את הקבועים השונים בקוד לרובוט הספציפי שלכם. חלק מהרובוטים מסתובבים או נוסעים בקלות רבה יותר מאחרים, כך שכיוונון קבועי הבקרה הפרופורציונלית חייב להיעשות על בסיס כל מקרה לגופו. ודאו שהרובוט נוסע נכון באמצעות בקר המשחק לפני הפעלת התנהגות החיפוש של 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;
  }
}