Added functionality to VisionSubsystem to determine if we can see the…

… inner target, modified VisionRotateToTargetCommand to get the proper angle to the target.

src/main/java/org/frcteam2910/c2020/commands/VisionRotateToTargetCommand.java

@@ -4,25 +4,27 @@
 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;
 import org.frcteam2910.common.robot.drivers.Limelight;
 
 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);

src/main/java/org/frcteam2910/c2020/subsystems/VisionSubsystem.java

@@ -1,53 +1,90 @@
 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");
         distanceToTargetEntry = tab.add("distance to target", 0.0)
                 .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);
         }
     }
 }