Ana içeriğe geç

2019 Deep Space Robotu

2019 FRC oyunu Deep Space, birçok hedefin üzerinde görüş hedefleri bulunduruyordu. Aşağıda, Deep Space'te bir hedefe otomatik olarak sürüş yapmak için basit bir yöntem uygulayan Java ve Labview'de eksiksiz örnek programlar bulabilirsiniz.

Bunlar çok basit programlardır ve yalnızca robotunuzu kontrol etmek için limelight takip verilerini kullanma konseptini göstermek amacıyla hazırlanmıştır. Her programda robotunuzu bir gamepad ile sürebilirsiniz. 'A' düğmesini basılı tutarsanız ve limelight geçerli bir hedef görürse (pipeline'ınızdaki ayarlara bağlı olarak) robot otomatik olarak hedefe doğru sürecektir. Koddaki çeşitli sabitleri kendi robotunuz için ayarlamaya dikkat edin. Bazı robotlar diğerlerinden daha kolay döner veya hareket eder, bu nedenle oransal kontrol sabitlerinin ayarlanması duruma göre yapılmalıdır. Limelight arama davranışını etkinleştirmeden önce robotun gamepad kontrolcüsü ile doğru şekilde hareket ettiğinden emin olun.

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() {
  }

  /**
   * Bu fonksiyon, bir limelight kamerasından gelen takip verilerine dayalı olarak
   * sürüş ve direksiyon komutları oluşturmak için basit bir yöntem uygular.
   */
  public void Update_Limelight_Tracking()
  {
        // Bu sayılar Robotunuz için ayarlanmalıdır! Dikkatli olun!
        final double STEER_K = 0.03;                    // hedefe doğru ne kadar sert dönülecek
        final double DRIVE_K = 0.26;                    // hedefe doğru ne kadar sert ileri sürülecek
        final double DESIRED_TARGET_AREA = 13.0;        // Robot duvara ulaştığında hedefin alanı
        final double MAX_DRIVE = 0.7;                   // Çok hızlı sürmemek için basit hız limiti

        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;

        // Oransal direksiyon ile başla
        double steer_cmd = tx * STEER_K;
        m_LimelightSteerCommand = steer_cmd;

        // hedef alanı istenen alana ulaşana kadar ileri sürmeye çalış
        double drive_cmd = (DESIRED_TARGET_AREA - ta) * DRIVE_K;

        // robotun hedefe çok hızlı sürmesine izin verme
        if (drive_cmd > MAX_DRIVE)
        {
          drive_cmd = MAX_DRIVE;
        }
        m_LimelightDriveCommand = drive_cmd;
  }
}