Fixed limelight aiming

src/main/java/frc4388/robot/Constants.java

@@ -221,16 +221,17 @@ public final class Constants {
     // public static final double TARGET_HEIGHT = 67.5;
     // public static final double FOV = 29.8; //Field of view limelight
 
-    public static final double LIME_ANGLE = 50;
+    public static final double LIME_ANGLE = 56.4;
 
     public static final String NAME = "photonCamera";
 
     public static final double TARGET_HEIGHT = 8*12 + 8; //TODO: Convert to metric (does this still need to be converted?)
+    public static final double LIME_HEIGHT = 26;
     public static final double TARGET_RADIUS = 4*12; //TODO: Convert to metric (does this still need to be converted?)
     public static final double H_FOV = 59.6;
     public static final double V_FOV = 49.7;
-    public static final double LIME_VIXELS = 960;
-    public static final double LIME_HIXELS = 720;
+    public static final double LIME_HIXELS = 960;
+    public static final double LIME_VIXELS = 720;
     public static final double TURRET_kP = 0.1;
 
     public static final double RANGE = 10;

src/main/java/frc4388/robot/Robot.java

@@ -214,6 +214,8 @@ public class Robot extends TimedRobot {
       } else
         LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
     }
+
+    m_robotContainer.m_robotVisionOdometry.setLEDs(false);
   }
 
   @Override

src/main/java/frc4388/robot/RobotContainer.java

@@ -107,7 +107,7 @@ public class RobotContainer {
   public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
   public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
   public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
-  private final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
+  public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
   private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(30);
   private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(31);
   public final Climber m_robotClimber = new Climber(testElbowMotor);
@@ -304,8 +304,8 @@ public class RobotContainer {
         .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/
 
        //B > Shoot with Lime
-    // new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
-    //     .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));
+    new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
+        .whenPressed(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotSwerveDrive, m_robotVisionOdometry));
   
     /* Button Box Buttons */
 

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

@@ -16,7 +16,9 @@ import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.Hood;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Turret;
+import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.VisionOdometry;
+import frc4388.utility.DesmosServer;
 
 public class TrackTarget extends CommandBase {
   /** Creates a new TrackTarget. */
@@ -37,6 +39,8 @@ public class TrackTarget extends CommandBase {
   Pose2d pos = new Pose2d();
   ArrayList<Point> points = new ArrayList<>();
 
+  boolean isExecuted = false;
+
   // public static Gains m_aimGains;
 
   public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, SwerveDrive drive, VisionOdometry visionOdometry) {
@@ -64,25 +68,37 @@ public class TrackTarget extends CommandBase {
     try {
       m_visionOdometry.setLEDs(true);
       points = m_visionOdometry.getTargetPoints();
-      double pointTotal = 0;
-      for(Point point : points) {
-        pointTotal = pointTotal + point.x;
+      for(int i = 0; i < points.size(); i++) {
+        DesmosServer.putPoint("Point" + i, points.get(i));
       }
-      average = pointTotal/points.size();
-      output = average/VisionConstants.LIME_HIXELS * VisionConstants.TURRET_kP;
-      m_turret.runTurretWithInput(output);
-      pos = m_visionOdometry.getVisionOdometry();
-      distance = Math.hypot(pos.getX(), pos.getY());
+
+      Point average = VisionOdometry.averagePoint(points);
+      DesmosServer.putPoint("average", average);
+
+      output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
+      output *= 2;
+      DesmosServer.putDouble("output", output);
+      // m_turret.runTurretWithInput(output);
+
+      double y_rot = average.y / VisionConstants.LIME_VIXELS;
+      y_rot *= Math.toRadians(VisionConstants.V_FOV);
+      y_rot -= Math.toRadians(VisionConstants.V_FOV) / 2;
+      y_rot += Math.toRadians(VisionConstants.LIME_ANGLE);
+
+      double distance = (VisionConstants.TARGET_HEIGHT - VisionConstants.LIME_HEIGHT) / Math.tan(y_rot);
+      DesmosServer.putDouble("distance", distance);
+
+      // isExecuted = true;
     }
     catch (Exception e){
       System.err.println("Exception: " + e.toString() + ", Line 78 at TrackTarget.java");
     }
-    vel = m_boomBoom.getVelocity(distance);
-    hood = m_boomBoom.getHood(distance);
+    // vel = m_boomBoom.getVelocity(distance);
+    // hood = m_boomBoom.getHood(distance);
     // m_boomBoom.runDrumShooter(vel);
     // m_boomBoom.runDrumShooterVelocityPID(vel);
     // m_hood.runAngleAdjustPID(hood);
-    //m_turret.runshooterRotatePID(m_targetAngle);
+    // m_turret.runshooterRotatePID(m_targetAngle);
   }
 
   // Called once the command ends or is interrupted.
@@ -94,5 +110,6 @@ public class TrackTarget extends CommandBase {
   @Override
   public boolean isFinished() {
     return false;
+    // return isExecuted && Math.abs(output) < .1;
   }
 }