From 35a02fba6f64ae31d8110fb0b2b59be3e8f5ea1b Mon Sep 17 00:00:00 2001 From: Shakeel Date: Sat, 8 Feb 2020 14:56:55 -0800 Subject: [PATCH] Added functionality to VisionSubsystem to determine if we can see the inner target, modified VisionRotateToTargetCommand to get the proper angle to the target. --- .../commands/VisionRotateToTargetCommand.java | 12 ++-- .../c2020/subsystems/VisionSubsystem.java | 63 +++++++++++++++---- 2 files changed, 57 insertions(+), 18 deletions(-) diff --git a/src/main/java/org/frcteam2910/c2020/commands/VisionRotateToTargetCommand.java b/src/main/java/org/frcteam2910/c2020/commands/VisionRotateToTargetCommand.java index 84576d5..9057c2c 100644 --- a/src/main/java/org/frcteam2910/c2020/commands/VisionRotateToTargetCommand.java +++ b/src/main/java/org/frcteam2910/c2020/commands/VisionRotateToTargetCommand.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandBase; import org.frcteam2910.c2020.subsystems.DrivetrainSubsystem; +import org.frcteam2910.c2020.subsystems.VisionSubsystem; import org.frcteam2910.common.control.PidConstants; import org.frcteam2910.common.control.PidController; import org.frcteam2910.common.math.Vector2; @@ -11,18 +12,19 @@ public class VisionRotateToTargetCommand extends CommandBase { private static final PidConstants PID_CONSTANTS = new PidConstants(0.0, 0.0, 0.0); - private static final String LIMELIGHT_NAME = "limelight"; - private static final Limelight LIMELIGHT = new Limelight(NetworkTableInstance.getDefault().getTable(LIMELIGHT_NAME)); + private static final Limelight LIMELIGHT = new Limelight(); private final DrivetrainSubsystem drivetrain; + private final VisionSubsystem visionSubsystem; private PidController controller = new PidController(PID_CONSTANTS); private double lastTime = 0.0; - public VisionRotateToTargetCommand(DrivetrainSubsystem drivetrain) { + public VisionRotateToTargetCommand(DrivetrainSubsystem drivetrain, VisionSubsystem visionSubsystem) { this.drivetrain = drivetrain; - + this.visionSubsystem = visionSubsystem; addRequirements(drivetrain); + addRequirements(visionSubsystem); } @Override @@ -40,7 +42,7 @@ public void execute() { if(LIMELIGHT.hasTarget()) { double currentAngle = drivetrain.getPose().rotation.toRadians(); - double targetAngle = drivetrain.getPoseAtTime(time - LIMELIGHT.getPipelineLatency() / 1000.0).rotation.toRadians() - LIMELIGHT.getTargetPosition().x; + double targetAngle = drivetrain.getPoseAtTime(time - LIMELIGHT.getPipelineLatency() / 1000.0).rotation.toRadians() - visionSubsystem.getAngleToTarget().getAsDouble(); controller.setSetpoint(targetAngle); double rotationalVelocity = controller.calculate(currentAngle, dt); drivetrain.drive(Vector2.ZERO, rotationalVelocity, false); diff --git a/src/main/java/org/frcteam2910/c2020/subsystems/VisionSubsystem.java b/src/main/java/org/frcteam2910/c2020/subsystems/VisionSubsystem.java index dc66e49..2e06eb5 100644 --- a/src/main/java/org/frcteam2910/c2020/subsystems/VisionSubsystem.java +++ b/src/main/java/org/frcteam2910/c2020/subsystems/VisionSubsystem.java @@ -1,27 +1,32 @@ package org.frcteam2910.c2020.subsystems; import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Subsystem; +import org.frcteam2910.common.math.MathUtils; import org.frcteam2910.common.math.Vector2; import org.frcteam2910.common.robot.drivers.Limelight; import java.util.OptionalDouble; public class VisionSubsystem implements Subsystem { - private static final double TARGET_HEIGHT = 98.25; - // TODO: Put the actual height of the Limelight - private static final double LIMELIGHT_HEIGHT = 20.0; - // TODO: Put the actual angle the Limelight is mounted at - private static final double LIMELIGHT_MOUNTING_ANGLE = Math.toRadians(20.0); + private static final double TARGET_HEIGHT = 46.25; + private static final double LIMELIGHT_HEIGHT = 5.5; - private final Limelight limelight = new Limelight(); + private static final double INNER_TARGET_DEPTH = 29.25; + + private static final double LIMELIGHT_MOUNTING_ANGLE = Math.toRadians(32.2); + private static final double INNER_TARGET_RANGE_ANGLE = Math.toRadians(16.2); + + private static final Limelight LIMELIGHT = new Limelight(); private final NetworkTableEntry distanceToTargetEntry; + private final NetworkTableEntry canSeeInnerTargetEntry; private OptionalDouble distanceToTarget; + private OptionalDouble angleToTarget; + private boolean canSeeInnerTarget; public VisionSubsystem() { ShuffleboardTab tab = Shuffleboard.getTab("Vision"); @@ -29,25 +34,57 @@ public VisionSubsystem() { .withPosition(0, 0) .withSize(1, 1) .getEntry(); + canSeeInnerTargetEntry = tab.add("can see inner target", false) + .withPosition(0, 0) + .withSize(1, 1) + .getEntry(); } public OptionalDouble getDistanceToTarget() { return distanceToTarget; } + public OptionalDouble getAngleToTarget() { + return angleToTarget; + } + + public boolean isInnerTargetVisible() { + return canSeeInnerTarget; + } + @Override public void periodic() { // Determine whether the Limelight has a target or not - if (limelight.hasTarget()) { - // If so, get the target position and calculate the distance to it - Vector2 targetPosition = limelight.getTargetPosition(); + if (LIMELIGHT.hasTarget()) { + // Calculate the distance to the outer target + Vector2 targetPosition = LIMELIGHT.getTargetPosition(); double theta = LIMELIGHT_MOUNTING_ANGLE + targetPosition.y; - distanceToTarget = OptionalDouble.of((TARGET_HEIGHT - LIMELIGHT_HEIGHT) / Math.tan(theta)); - distanceToTargetEntry.setDouble(distanceToTarget.getAsDouble()); + double distanceToOuterTarget = (TARGET_HEIGHT - LIMELIGHT_HEIGHT) / Math.tan(theta); + double dYOuter = distanceToOuterTarget * Math.sin(targetPosition.x); + double dXOuter = Math.sqrt(Math.pow(distanceToOuterTarget, 2) - Math.pow(dYOuter, 2)); + + // Calculate the distance to the inner target + double dXInner = dXOuter + INNER_TARGET_DEPTH; + double distanceToInnerTarget = Math.hypot(dXInner, dYOuter); + double alpha = Math.asin(dYOuter / dXInner); + + // Bring to light whether we can see the inner target + canSeeInnerTarget = MathUtils.isInRange(-INNER_TARGET_RANGE_ANGLE, INNER_TARGET_RANGE_ANGLE, alpha); + if (canSeeInnerTarget) { + distanceToTarget = OptionalDouble.of(distanceToInnerTarget); + angleToTarget = OptionalDouble.of(alpha); + } else { + distanceToTarget = OptionalDouble.of(distanceToOuterTarget); + angleToTarget = OptionalDouble.of(targetPosition.x); + } } else { - // Else, set distanceToTarget to an empty OptionalDouble distanceToTarget = OptionalDouble.empty(); + angleToTarget = OptionalDouble.empty(); + canSeeInnerTarget = false; + + // Update shuffleboard distanceToTargetEntry.setDouble(-1.0); + canSeeInnerTargetEntry.setBoolean(canSeeInnerTarget); } } }