Track target working

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

@@ -236,8 +236,8 @@ public final class Constants {
     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_HIXELS = 960;
+    public static final double V_FOV = 45.7;
+    public static final double LIME_HIXELS = 920;
     public static final double LIME_VIXELS = 720;
     public static final double TURRET_kP = 0.1;
 

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

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

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

@@ -321,7 +321,8 @@ public class RobotContainer {
        //B > Shoot with Lime
     new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
         .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry))
-        .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false)));
+        .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false)))
+        .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)));
         // .whileHeld%
 
   

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

@@ -9,6 +9,7 @@ import java.util.ArrayList;
 import org.opencv.core.Point;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
 import frc4388.robot.Constants.VisionConstants;
@@ -76,7 +77,8 @@ public class TrackTarget extends CommandBase {
       DesmosServer.putPoint("average", average);
 
       output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
-      output *= 2;
+      output *= 4;
+      // output *= 0.5;
       DesmosServer.putDouble("output", output);
       m_turret.runTurretWithInput(output);
 
@@ -94,15 +96,21 @@ public class TrackTarget extends CommandBase {
       DesmosServer.putDouble("distanceReg", regressedDistance);
 
       //Vision odemetry circle fit based pose estimate
-      Point targetOffset = m_visionOdometry.getTargetOffset();
-      DesmosServer.putPoint("targetOff", targetOffset);
+      // Point targetOffset = m_visionOdometry.getTargetOffset();
+      // DesmosServer.putPoint("targetOff", targetOffset);
 
-      vel = m_boomBoom.getVelocity(distance);
-      hood = m_boomBoom.getHood(distance);
+      vel = m_boomBoom.getVelocity(regressedDistance + 30);
+      hood = m_boomBoom.getHood(regressedDistance + 30);
       // m_boomBoom.runDrumShooter(vel);
       m_boomBoom.runDrumShooterVelocityPID(vel);
       m_hood.runAngleAdjustPID(hood);
 
+      SmartDashboard.putNumber("Regressed Distance", regressedDistance);
+      SmartDashboard.putNumber("Distance", distance);
+      SmartDashboard.putNumber("Hood Target Angle Track", hood);
+      SmartDashboard.putNumber("Vel Target Track", vel);
+
+
       // isExecuted = true;
     }
     catch (Exception e){
@@ -113,7 +121,7 @@ public class TrackTarget extends CommandBase {
   }
 
   public final double distanceRegression(double distance) {
-    return (1.09517561985 * distance + 20.1846165624);
+    return (79.6078 * Math.pow(1.01343, distance) - 56.6671);
   }
 
   public void updateRegressionDesmos() {

src/main/java/frc4388/robot/subsystems/BoomBoom.java

@@ -217,7 +217,7 @@ public class BoomBoom extends SubsystemBase {
 
   public void runDrumShooterVelocityPID(double targetVel) {
     SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset);
-    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel + pidOffset); // Init
+    m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
     
     // New BoomBoom controller stuff
     // Controls a motor with the output of the BangBang controller

src/main/java/frc4388/robot/subsystems/VisionOdometry.java

@@ -59,6 +59,8 @@ public class VisionOdometry extends SubsystemBase {
   public ArrayList<Point> getTargetPoints() throws VisionObscuredException {
     PhotonPipelineResult result = m_camera.getLatestResult();
     latency = result.getLatencyMillis();
+
+    System.out.println("Result: " + result.hasTargets() + ", latency: " + latency);
   
     if(!result.hasTargets())
       throw new VisionObscuredException();