メインコンテンツまでスキップ

2019 Deep Space ロボット

2019年のFRCゲーム「Deep Space」では、多くのゴールの上にビジョンターゲットがありました。 以下では、Deep Spaceでゴールに向かって自動的に走行する簡単な方法を実装した、JavaとLabviewの完全なサンプルプログラムを見つけることができます。

これらは非常にシンプルなプログラムで、ライムライトのトラッキングデータを使用してロボットを制御するコンセプトを示すことだけを目的としています。 各プログラムでは、ゲームパッドでロボットを操縦できます。 'A'ボタンを押し続け、ライムライトが有効なターゲットを検出すると （パイプラインの設定に応じて）、ロボットは自動的にターゲットに向かって走行します。 コード内のさまざまな定数を、特定のロボットに合わせて調整するよう注意してください。 ロボットによって回転や走行の容易さが異なるため、比例制御定数の調整はケースバイケースで行う必要があります。 ライムライトのシーキング動作を有効にする前に、ゲームパッドコントローラーを使用してロボットが正しく走行することを確認してください。

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

  /**
   * この関数は、ライムライトカメラからのトラッキングデータに基づいて
   * 走行とステアリングのコマンドを生成する簡単な方法を実装しています。
   */
  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;
  }
}