From d9b49b96736b054e2f7f9952c641952eba5d1b4b Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Thu, 17 Mar 2022 19:41:20 -0600
Subject: [PATCH] target lock thing on smart dash

---
 .../robot/commands/ShooterCommands/Shoot.java |  8 ++--
 .../commands/ShooterCommands/TrackTarget.java | 37 ++++++++++++-------
 2 files changed, 28 insertions(+), 17 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
index d39f4ce..aabac8f 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
@@ -187,9 +187,9 @@ public class Shoot extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (simMode) {
-      return isAimedInTolerance;
-    }
-    return false;
+    // if (simMode) {
+    return isAimedInTolerance;
+    // }
+    // return false;
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index bf524e6..426bcf2 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/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.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
@@ -40,6 +41,10 @@ public class TrackTarget extends CommandBase {
   Pose2d pos = new Pose2d();
   ArrayList<Point> points = new ArrayList<>();
 
+  private boolean targetLocked = false;
+  private double velocityTolerance = 100.0;
+  private double hoodTolerance = 5.0;
+
   double m=0;
   double b=0;
   boolean isExecuted = false;
@@ -67,62 +72,68 @@ public class TrackTarget extends CommandBase {
   @Override
   public void execute() {
     //m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees());
+    SmartDashboard.putBoolean("Target Locked", this.targetLocked);
+    
     try {
       m_visionOdometry.setLEDs(true);
       points = m_visionOdometry.getTargetPoints();
-
+      
       Point average = VisionOdometry.averagePoint(points);
-
+      
       for(Point point : points) {
         Vector2D difference = new Vector2D(point);
         difference.subtract(new Vector2D(average));
 
         if(difference.magnitude() < VisionConstants.POINTS_THRESHOLD)
-          points.remove(point);
+        points.remove(point);
       }
-
+      
       average = VisionOdometry.averagePoint(points);
       DesmosServer.putPoint("average", average);
-
+      
       for(int i = 0; i < points.size(); i++) {
         DesmosServer.putPoint("Point" + i, points.get(i));
       }
-
+      
       output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
       output *= 4;
       // output *= 0.5;
       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);
-
+      
       updateRegressionDesmos();
       double regressedDistance = distanceRegression(distance);
       regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS;
       DesmosServer.putDouble("distanceReg", regressedDistance);
-
-      //Vision odemetry circle fit based pose estimate
+      
+      //Vision odometry circle fit based pose estimate
       // Point targetOffset = m_visionOdometry.getTargetOffset();
       // DesmosServer.putPoint("targetOff", targetOffset);
-
+      
       vel = m_boomBoom.getVelocity(regressedDistance + 30);
       hood = m_boomBoom.getHood(regressedDistance + 30);
       // m_boomBoom.runDrumShooter(vel);
       m_boomBoom.runDrumShooterVelocityPID(vel);
       m_hood.runAngleAdjustPID(hood);
+      
+      double currentDrumVel = this.m_boomBoom.m_shooterFalconLeft.getSelectedSensorVelocity();
+      double currentHood = this.m_hood.getEncoderPosition();
+  
+      this.targetLocked = (Math.abs(currentDrumVel - vel) < velocityTolerance) && (Math.abs(currentHood - hood) < hoodTolerance);
 
       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){