Ana içeriğe geç

2019 Deep Space Robotu

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

Bunlar çok basit programlardır ve sadece robotunuzu kontrol etmek için limelight izleme verilerini kullanma konseptini göstermek amacıyla tasarlanmıştır. Her programda, robotunuzu bir oyun kumandası 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ürüş yapacaktır. Kodda bulunan çeşitli sabitleri kendi robotunuza göre ayarlamaya dikkat edin. Bazı robotlar diğerlerinden daha kolay dönüş yapar veya sürülür, bu nedenle orantılı kontrol sabitleri her duruma göre ayrı ayrı ayarlanmalıdır. Limelight hedef arama davranışını etkinleştirmeden önce robotun oyun kumandası ile doğru şekilde sürüldüğünden 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, limelight kamerasından gelen izleme verilerine dayalı olarak
   * sürüş ve yönlendirme 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üleceği
        final double DRIVE_K = 0.26;                    // hedefe doğru ne kadar sert sürüleceği
        final double DESIRED_TARGET_AREA = 13.0;        // Robot duvara ulaştığında hedefin alanı
        final double MAX_DRIVE = 0.7;                   // Çok hızlı sürmememiz için basit bir hız sınırı

        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;

        // Orantılı yönlendirme ile başlayın
        double steer_cmd = tx * STEER_K;
        m_LimelightSteerCommand = steer_cmd;

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

        // robotun hedefe çok hızlı gitmesine izin vermeyin
        if (drive_cmd > MAX_DRIVE)
        {
          drive_cmd = MAX_DRIVE;
        }
        m_LimelightDriveCommand = drive_cmd;
  }
}