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

רובוט 2019 Deep Space

משחק ה-FRC לשנת 2019 Deep Space כלל מטרות ראייה מעל רבים מהיעדים. להלן תוכלו למצוא תוכניות דוגמה מלאות בג'אווה ו-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;
  }
}