跳到主要内容

2019 深空机器人

2019年FRC比赛"深空"在许多目标上方设有视觉目标。 以下是Java和Labview的完整示例程序，实现了一种简单的方法来自动驾驶机器人朝向深空中的目标。

这些程序非常简单，仅用于展示使用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 = "默认";
  private static final String kCustomAuto = "我的自动";
  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("默认自动", kDefaultAuto);
        m_chooser.addOption("我的自动", kCustomAuto);
        SmartDashboard.putData("自动选项", 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;
  }
}