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üş için basit bir yöntemi uygulayan Java ve Labview'da tam örnek programlar bulabilirsiniz.

Bunlar çok basit programlardır ve sadece robotunuzu kontrol etmek için limelight izleme verilerini kullanma konseptini göstermek için 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. Koddaki çeşitli sabitleri robotunuza göre ayarlamaya dikkat edin. Bazı robotlar diğerlerinden daha kolay dönüş yapar veya sürülür, bu yüzden oransal kontrol sabitleri her durum için ayrı ayrı ayarlanmalıdır. Limelight hedef arama davranışını etkinleştirmeden önce robotun oyun kumandası kontrolörü 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 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 yönlendirme ile başla
        double steer_cmd = tx * STEER_K;
        m_LimelightSteerCommand = steer_cmd;

        // hedef alan istenen alana ulaşana kadar ileri sürmeyi dene
        double drive_cmd = (DESIRED_TARGET_AREA - ta) * DRIVE_K;

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