2019 深空机器人
2019年FRC比赛"深空"在许多目标上方都有视觉目标。 下面你可以找 到用Java和Labview编写的完整示例程序,这些程序实现了一个简单的方法来自动驾驶机器人朝向深空中的目标。
这些都是非常简单的程序,仅用于展示如何使用Limelight跟踪数据来控制你的机器人的概念。 在每个程序中,你都可以用游戏手柄控制机器人。 如果你按住'A'键,并且Limelight检测到有效目标(取决于你的管道设置), 那么机器人就会自动驾驶朝向目标。 请注意要根据你的特定机器人调整代码中的各种常量。 有些机器人比其他机器人更容易转向或驾驶,所以必须根据具体情况调整比例控制常量。 在启用Limelight寻的行为之前,请确保使用游戏手柄控制器可以正确驾驶机器人。
- Java
- LabView
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() {
}
/**
* 该函数实现了一个简单的方法,基于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;
}
}
这是一个LabView VI的程序框图,用于读取Limelight的跟踪数据并生成驱动和转向命令。这个图像是一个"LabView代码片段"。 只需将图像文件保存到计算 机,然后拖入LabView VI中,程序框图就会被复制。
你也可以从此链接下载完整的LabView源代码