使用Swerve进行瞄准和测距
完整的示例项目请参见这里
// 使用Limelight进行简单的比例转向控制。
// "比例控制"是一种控制算法,其输出与误差成正比。
// 在这种情况下,我们将返回一个与Limelight的"tx"值成正比的角速度。
double limelight_aim_proportional()
{
// kP(比例常数)
// 这是一个手动调整的数值,用于确定我们的比例控制循环的灵敏度
// 如果它太高,机器人会围绕目标振荡。
// 如果它太低,机器人永远不会达到目标
// 如果机器人从未朝正确方向转动,应该反转kP。
double kP = .035;
// tx的范围是从(-hfov/2)到(hfov/2)度。如果你的目标在
// Limelight 3视野的最右边缘,tx应该返 回大约31度。
double targetingAngularVelocity = LimelightHelpers.getTX("limelight") * kP;
// 转换为弧度/秒,以适应我们的驱动方法
targetingAngularVelocity *= Drivetrain.kMaxAngularSpeed;
// 反转,因为当目标在十字准线右侧时tx为正
targetingAngularVelocity *= -1.0;
return targetingAngularVelocity;
}
// 使用Limelight的"ty"值进行简单的比例距离控制
// 如果你的Limelight安装高度和目标安装高度不同,这种方法效果最佳。
// 如果你的Limelight和目标安装在相同或相似的高度,请使用"ta"(面积)而不是"ty"来进行目标距离测量
double limelight_range_proportional()
{
double kP = .1;
double targetingForwardSpeed = LimelightHelpers.getTY("limelight") * kP;
targetingForwardSpeed *= Drivetrain.kMaxSpeed;
targetingForwardSpeed *= -1.0;
return targetingForwardSpeed;
}
private void drive(boolean fieldRelative) {
// 获取x速度。我们反转这个值,因为Xbox控制器在我们向前推时返回负值。
var xSpeed =
-m_xspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftY(), 0.02))
* Drivetrain.kMaxSpeed;
// 获取y速度或侧向/平移速度。我们反转这个值,因为
// 当我们向左拉时,我们希望得到一个正值。默认情况下,Xbox控制器
// 在你向右拉时返回正值。
var ySpeed =
-m_yspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftX(), 0.02))
* Drivetrain.kMaxSpeed;
// 获取角速度。我们反转这个值,因为我们希望
// 向左拉时得到一个正值(记住,在数学中逆时针为正)。
// 默认情况下,Xbox控制器在你向右拉时返回正值。
var rot =
-m_rotLimiter.calculate(MathUtil.applyDeadband(m_controller.getRightX(), 0.02))
* Drivetrain.kMaxAngularSpeed;
// 当A按钮被按下时,用我们的Limelight方法的输出覆盖一些驱动值
if(m_controller.getAButton())
{
final var rot_limelight = limelight_aim_proportional();
rot = rot_limelight;
final var forward_limelight = limelight_range_proportional();
xSpeed = forward_limelight;
// 使用Limelight时,关闭场地相对驱动。
fieldRelative = false;
}
m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative, getPeriod());
}