Add Shooting capabilities

src/main/java/frc4388/robot/commands/TrackTarget.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.VisionConstants;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Shooter;
@@ -28,6 +29,8 @@ public class TrackTarget extends CommandBase {
   double yAngle = 0;
   double target = 0;
   public double distance;
+  public static double fireVel;
+  public static double fireAngle;
 
   /**
    * Uses the Limelight to track the target
@@ -46,6 +49,13 @@ public class TrackTarget extends CommandBase {
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
 
+  public double addFireVel() {
+    return fireVel;
+  }
+  public double addFireAngle() {
+    return fireAngle;
+  }
+
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
@@ -65,6 +75,12 @@ public class TrackTarget extends CommandBase {
         //Finding Distance
       distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
       SmartDashboard.putNumber("Distance to Target", distance);
+
+      double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT);
+      double xVel = (distance*VisionConstants.GRAV)/(yVel);
+
+      fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
+      fireAngle = Math.atan(yVel/xVel);
     }
   }