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

רובוט 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;
  }
}