From 065e48fed1a883f9559e83a5975eb903e9eb8cd5 Mon Sep 17 00:00:00 2001
From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com>
Date: Mon, 21 Mar 2022 19:30:51 -0600
Subject: [PATCH 01/20] Track target

---
 .../robot/commands/ShooterCommands/TrackTarget.java        | 3 +++
 src/main/java/frc4388/robot/subsystems/VisionOdometry.java | 7 +++++++
 2 files changed, 10 insertions(+)

diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index 3c83331..1ec7f17 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -67,6 +67,7 @@ public class TrackTarget extends CommandBase {
   @Override
   public void execute() {    
     try {
+      m_visionOdometry.setDriverMode(false);
       m_visionOdometry.setLEDs(true);
 
       points = m_visionOdometry.getTargetPoints();
@@ -153,6 +154,8 @@ public class TrackTarget extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    m_visionOdometry.setLEDs(false);
+    m_visionOdometry.setDriverMode(true);
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java
index 315623d..0f8f0bd 100644
--- a/src/main/java/frc4388/robot/subsystems/VisionOdometry.java
+++ b/src/main/java/frc4388/robot/subsystems/VisionOdometry.java
@@ -47,6 +47,9 @@ public class VisionOdometry extends SubsystemBase {
     m_camera = new PhotonCamera(VisionConstants.NAME);
     m_drive = drive;
     m_shooter = shooter;
+
+    setLEDs(false);
+    setDriverMode(true);
   }
 
   /** Gets the vision points from the limelight
@@ -93,6 +96,10 @@ public class VisionOdometry extends SubsystemBase {
     m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff);
   }
 
+  public void setDriverMode(boolean driverMode) {
+    m_camera.setDriverMode(driverMode);
+  }
+
   public Point getTargetOffset() throws VisionObscuredException {
     ArrayList<Point> screenPoints = getTargetPoints();
 

From 4c5106d14fccc13e68bfb7dc4f73ddc7a4eedf7f Mon Sep 17 00:00:00 2001
From: Ryan <ryan123rudder@gmail.com>
Date: Mon, 21 Mar 2022 20:00:34 -0600
Subject: [PATCH 02/20] sorta working / some fixes (namely funky turning after
 shooter)

---
 src/main/java/frc4388/robot/Constants.java    |  4 +-
 .../java/frc4388/robot/RobotContainer.java    | 17 +++---
 .../robot/commands/ShooterCommands/Seek.java  |  2 +-
 .../commands/ShooterCommands/TrackTarget.java | 52 +++++++++----------
 4 files changed, 40 insertions(+), 35 deletions(-)

diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index 650a28d..d129c34 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -326,8 +326,8 @@ public final class Constants {
     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 = 45.7;
-    public static final double LIME_HIXELS = 920;
-    public static final double LIME_VIXELS = 720;
+    public static final double LIME_HIXELS = 640;
+    public static final double LIME_VIXELS = 480;
     public static final double TURRET_kP = 0.1;
 
     public static final double POINTS_THRESHOLD = 400;
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index c509da2..165550a 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -218,6 +218,11 @@ public class RobotContainer {
         if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
         if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); }
        }, m_robotClimber));
+    
+    m_robotBoomBoom.setDefaultCommand(
+      new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))
+      );
+
     //  autoInit();
     //  recordInit();
   }
@@ -267,8 +272,8 @@ public class RobotContainer {
         .whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
 
       // X > Toggles extender in and out
-    // new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
-    //     .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
+    new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
+        .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
 
       // A > Spit Out Ball
     new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
@@ -281,11 +286,11 @@ public class RobotContainer {
     
 
       //! Test Buttons
-    new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
-     .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
+    // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+    //  .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
       
-    new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
-     .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
+    new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
+     .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
 
     // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
     //  .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java
index 73f82d0..310b77e 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/Seek.java
@@ -24,6 +24,6 @@ public class Seek extends SequentialCommandGroup {
   public Seek(SwerveDrive swerve, BoomBoom drum, Turret turret, Hood hood, VisionOdometry visionOdometry) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
-    addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(swerve, turret, drum, hood, visionOdometry));
+    addCommands(new Shoot(swerve, drum, turret, hood, visionOdometry, false, false), new TrackTarget(turret, drum, hood, visionOdometry));
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index 1ec7f17..7036b15 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -46,8 +46,7 @@ public class TrackTarget extends CommandBase {
 
   boolean isExecuted = false;
 
-  public TrackTarget (SwerveDrive swerve, Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
-    m_swerve = swerve;
+  public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
     m_turret = turret;
     m_boomBoom = boomBoom;
     m_hood = hood;
@@ -71,22 +70,22 @@ public class TrackTarget extends CommandBase {
       m_visionOdometry.setLEDs(true);
 
       points = m_visionOdometry.getTargetPoints();
-      points = filterPoints(points);
+      // points = filterPoints(points);
       Point average = VisionOdometry.averagePoint(points);
       
       double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
       output *= 2;
       
       m_turret.runTurretWithInput(output);
-      double position = m_turret.m_boomBoomRotateEncoder.getPosition();
+      // double position = m_turret.m_boomBoomRotateEncoder.getPosition();
 
-      if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
-            Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
-        m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
-      else
-        m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
-                                RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
-                                true);
+      // if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
+      //       Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
+      //   m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
+      // else
+      //   m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
+      //                           RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
+      //                           true);
 
       
       double regressedDistance = getDistance(average.y);
@@ -113,25 +112,25 @@ public class TrackTarget extends CommandBase {
     }
   }
 
-  public ArrayList<Point> filterPoints(ArrayList<Point> points) {
-    Point average = VisionOdometry.averagePoint(points);
+  // public ArrayList<Point> filterPoints(ArrayList<Point> points) {
+  //   Point average = VisionOdometry.averagePoint(points);
       
-    HashMap<Point, Double> pointDistances = new HashMap<>();
-    double distanceSum = 0;
+  //   HashMap<Point, Double> pointDistances = new HashMap<>();
+  //   double distanceSum = 0;
 
-    for(Point point : points) {
-      Vector2D difference = new Vector2D(point);
-      difference.subtract(new Vector2D(average));
+  //   for(Point point : points) {
+  //     Vector2D difference = new Vector2D(point);
+  //     difference.subtract(new Vector2D(average));
 
-      double mag = difference.magnitude();
-      distanceSum += mag;
+  //     double mag = difference.magnitude();
+  //     distanceSum += mag;
 
-      pointDistances.put(point, mag);
-    }
+  //     pointDistances.put(point, mag);
+  //   }
 
-    final double averageDist = distanceSum / points.size();
-    return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
-  }
+  //   final double averageDist = distanceSum / points.size();
+  //   return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
+  // }
 
   public final double getDistance(double averageY) {
     double y_rot = averageY / VisionConstants.LIME_VIXELS;
@@ -143,7 +142,8 @@ public class TrackTarget extends CommandBase {
     
     double regressedDistance = distanceRegression(distance);
     regressedDistance += VisionConstants.EDGE_TO_CENTER + VisionConstants.LIMELIGHT_RADIUS;
-
+    SmartDashboard.putNumber("Distance from Lime 123", distance);
+    SmartDashboard.putNumber("Regressed Distance from Lime 123", regressedDistance);
     return regressedDistance;
   }
 

From e83c1e5cf881de27a36db77d47c6b7594b81dc9a Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Mon, 21 Mar 2022 22:49:54 -0600
Subject: [PATCH 03/20] autos better + drive for time + rotate until target

---
 src/main/deploy/pathplanner/L Path.path       | 56 +-----------------
 src/main/deploy/pathplanner/Move Forward.path |  2 +-
 src/main/deploy/pathplanner/Rotate.path       |  2 +-
 src/main/deploy/pathplanner/Test Forward.path |  1 +
 src/main/java/frc4388/robot/Constants.java    | 28 ++++-----
 src/main/java/frc4388/robot/Robot.java        |  3 +-
 .../java/frc4388/robot/RobotContainer.java    | 43 ++++++++++----
 .../DriveCommands/DriveWithInputForTime.java  | 59 +++++++++++++++++++
 .../DriveCommands/RotateUntilTarget.java      | 52 ++++++++++++++++
 .../frc4388/robot/subsystems/SwerveDrive.java | 21 +++++--
 .../robot/subsystems/SwerveModule.java        |  4 ++
 11 files changed, 184 insertions(+), 87 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/Test Forward.path
 create mode 100644 src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
 create mode 100644 src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java

diff --git a/src/main/deploy/pathplanner/L Path.path b/src/main/deploy/pathplanner/L Path.path
index 15b82fa..de23fbf 100644
--- a/src/main/deploy/pathplanner/L Path.path	
+++ b/src/main/deploy/pathplanner/L Path.path	
@@ -1,55 +1 @@
-{
-    "waypoints": [
-        {
-            "anchorPoint": {
-                "x": 2.0,
-                "y": 2.0
-            },
-            "prevControl": null,
-            "nextControl": {
-                "x": 3.0,
-                "y": 2.0
-            },
-            "holonomicAngle": 0.0,
-            "isReversal": false,
-            "velOverride": null,
-            "isLocked": false
-        },
-        {
-            "anchorPoint": {
-                "x": 5.0,
-                "y": 2.0
-            },
-            "prevControl": {
-                "x": 4.000000000000002,
-                "y": 2.0
-            },
-            "nextControl": {
-                "x": 5.004218182347978,
-                "y": 2.0
-            },
-            "holonomicAngle": 0.0,
-            "isReversal": false,
-            "velOverride": null,
-            "isLocked": false
-        },
-        {
-            "anchorPoint": {
-                "x": 5.0,
-                "y": 5.0
-            },
-            "prevControl": {
-                "x": 4.991875448297241,
-                "y": 5.0
-            },
-            "nextControl": null,
-            "holonomicAngle": 0.0,
-            "isReversal": false,
-            "velOverride": null,
-            "isLocked": false
-        }
-    ],
-    "maxVelocity": 1.0,
-    "maxAcceleration": 1.0,
-    "isReversed": null
-}
\ No newline at end of file
+{"waypoints":[{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":null,"nextControl":{"x":5.0,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.000000000000002,"y":2.0},"nextControl":{"x":5.004218182347978,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":4.991875448297241,"y":3.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/Move Forward.path b/src/main/deploy/pathplanner/Move Forward.path
index b63994d..1e2c78c 100644
--- a/src/main/deploy/pathplanner/Move Forward.path	
+++ b/src/main/deploy/pathplanner/Move Forward.path	
@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":3.8761638155965237,"y":2.097178183811552},"prevControl":null,"nextControl":{"x":3.891027717524718,"y":2.097178183811552},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.989837747689554,"y":2.097178183811552},"prevControl":{"x":4.984427304969435,"y":2.097178183811552},"nextControl":{"x":4.984427304969435,"y":2.097178183811552},"holonomicAngle":91.52752544221289,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.8761638155965237,"y":2.140568077269722},"prevControl":{"x":3.3359301436282043,"y":2.140568077269722},"nextControl":null,"holonomicAngle":178.2100893917539,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
\ No newline at end of file
+{"waypoints":[{"anchorPoint":{"x":3.0,"y":2.0},"prevControl":null,"nextControl":{"x":3.0148639019281944,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.45976632803168,"y":2.0},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/Rotate.path b/src/main/deploy/pathplanner/Rotate.path
index 0707701..bc807e0 100644
--- a/src/main/deploy/pathplanner/Rotate.path
+++ b/src/main/deploy/pathplanner/Rotate.path
@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":false}
\ No newline at end of file
+{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":0.05,"maxAcceleration":0.05,"isReversed":false}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/Test Forward.path b/src/main/deploy/pathplanner/Test Forward.path
new file mode 100644
index 0000000..428cb4a
--- /dev/null
+++ b/src/main/deploy/pathplanner/Test Forward.path	
@@ -0,0 +1 @@
+{"waypoints":[{"anchorPoint":{"x":0.0,"y":0.0},"prevControl":null,"nextControl":{"x":1.0,"y":0.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":0.0},"prevControl":{"x":3.0,"y":0.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
\ No newline at end of file
diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java
index d129c34..7101859 100644
--- a/src/main/java/frc4388/robot/Constants.java
+++ b/src/main/java/frc4388/robot/Constants.java
@@ -46,18 +46,18 @@ public final class Constants {
     public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant?
 
     // IDs
-    public static final int LEFT_FRONT_STEER_CAN_ID = 2;           // *
-    public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;           // *
-    public static final int RIGHT_FRONT_STEER_CAN_ID = 4;          // *
-    public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;          // *
-    public static final int LEFT_BACK_STEER_CAN_ID = 6;            // *
-    public static final int LEFT_BACK_WHEEL_CAN_ID = 7;            // *
-    public static final int RIGHT_BACK_STEER_CAN_ID = 8;           // *
-    public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;           // *
-    public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;  // *
-    public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // *
-    public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;   // *
-    public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;  // *
+    public static final int LEFT_FRONT_STEER_CAN_ID = 2;
+    public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
+    public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
+    public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
+    public static final int LEFT_BACK_STEER_CAN_ID = 6;
+    public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
+    public static final int RIGHT_BACK_STEER_CAN_ID = 8;
+    public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
+    public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
+    public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
+    public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
+    public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
     public static final int GYRO_ID = 14;
 
     // offsets are in degrees
@@ -85,8 +85,8 @@ public final class Constants {
     // swerve auto constants
     public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
     public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
-    public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0.1, 0.3,
-        new TrapezoidProfile.Constraints(Math.PI, Math.PI));
+    public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(2.0, 0, 0.01,//0.1, 0.3,
+        new TrapezoidProfile.Constraints(Math.PI, Math.PI/2));
     public static final boolean PATH_RECORD_VELOCITY = true;
     public static final double PATH_MAX_VELOCITY = 5.0;
     public static final double PATH_MAX_ACCELERATION = 5.0;
diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 9929191..80d5759 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -213,8 +213,7 @@ public class Robot extends TimedRobot {
    * This function is called periodically during autonomous.
    */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 165550a..55bbf1f 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -46,6 +46,8 @@ import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.Trajectory.State;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.Filesystem;
@@ -59,6 +61,7 @@ import edu.wpi.first.wpilibj2.command.NotifierCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.Claws;
@@ -71,7 +74,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
 import frc4388.robot.commands.ClimberCommands.RunClaw;
 import frc4388.robot.commands.ClimberCommands.RunClimberPath;
-// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
+import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
+import frc4388.robot.commands.DriveCommands.RotateUntilTarget;
 import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
 import frc4388.robot.commands.ShooterCommands.AimToCenter;
 import frc4388.robot.commands.ShooterCommands.Seek;
@@ -97,6 +101,7 @@ import frc4388.utility.PathPlannerUtil;
 import frc4388.utility.Vector2D;
 import frc4388.utility.PathPlannerUtil.Path.Waypoint;
 import frc4388.utility.controller.ButtonBox;
+import frc4388.utility.controller.DeadbandedRawXboxController;
 import frc4388.utility.controller.DeadbandedXboxController;
 
 /**
@@ -128,8 +133,8 @@ public class RobotContainer {
   public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
 
   // Controllers
-  private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
-  private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
+  private final static XboxController m_driverXbox = new DeadbandedRawXboxController(OIConstants.XBOX_DRIVER_ID);
+  private final static XboxController m_operatorXbox = new DeadbandedRawXboxController(OIConstants.XBOX_OPERATOR_ID);
   private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
 
   // Autonomous
@@ -371,8 +376,11 @@ public class RobotContainer {
       if (inputs[i] instanceof String) {
 
         PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
-        PathPlannerState initState = (PathPlannerState) traj.sample(0);
-        commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
+        PathPlannerState initState = traj.getInitialState();
+
+        Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation);
+
+        commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive));
         commands.add(new PPSwerveControllerCommand(
                           traj,
                           m_robotSwerveDrive::getOdometry,
@@ -383,12 +391,13 @@ public class RobotContainer {
                           m_robotSwerveDrive::setModuleStates,
                           m_robotSwerveDrive));
       }
+
       if (inputs[i] instanceof Command) {
         commands.add((Command) inputs[i]);
       }
     }
     
-    commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
+    commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive));
     Command[] ret = new Command[commands.size()];
     ret = commands.toArray(ret);
     return ret;
@@ -421,6 +430,13 @@ public class RobotContainer {
     //   }).withName("No Autonomous Path");
     // }
 
+    PIDController xController = SwerveDriveConstants.X_CONTROLLER;
+    PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
+    ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
+    // thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+    loadPath("Move Forward");
+
     // ! this will run each of the specified PathPlanner paths in sequence.
     // * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
 
@@ -429,8 +445,16 @@ public class RobotContainer {
     // *                                           null,
     // *                                           new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")),
     // *                                           new RunCommand(() -> m_robotIntake.runAtOutput(0.5))));
+    
+    // return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset(), m_robotSwerveDrive),
+    //                                                       // new InstantCommand(() -> this.resetOdometry(new Pose2d())),
+    //                                                       new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive), 
+    //                                                       "Diamond"));
 
-    return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command
+    return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
+                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
+                                       new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
+                                       new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
   }
 
   public static XboxController getDriverController() {
@@ -563,7 +587,7 @@ public class RobotContainer {
   }
 
   private void saveRecording() {
-    // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
+    // ! IMPORTANT: Had to chown the pathplanner folder in order to save autos.
     File outputFile = PATHPLANNER_DIRECTORY
         .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
     LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
@@ -584,8 +608,7 @@ public class RobotContainer {
   public void recordPeriodic() {
     Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
     Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
-    // FIXME: Chassis speeds are created from joystick inputs and do not reflect
-    // actual robot velocity.
+    // * FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity
     Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0],
         m_robotSwerveDrive.getChassisSpeeds()[1]);
     Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
new file mode 100644
index 0000000..4a04a22
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.DriveCommands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class DriveWithInputForTime extends CommandBase {
+
+  // subsystems
+  SwerveDrive swerve;
+
+  double[] inputs;
+
+  // timing
+  long start;
+  long elapsed;
+  double duration;
+
+  /**
+   * DriveWithInput for a specified amount of time.
+   * @param inputs Inputs used in DriveWithInput (xspeed, yspeed, xrot, yrot).
+   * @param time Time to DriveWithInput for, in seconds.
+   */
+  public DriveWithInputForTime(SwerveDrive swerve, double[] inputs, double duration) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.swerve = swerve;
+    this.inputs = inputs;
+    this.duration = duration;
+
+    addRequirements(this.swerve);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    start = System.currentTimeMillis();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    elapsed = System.currentTimeMillis() - start;
+    this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return (((long) duration) >= (start - elapsed));
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java
new file mode 100644
index 0000000..76ec283
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java
@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.DriveCommands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.VisionOdometry;
+
+public class RotateUntilTarget extends CommandBase {
+
+  // subsystems
+  SwerveDrive swerve;
+  VisionOdometry visionOdometry;
+
+  double rotateSpeed;
+
+  /** Creates a new RotateUntilTarget. */
+  public RotateUntilTarget(SwerveDrive swerve, VisionOdometry visionOdometry, double rotateSpeed) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.swerve = swerve;
+    this.visionOdometry = visionOdometry;
+
+    this.rotateSpeed = rotateSpeed;
+
+    addRequirements(this.swerve, this.visionOdometry);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.visionOdometry.setLEDs(true);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    this.swerve.driveWithInput(0.0, 0.0, rotateSpeed, true);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return this.visionOdometry.m_camera.getLatestResult().hasTargets();
+  }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index ed9c193..f913873 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -158,6 +158,13 @@ public class SwerveDrive extends SubsystemBase {
     // modules[0].setDesiredState(desiredStates[0], false);
   }
 
+  public void setModuleRotationsToAngle(double angle) {
+    for (int i = 0; i < modules.length; i++) {
+      SwerveModule module = modules[i];
+      module.rotateToAngle(angle);
+    }
+  }
+
   @Override
   public void periodic() {
 
@@ -228,10 +235,16 @@ public class SwerveDrive extends SubsystemBase {
    * Updates the field relative position of the robot.
    */
   public void updateOdometry() {
-    m_poseEstimator.update( getRegGyro(),
-                            modules[0].getState(), 
-                            modules[1].getState(), 
-                            modules[2].getState(), 
+    Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI/2));
+    Rotation2d actual =  new Rotation2d(-1 * m_gyro.getRotation2d().getRadians());
+
+    SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
+    SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
+
+    m_poseEstimator.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
+                            modules[0].getState(),
+                            modules[1].getState(),
+                            modules[2].getState(),
                             modules[3].getState());
   }
   
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
index d4905af..59e9e13 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
@@ -182,6 +182,10 @@ public class SwerveModule extends SubsystemBase {
     angleMotor.set(0);
   }
 
+  public void rotateToAngle(double angle) {
+    this.angleMotor.set(TalonFXControlMode.Position, angle);
+  }
+
   @Override
   public void periodic() {
 

From 1833d518550a5ecdbbb0d8ee4f49cd656041c9a2 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Mon, 21 Mar 2022 23:13:12 -0600
Subject: [PATCH 04/20] ank;dfh

---
 .../frc4388/robot/commands/ShooterCommands/TrackTarget.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index 7036b15..8a51cc8 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -90,9 +90,9 @@ public class TrackTarget extends CommandBase {
       
       double regressedDistance = getDistance(average.y);
       
-      // ! add 30 to the distance to get in target. May need to be adjusted
-      velocity = m_boomBoom.getVelocity(regressedDistance + 30);
-      hoodPosition = m_boomBoom.getHood(regressedDistance + 30);
+      // ! no longer a +30 lol -aarav
+      velocity = m_boomBoom.getVelocity(regressedDistance);
+      hoodPosition = m_boomBoom.getHood(regressedDistance);
       
       m_boomBoom.runDrumShooterVelocityPID(velocity);
       m_hood.runAngleAdjustPID(hoodPosition);

From d35cc8432ef76eca1a8582386c87ffe088c463b4 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Mon, 21 Mar 2022 23:47:30 -0600
Subject: [PATCH 05/20] tried using getTargetPoints + turret already aimed auto

---
 src/main/java/frc4388/robot/RobotContainer.java          | 8 +++++++-
 .../robot/commands/DriveCommands/RotateUntilTarget.java  | 9 ++++++++-
 2 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 55bbf1f..c6deaeb 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -451,10 +451,16 @@ public class RobotContainer {
     //                                                       new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive), 
     //                                                       "Diamond"));
 
+    // * assume turret is already pointed towards target.
     return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
                                        new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
-                                       new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
                                        new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
+
+    // * aim with RotateUntilTarget
+    // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
+    //                                    new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
+    //                                    new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
+    //                                    new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
   }
 
   public static XboxController getDriverController() {
diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java
index 76ec283..c7e0149 100644
--- a/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java
+++ b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java
@@ -4,9 +4,12 @@
 
 package frc4388.robot.commands.DriveCommands;
 
+import javax.swing.plaf.basic.BasicTreeUI.TreeCancelEditingAction;
+
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.VisionOdometry;
+import frc4388.utility.VisionObscuredException;
 
 public class RotateUntilTarget extends CommandBase {
 
@@ -47,6 +50,10 @@ public class RotateUntilTarget extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return this.visionOdometry.m_camera.getLatestResult().hasTargets();
+
+    try { this.visionOdometry.getTargetPoints(); } catch (VisionObscuredException voe) { return false; }
+
+    return true;
+    // return this.visionOdometry.m_camera.getLatestResult().hasTargets();
   }
 }

From 5649408afeca2144986f514c99efc9f93279f13e Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Tue, 22 Mar 2022 00:06:49 -0600
Subject: [PATCH 06/20] Create RunCommandForTime.java

---
 .../robot/commands/RunCommandForTime.java     | 78 +++++++++++++++++++
 1 file changed, 78 insertions(+)
 create mode 100644 src/main/java/frc4388/robot/commands/RunCommandForTime.java

diff --git a/src/main/java/frc4388/robot/commands/RunCommandForTime.java b/src/main/java/frc4388/robot/commands/RunCommandForTime.java
new file mode 100644
index 0000000..0424b30
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/RunCommandForTime.java
@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+public class RunCommandForTime extends CommandBase {
+
+  // command
+  Command command;
+
+  // timing
+  long start;
+  long elapsed;
+  double duration;
+
+  // override
+  boolean override;
+
+  /**
+   * Runs given command for given time.
+   * @param command Command to run.
+   * @param duration Time to run for, in seconds.
+   * @param override If true: end command when time ends, even if the command isn't finished. If false: end command when it finished and time has ended.
+   */
+  public RunCommandForTime(Command command, double duration, boolean override) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.command = command;
+    this.duration = duration;
+    this.override = override;
+
+    addRequirements(this.command.getRequirements().toArray(Subsystem[]::new));
+  }
+
+  /**
+   * Runs given command for given time.
+   * @param command Command to run.
+   * @param duration Time to run for, in seconds.
+   */
+  public RunCommandForTime(Command command, double duration) {
+    this(command, duration, false);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.start = System.currentTimeMillis();
+    this.command.initialize();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    this.elapsed = System.currentTimeMillis() - this.start;
+    this.command.execute();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    this.command.end(interrupted);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (this.override) {
+      return (((long) this.duration) >= (this.elapsed - this.start));
+    } else {
+      return (this.command.isFinished() && (((long) this.duration) >= (this.elapsed - this.start)));
+    }
+  }
+}

From 24d72af2d9195809bdb9bcf1b24872ed14099bcc Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Tue, 22 Mar 2022 00:18:37 -0600
Subject: [PATCH 07/20] TT timing in auto, idk if works

---
 .../java/frc4388/robot/RobotContainer.java    |  2 +-
 .../commands/ShooterCommands/TrackTarget.java | 33 +++++++++++++++++--
 2 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index c6deaeb..cd1daf3 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -454,7 +454,7 @@ public class RobotContainer {
     // * assume turret is already pointed towards target.
     return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
                                        new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
-                                       new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
+                                       new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true));
 
     // * aim with RotateUntilTarget
     // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index 8a51cc8..5fc7593 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -35,6 +35,8 @@ public class TrackTarget extends CommandBase {
   BoomBoom m_boomBoom;
   Hood m_hood;
 
+  boolean isAuto;
+
   static double velocity;
   static double hoodPosition;
 
@@ -46,18 +48,35 @@ public class TrackTarget extends CommandBase {
 
   boolean isExecuted = false;
 
-  public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
+  // timing
+  boolean isAimed;
+
+  boolean timerStarted;
+  long startTime;
+  private double timeTolerance;
+
+  public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) {
     m_turret = turret;
     m_boomBoom = boomBoom;
     m_hood = hood;
     m_visionOdometry = visionOdometry;
 
+    this.isAuto = isAuto;
+    this.timeTolerance = 1000;
+
     addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry);
   }
 
+  public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
+    this(turret, boomBoom, hood, visionOdometry, false);
+  }
+
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    timerStarted = false;
+    startTime = 0;
+
     velocity = 0;
     hoodPosition = 0;
   }
@@ -110,6 +129,8 @@ public class TrackTarget extends CommandBase {
     } catch (Exception e){
       e.printStackTrace();
     }
+
+    // timing
   }
 
   // public ArrayList<Point> filterPoints(ArrayList<Point> points) {
@@ -161,7 +182,15 @@ public class TrackTarget extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    if (this.isAuto) {
+      if (targetLocked& !timerStarted) {
+        timerStarted = true;
+        startTime = System.currentTimeMillis();
+      }
+      return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
+    } else {
+      return false;
+    }
     // return isExecuted && Math.abs(output) < .1;
   }
 }

From 500f812bf5600d595162f6b905c07b76e5c3e40f Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Tue, 22 Mar 2022 00:18:49 -0600
Subject: [PATCH 08/20] aarav is stupid. that is all.

---
 .../robot/commands/DriveCommands/DriveWithInputForTime.java     | 2 +-
 src/main/java/frc4388/robot/commands/RunCommandForTime.java     | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
index 4a04a22..351109e 100644
--- a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
+++ b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
@@ -29,7 +29,7 @@ public class DriveWithInputForTime extends CommandBase {
 
     this.swerve = swerve;
     this.inputs = inputs;
-    this.duration = duration;
+    this.duration = duration * 1000; // ! convert seconds to milliseconds, duh
 
     addRequirements(this.swerve);
   }
diff --git a/src/main/java/frc4388/robot/commands/RunCommandForTime.java b/src/main/java/frc4388/robot/commands/RunCommandForTime.java
index 0424b30..25b24ff 100644
--- a/src/main/java/frc4388/robot/commands/RunCommandForTime.java
+++ b/src/main/java/frc4388/robot/commands/RunCommandForTime.java
@@ -31,7 +31,7 @@ public class RunCommandForTime extends CommandBase {
     // Use addRequirements() here to declare subsystem dependencies.
 
     this.command = command;
-    this.duration = duration;
+    this.duration = duration * 1000; // ! convert seconds to milliseconds, duh
     this.override = override;
 
     addRequirements(this.command.getRequirements().toArray(Subsystem[]::new));

From 2e22a1a23cb80a526f083303a08c0e26d910538a Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Tue, 22 Mar 2022 00:28:17 -0600
Subject: [PATCH 09/20] added storage running to autos

---
 src/main/java/frc4388/robot/RobotContainer.java       | 11 +++++++++--
 .../robot/commands/ShooterCommands/TrackTarget.java   |  3 ++-
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index cd1daf3..a008486 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -59,6 +59,7 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.NotifierCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
@@ -454,13 +455,19 @@ public class RobotContainer {
     // * assume turret is already pointed towards target.
     return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
                                        new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
-                                       new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true));
+                                       new ParallelRaceGroup(
+                                         new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
+                                         new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
+                                       ));
 
     // * aim with RotateUntilTarget
     // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
     //                                    new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
     //                                    new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
-    //                                    new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
+    //                                    new ParallelRaceGroup(
+    //                                      new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
+    //                                      new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
+    //                                    ));
   }
 
   public static XboxController getDriverController() {
diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index 5fc7593..7bf4fd4 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -130,7 +130,8 @@ public class TrackTarget extends CommandBase {
       e.printStackTrace();
     }
 
-    // timing
+    // run storage
+    
   }
 
   // public ArrayList<Point> filterPoints(ArrayList<Point> points) {

From 8af4535191f5eabbb6c632e110299546986399b9 Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Tue, 22 Mar 2022 00:33:05 -0600
Subject: [PATCH 10/20] fixed time logic

---
 src/main/java/frc4388/robot/RobotContainer.java               | 2 ++
 .../robot/commands/DriveCommands/DriveWithInputForTime.java   | 2 +-
 src/main/java/frc4388/robot/commands/RunCommandForTime.java   | 4 ++--
 .../java/frc4388/robot/commands/StorageCommands/Drum.java     | 0
 4 files changed, 5 insertions(+), 3 deletions(-)
 delete mode 100644 src/main/java/frc4388/robot/commands/StorageCommands/Drum.java

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index a008486..9bd41dc 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -460,6 +460,8 @@ public class RobotContainer {
                                          new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
                                        ));
 
+    // TODO: we should test TrackTarget timing with my RunCommandForTime thing at some point, same with DriveWithInput timing
+
     // * aim with RotateUntilTarget
     // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
     //                                    new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
index 351109e..6f436b9 100644
--- a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
+++ b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
@@ -54,6 +54,6 @@ public class DriveWithInputForTime extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return (((long) duration) >= (start - elapsed));
+    return (elapsed >= duration);
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/RunCommandForTime.java b/src/main/java/frc4388/robot/commands/RunCommandForTime.java
index 25b24ff..5e35682 100644
--- a/src/main/java/frc4388/robot/commands/RunCommandForTime.java
+++ b/src/main/java/frc4388/robot/commands/RunCommandForTime.java
@@ -70,9 +70,9 @@ public class RunCommandForTime extends CommandBase {
   @Override
   public boolean isFinished() {
     if (this.override) {
-      return (((long) this.duration) >= (this.elapsed - this.start));
+      return (this.elapsed >= this.duration);
     } else {
-      return (this.command.isFinished() && (((long) this.duration) >= (this.elapsed - this.start)));
+      return (this.command.isFinished() && (this.elapsed >= this.duration));
     }
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/StorageCommands/Drum.java b/src/main/java/frc4388/robot/commands/StorageCommands/Drum.java
deleted file mode 100644
index e69de29..0000000

From 2316b1d4268eaa1ff61b6da9ebbc88978c3ba6d2 Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Tue, 22 Mar 2022 14:32:56 -0600
Subject: [PATCH 11/20] Robot Rock

auto thignbleljkefoijwejfoiwjfojfowjfkjkjvkjjfkwje
---
 .../robot/commands/PlaybackDriveInput.java    | 156 ++++++++++++++++++
 .../robot/commands/RecordDriveInput.java      | 106 ++++++++++++
 2 files changed, 262 insertions(+)
 create mode 100644 src/main/java/frc4388/robot/commands/PlaybackDriveInput.java
 create mode 100644 src/main/java/frc4388/robot/commands/RecordDriveInput.java

diff --git a/src/main/java/frc4388/robot/commands/PlaybackDriveInput.java b/src/main/java/frc4388/robot/commands/PlaybackDriveInput.java
new file mode 100644
index 0000000..9827145
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/PlaybackDriveInput.java
@@ -0,0 +1,156 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import java.io.File;
+import java.io.IOException;
+import java.util.Comparator;
+import java.util.Map;
+import java.util.Optional;
+import java.util.function.Function;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+import java.util.regex.Pattern;
+import java.util.stream.IntStream;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.utility.CSV;
+
+public class PlaybackDriveInput extends CommandBase {
+  private static final Logger LOGGER = Logger.getLogger(PlaybackDriveInput.class.getSimpleName());
+
+  public static class DriveInputEntry {
+    public double millis, leftAxisX, leftAxisY, rightAxisX;
+  }
+
+  private DriveInputEntry[] driveInputEntries;
+
+  private SwerveDrive swerve;
+  private long startTime;
+
+  /** Creates a new PlaybackDriveInput. */
+  public PlaybackDriveInput(SwerveDrive swerve, String autoName) {
+    this.swerve = swerve;
+
+    try {
+      // This is a helper class that allows us to read a CSV file into a Java array.
+      CSV<DriveInputEntry> csv = new CSV<>(DriveInputEntry::new) {
+        // This is a regular expression that removes all parentheses from the header of the CSV file.
+        private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
+
+        /**
+         * Removes the parentheses from the CSV header
+         * 
+         * @param header The header to be sanitized.
+         * @return The headerSanitizer method is overriding the headerSanitizer method in the parent class.
+         *         The parentheses.matcher(header) is matching the header with the parentheses regular
+         *         expression. The replaceAll method is replacing all of the parentheses with an empty
+         *         string. The super.headerSanitizer(parentheses.matcher(header).replaceAll("")) is calling
+         *         the parent sanitizer.
+         */
+        @Override
+        protected String headerSanitizer(final String header) {
+          return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
+        }
+      };
+      // This is reading the CSV file into a Java array.
+      driveInputEntries = csv.read(new File(Filesystem.getDeployDirectory(), autoName + ".csv").toPath());
+      // This is a running a helper method that is logging the contents of the table to the console on a new thread.
+      // ! new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(driveInputEntries, RobotBase.isSimulation()))).start();
+    } catch (final IOException exception) {
+      DriveInputEntry dummyEntry = new DriveInputEntry();
+      dummyEntry.millis = 0.0;
+      dummyEntry.leftAxisX = 0.0;
+      dummyEntry.leftAxisY = 0.0;
+      dummyEntry.rightAxisX = 0.0;
+      driveInputEntries = new DriveInputEntry[] { dummyEntry };
+      LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception);
+    }
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    startTime = System.currentTimeMillis();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    long currentTime = System.currentTimeMillis() - startTime;
+
+    double leftAxisX = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.leftAxisX).doubleValue();
+    double leftAxisY = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.leftAxisY).doubleValue();
+    double rightAxisX = linearInterpolate(driveInputEntries, currentTime, e -> e.millis, e -> e.rightAxisX).doubleValue();
+
+    swerve.driveWithInput(leftAxisX, leftAxisY, rightAxisX, true);
+  }
+
+  /**
+   * Using the given lookup value (x) and lookup getter function, locates the nearest entries in the
+   * given table to be used as the lower (x0) and upper (x1) bounds for interpolation. Returns the
+   * interpolation (y) between the two values (y0 and y1) accquired by applying the target getter
+   * function to the lower and upper bounds entries.
+   * 
+   * @param table An array of entries to search through.
+   * @param lookupValue The value to lookup in the table.
+   * @param lookupGetter A function that takes an entry from the table and returns .
+   * @param targetGetter A function that takes an E and returns a Number.
+   * @return The interpolated value.
+   */
+  private static <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
+    final Map.Entry<Integer, E> closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1]));
+    final E closestRecord = closestEntry.getValue();
+    final int closestRecordIndex = closestEntry.getKey();
+    final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))];
+    return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord));
+  }
+
+  /**
+   * If the value is in the table, return the entry. Otherwise, return the entry with the closest
+   * value
+   * 
+   * @param table The array of values to search.
+   * @param value The value to search for.
+   * @param valueGetter A function that takes an element of the table and returns a Number to compare
+   *        with the given value.
+   * @param exactMatch If true, the lookup will only return a match if the value is exactly equal to
+   *        the value of the entry. If false, the lookup will return a match with a value closest to
+   *        the given value.
+   * @return The entry with the closest value to the given value.
+   */
+  private static <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
+    final Optional<Map.Entry<Integer, E>> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue())));
+    return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty();
+  }
+
+  /**
+   * Given a value x, and two values x0 and x1, and two values y0 and y1, return a value y that is a
+   * linear interpolation of the two values y0 and y1
+   * 
+   * @param x The value to interpolate.
+   * @param x0 the x coordinate of the lower bound to interpolate to
+   * @param y0 The value at x0.
+   * @param x1 the x-coordinate of the upper bound to interpolate to
+   * @param y1 The value at x1.
+   * @return The interpolation between y0 and y1 at x.
+   */
+  private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) {
+    final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue());
+    return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc4388/robot/commands/RecordDriveInput.java b/src/main/java/frc4388/robot/commands/RecordDriveInput.java
new file mode 100644
index 0000000..b5437e3
--- /dev/null
+++ b/src/main/java/frc4388/robot/commands/RecordDriveInput.java
@@ -0,0 +1,106 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands;
+
+import java.io.File;
+import java.io.FileOutputStream;
+import java.io.PrintWriter;
+import java.util.HashMap;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class RecordDriveInput extends CommandBase {
+  private HashMap<Long, double[]> timedInput;
+
+  private SwerveDrive swerve;
+
+  private Supplier<Double> leftAxisXSupplier;
+  private Supplier<Double> leftAxisYSupplier;
+  private Supplier<Double> rightAxisXSupplier;
+
+  private String autoName;
+
+  private long startTime;
+  private long count;
+
+  private int nthRecord;
+
+  /** Creates a new RecordDriveInput. */
+  public RecordDriveInput(
+        SwerveDrive swerve,
+        Supplier<Double> leftAxisXSupplier,
+        Supplier<Double> leftAxisYSupplier,
+        Supplier<Double> rightAxisXSupplier,
+        String autoName,
+        int nthRecord)
+  {
+    this.swerve = swerve;
+    this.leftAxisXSupplier = leftAxisXSupplier;
+    this.leftAxisYSupplier = leftAxisYSupplier;
+    this.rightAxisXSupplier = rightAxisXSupplier;
+    this.autoName = autoName;
+    this.nthRecord = nthRecord;
+
+    timedInput = new HashMap<>();
+
+    addRequirements(this.swerve);
+  }
+
+  public RecordDriveInput(
+        SwerveDrive swerve,
+        Supplier<Double> leftAxisXSupplier,
+        Supplier<Double> leftAxisYSupplier,
+        Supplier<Double> rightAxisXSupplier,
+        String autoName)
+  {
+    this(swerve, leftAxisXSupplier, leftAxisYSupplier, rightAxisXSupplier, autoName, 1);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    startTime = System.currentTimeMillis();
+    count = 0;
+
+    timedInput.put((long) 0, new double[] {0, 0, 0});
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    long timeFromStart = System.currentTimeMillis() - startTime;
+    double[] input = {leftAxisXSupplier.get(), leftAxisYSupplier.get(), rightAxisXSupplier.get()};
+
+    if(count % (long) nthRecord == 0)
+      timedInput.put(timeFromStart, input);
+
+    swerve.driveWithInput(input[0], input[1], input[2], true);
+    count++;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    File csvOutput = new File(autoName + ".csv");
+    try(PrintWriter writer = new PrintWriter(csvOutput)) {
+      writer.println("millis,leftx,lefty,rightx");
+
+      for(long millis : timedInput.keySet())
+        writer.println(millis + "," + timedInput.get(millis)[0] + "," + timedInput.get(millis)[1] + "," + timedInput.get(millis)[2]);
+
+      writer.close();
+    } catch(Exception e) {
+      e.printStackTrace();
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
\ No newline at end of file

From 9e95856cbc723bb40112a8e32016e42bacb820d2 Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Tue, 22 Mar 2022 16:44:01 -0600
Subject: [PATCH 12/20] stop at zero

---
 src/main/java/frc4388/robot/commands/RecordDriveInput.java | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/java/frc4388/robot/commands/RecordDriveInput.java b/src/main/java/frc4388/robot/commands/RecordDriveInput.java
index b5437e3..19e27f3 100644
--- a/src/main/java/frc4388/robot/commands/RecordDriveInput.java
+++ b/src/main/java/frc4388/robot/commands/RecordDriveInput.java
@@ -92,6 +92,7 @@ public class RecordDriveInput extends CommandBase {
       for(long millis : timedInput.keySet())
         writer.println(millis + "," + timedInput.get(millis)[0] + "," + timedInput.get(millis)[1] + "," + timedInput.get(millis)[2]);
 
+      writer.println((millis + 1) + ",0,0,0");
       writer.close();
     } catch(Exception e) {
       e.printStackTrace();

From 81ef2a858ae64086b7dfda0edd3b2eff1884f88c Mon Sep 17 00:00:00 2001
From: aarav18 <aaravshah18@gmail.com>
Date: Tue, 22 Mar 2022 16:56:40 -0600
Subject: [PATCH 13/20] tested RunCommandForTime + DWIForTime in robotsim, both
 work

---
 simgui.json                                   |  1 +
 .../java/frc4388/robot/RobotContainer.java    | 32 ++++++++++---------
 .../DriveCommands/DriveWithInputForTime.java  |  6 +++-
 .../robot/commands/RunCommandForTime.java     |  3 ++
 .../frc4388/robot/subsystems/BoomBoom.java    | 12 +++----
 5 files changed, 32 insertions(+), 22 deletions(-)

diff --git a/simgui.json b/simgui.json
index f3617f0..2a797b5 100644
--- a/simgui.json
+++ b/simgui.json
@@ -22,6 +22,7 @@
     "types": {
       "/FMSInfo": "FMSInfo",
       "/LiveWindow/BoomBoom": "Subsystem",
+      "/LiveWindow/Claws": "Subsystem",
       "/LiveWindow/Climber": "Subsystem",
       "/LiveWindow/Extender": "Subsystem",
       "/LiveWindow/Hood": "Subsystem",
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 9bd41dc..ee57648 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -72,6 +72,7 @@ import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.StorageConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.commands.RunCommandForTime;
 import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
 import frc4388.robot.commands.ClimberCommands.RunClaw;
 import frc4388.robot.commands.ClimberCommands.RunClimberPath;
@@ -226,7 +227,7 @@ public class RobotContainer {
        }, m_robotClimber));
     
     m_robotBoomBoom.setDefaultCommand(
-      new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))
+      new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
       );
 
     //  autoInit();
@@ -431,13 +432,6 @@ public class RobotContainer {
     //   }).withName("No Autonomous Path");
     // }
 
-    PIDController xController = SwerveDriveConstants.X_CONTROLLER;
-    PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
-    ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
-    // thetaController.enableContinuousInput(-Math.PI, Math.PI);
-
-    loadPath("Move Forward");
-
     // ! this will run each of the specified PathPlanner paths in sequence.
     // * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
 
@@ -453,14 +447,22 @@ public class RobotContainer {
     //                                                       "Diamond"));
 
     // * assume turret is already pointed towards target.
-    return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
-                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
-                                       new ParallelRaceGroup(
-                                         new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
-                                         new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
-                                       ));
+    // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
+    //                                    new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
+    //                                    new ParallelRaceGroup(
+    //                                      new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
+    //                                      new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
+    //                                    ));
 
-    // TODO: we should test TrackTarget timing with my RunCommandForTime thing at some point, same with DriveWithInput timing
+    // return new RunCommandForTime(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, true), m_robotSwerveDrive), 1.0, true);
+
+    return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
+                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.3, 0.0, 0.0}, 1.0));//,
+                                      //  new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
+                                      //  new ParallelCommandGroup(
+                                      //    new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
+                                      //    new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0)
+                                       //));
 
     // * aim with RotateUntilTarget
     // return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
index 6f436b9..a2ce001 100644
--- a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
+++ b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
@@ -43,6 +43,7 @@ public class DriveWithInputForTime extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    System.out.println("RUNNING");
     elapsed = System.currentTimeMillis() - start;
     this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true);
   }
@@ -54,6 +55,9 @@ public class DriveWithInputForTime extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return (elapsed >= duration);
+    System.out.println("Duration: " + duration);
+    System.out.println("Elapsed: " + elapsed);
+
+    return ((double) elapsed >= duration);
   }
 }
diff --git a/src/main/java/frc4388/robot/commands/RunCommandForTime.java b/src/main/java/frc4388/robot/commands/RunCommandForTime.java
index 5e35682..b66947d 100644
--- a/src/main/java/frc4388/robot/commands/RunCommandForTime.java
+++ b/src/main/java/frc4388/robot/commands/RunCommandForTime.java
@@ -56,6 +56,7 @@ public class RunCommandForTime extends CommandBase {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    System.out.println("RUNNING");
     this.elapsed = System.currentTimeMillis() - this.start;
     this.command.execute();
   }
@@ -70,6 +71,8 @@ public class RunCommandForTime extends CommandBase {
   @Override
   public boolean isFinished() {
     if (this.override) {
+      System.out.println("Duration: " + duration);
+      System.out.println("Elapsed: " + elapsed);
       return (this.elapsed >= this.duration);
     } else {
       return (this.command.isFinished() && (this.elapsed >= this.duration));
diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java
index 8e8c334..82eb661 100644
--- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java
+++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java
@@ -174,9 +174,9 @@ public class BoomBoom extends SubsystemBase {
   public void periodic() {
     // This method will be called once per scheduler run
     // speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0);
-    SmartDashboard.putNumber("Shooter Current", getCurrent());
-    SmartDashboard.putNumber("Shooter Voltage",  m_shooterFalconLeft.getMotorOutputVoltage());
-    SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
+    // SmartDashboard.putNumber("Shooter Current", getCurrent());
+    // SmartDashboard.putNumber("Shooter Voltage",  m_shooterFalconLeft.getMotorOutputVoltage());
+    // SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
   }
 
   public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
@@ -191,9 +191,9 @@ public class BoomBoom extends SubsystemBase {
   public void runDrumShooter(double speed) {
     // m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2);
     m_shooterFalconLeft.set(speed);
-    SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
-    SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
-    SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
+    // SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
+    // SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
+    // SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
 
   }
 

From 020cb41a486e44f8af0228c8da1fafa983caabb2 Mon Sep 17 00:00:00 2001
From: Ryan <ryan123rudder@gmail.com>
Date: Tue, 22 Mar 2022 17:07:06 -0600
Subject: [PATCH 14/20] current ver (ryan)

---
 .../java/frc4388/robot/RobotContainer.java    |  4 +-
 .../commands/ShooterCommands/TrackTarget.java | 52 +++++++++----------
 2 files changed, 28 insertions(+), 28 deletions(-)

diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 9bd41dc..cf9d43d 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/src/main/java/frc4388/robot/RobotContainer.java
@@ -226,7 +226,7 @@ public class RobotContainer {
        }, m_robotClimber));
     
     m_robotBoomBoom.setDefaultCommand(
-      new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))
+      new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
       );
 
     //  autoInit();
@@ -296,7 +296,7 @@ public class RobotContainer {
     //  .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
       
     new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
-     .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
+     .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, false));
 
     // new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
     //  .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index 7bf4fd4..c611fb9 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -89,29 +89,29 @@ public class TrackTarget extends CommandBase {
       m_visionOdometry.setLEDs(true);
 
       points = m_visionOdometry.getTargetPoints();
-      // points = filterPoints(points);
+      points = filterPoints(points);
       Point average = VisionOdometry.averagePoint(points);
       
       double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
-      output *= 2;
+      output *= 2.0;
       
       m_turret.runTurretWithInput(output);
-      // double position = m_turret.m_boomBoomRotateEncoder.getPosition();
+      double position = m_turret.m_boomBoomRotateEncoder.getPosition();
 
-      // if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
-      //       Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
-      //   m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
-      // else
-      //   m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
-      //                           RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
-      //                           true);
+      if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
+            Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
+        m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
+      else
+        m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
+                                RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
+                                true);
 
       
       double regressedDistance = getDistance(average.y);
       
       // ! no longer a +30 lol -aarav
-      velocity = m_boomBoom.getVelocity(regressedDistance);
-      hoodPosition = m_boomBoom.getHood(regressedDistance);
+      velocity = m_boomBoom.getVelocity(regressedDistance + 30);
+      hoodPosition = m_boomBoom.getHood(regressedDistance + 30);
       
       m_boomBoom.runDrumShooterVelocityPID(velocity);
       m_hood.runAngleAdjustPID(hoodPosition);
@@ -134,25 +134,25 @@ public class TrackTarget extends CommandBase {
     
   }
 
-  // public ArrayList<Point> filterPoints(ArrayList<Point> points) {
-  //   Point average = VisionOdometry.averagePoint(points);
+  public ArrayList<Point> filterPoints(ArrayList<Point> points) {
+    Point average = VisionOdometry.averagePoint(points);
       
-  //   HashMap<Point, Double> pointDistances = new HashMap<>();
-  //   double distanceSum = 0;
+    HashMap<Point, Double> pointDistances = new HashMap<>();
+    double distanceSum = 0;
 
-  //   for(Point point : points) {
-  //     Vector2D difference = new Vector2D(point);
-  //     difference.subtract(new Vector2D(average));
+    for(Point point : points) {
+      Vector2D difference = new Vector2D(point);
+      difference.subtract(new Vector2D(average));
 
-  //     double mag = difference.magnitude();
-  //     distanceSum += mag;
+      double mag = difference.magnitude();
+      distanceSum += mag;
 
-  //     pointDistances.put(point, mag);
-  //   }
+      pointDistances.put(point, mag);
+    }
 
-  //   final double averageDist = distanceSum / points.size();
-  //   return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
-  // }
+    final double averageDist = distanceSum / points.size();
+    return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 1.3 * averageDist).collect(Collectors.toList());
+  }
 
   public final double getDistance(double averageY) {
     double y_rot = averageY / VisionConstants.LIME_VIXELS;

From 308cd1ea2340f3ba5fdce4da0b279c1da665d6fb Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Tue, 22 Mar 2022 17:07:59 -0600
Subject: [PATCH 15/20] Updated testing shit

---
 .../frc4388/robot/commands/RecordDriveInput.java |  2 +-
 .../commands/ShooterCommands/TrackTarget.java    | 16 ++++++++++++++--
 2 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/RecordDriveInput.java b/src/main/java/frc4388/robot/commands/RecordDriveInput.java
index 19e27f3..cc120fc 100644
--- a/src/main/java/frc4388/robot/commands/RecordDriveInput.java
+++ b/src/main/java/frc4388/robot/commands/RecordDriveInput.java
@@ -92,7 +92,7 @@ public class RecordDriveInput extends CommandBase {
       for(long millis : timedInput.keySet())
         writer.println(millis + "," + timedInput.get(millis)[0] + "," + timedInput.get(millis)[1] + "," + timedInput.get(millis)[2]);
 
-      writer.println((millis + 1) + ",0,0,0");
+      //// writer.println(( + 1) + ",0,0,0");
       writer.close();
     } catch(Exception e) {
       e.printStackTrace();
diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index 7bf4fd4..910d229 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -88,8 +88,9 @@ public class TrackTarget extends CommandBase {
       m_visionOdometry.setDriverMode(false);
       m_visionOdometry.setLEDs(true);
 
-      points = m_visionOdometry.getTargetPoints();
-      // points = filterPoints(points);
+      //// points = m_visionOdometry.getTargetPoints();
+      points = getFakePoints();
+      //// points = filterPoints(points);
       Point average = VisionOdometry.averagePoint(points);
       
       double output = (average.x - VisionConstants.LIME_HIXELS/2.d) / VisionConstants.LIME_HIXELS;
@@ -154,6 +155,17 @@ public class TrackTarget extends CommandBase {
   //   return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
   // }
 
+  public final ArrayList<Point> getFakePoints() {
+    ArrayList<Point> fakePoints = new ArrayList<>();
+
+    for(int i = 0; i < 10; i++) {
+      Point p = new Point((Math.random() * 20) - 10 + (VisionConstants.LIME_HIXELS/2), (Math.random() * 20) - 10 + (VisionConstants.LIME_VIXELS/2));
+      fakePoints.add(p);
+    }
+
+    return fakePoints;
+  }
+
   public final double getDistance(double averageY) {
     double y_rot = averageY / VisionConstants.LIME_VIXELS;
     y_rot *= Math.toRadians(VisionConstants.V_FOV);

From f16206624a155b04b507843aac33deb8498abcfc Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Tue, 22 Mar 2022 17:16:07 -0600
Subject: [PATCH 16/20] comment go zoom

---
 .../commands/ShooterCommands/TrackTarget.java | 24 ++++++++++---------
 1 file changed, 13 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index 8cdf73f..8346f62 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -195,15 +195,17 @@ public class TrackTarget extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (this.isAuto) {
-      if (targetLocked& !timerStarted) {
-        timerStarted = true;
-        startTime = System.currentTimeMillis();
-      }
-      return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
-    } else {
-      return false;
-    }
-    // return isExecuted && Math.abs(output) < .1;
-  }
+  ////   if (this.isAuto) {
+  ////     if (targetLocked& !timerStarted) {
+  ////       timerStarted = true;
+  ////       startTime = System.currentTimeMillis();
+  ////     }
+  ////     return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
+  ////   } else {
+  ////     return false;
+  ////   }
+  //   // return isExecuted && Math.abs(output) < .1;
+  //// }
+
+  return false;
 }

From 74bfd5795705db0bb6e43bd88b9bf0570ce0df4e Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Tue, 22 Mar 2022 17:16:36 -0600
Subject: [PATCH 17/20] brackeys

---
 .../frc4388/robot/commands/ShooterCommands/TrackTarget.java    | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index 8346f62..765ab66 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -207,5 +207,6 @@ public class TrackTarget extends CommandBase {
   //   // return isExecuted && Math.abs(output) < .1;
   //// }
 
-  return false;
+    return false;
+  }
 }

From 162b3f7db593592407016023d5768dda4fc6e6c0 Mon Sep 17 00:00:00 2001
From: Ryan <ryan123rudder@gmail.com>
Date: Tue, 22 Mar 2022 17:23:11 -0600
Subject: [PATCH 18/20] ,mjnb

---
 .../frc4388/robot/commands/ShooterCommands/TrackTarget.java   | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index 8cdf73f..92d2442 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -88,8 +88,8 @@ public class TrackTarget extends CommandBase {
       m_visionOdometry.setDriverMode(false);
       m_visionOdometry.setLEDs(true);
 
-      //// points = m_visionOdometry.getTargetPoints();
-      points = getFakePoints();
+      points = m_visionOdometry.getTargetPoints();
+      // points = getFakePoints();
       //// points = filterPoints(points);
       Point average = VisionOdometry.averagePoint(points);
       

From 0c88db571a50a378a327a42ba9c435633e2bd068 Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Tue, 22 Mar 2022 17:34:54 -0600
Subject: [PATCH 19/20] desno

---
 src/main/java/frc4388/robot/Robot.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 80d5759..c6a4114 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -45,7 +45,7 @@ public class Robot extends TimedRobot {
   
   private double current;
 
-  private static DesmosServer desmosServer = new DesmosServer(8000);
+  // private static DesmosServer desmosServer = new DesmosServer(8000);
 
   public static Alliance alliance;
 
@@ -120,7 +120,7 @@ public class Robot extends TimedRobot {
       }
     });
 
-    desmosServer.start();
+    // desmosServer.start();
     m_robotContainer.m_robotVisionOdometry.setLEDs(false);
     // DesmosServer.putTable("table", "x1", new double[] {44}, "y1", new double[] {0});
   }

From 1193a3ecefbc433e571953baabf40ff8a773b9df Mon Sep 17 00:00:00 2001
From: Ryan <ryan123rudder@gmail.com>
Date: Tue, 22 Mar 2022 17:36:29 -0600
Subject: [PATCH 20/20] asdfghj

---
 .../commands/ShooterCommands/TrackTarget.java | 20 +++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index 3c6dc57..0428899 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -97,22 +97,22 @@ public class TrackTarget extends CommandBase {
       output *= 2.0;
       
       m_turret.runTurretWithInput(output);
-      double position = m_turret.m_boomBoomRotateEncoder.getPosition();
+      // double position = m_turret.m_boomBoomRotateEncoder.getPosition();
 
-      if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
-            Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
-        m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
-      else
-        m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
-                                RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
-                                true);
+      // if(Math.abs(position - ShooterConstants.TURRET_FORWARD_SOFT_LIMIT) < 5 ||
+      //       Math.abs(position - ShooterConstants.TURRET_REVERSE_SOFT_LIMIT) < 5)
+      //   m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(), output, true);
+      // else
+      //   m_swerve.driveWithInput(RobotContainer.getDriverController().getLeftX(), RobotContainer.getDriverController().getLeftY(),
+      //                           RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
+      //                           true);
 
       
       double regressedDistance = getDistance(average.y);
       
       // ! no longer a +30 lol -aarav
-      velocity = m_boomBoom.getVelocity(regressedDistance + 30);
-      hoodPosition = m_boomBoom.getHood(regressedDistance + 30);
+      velocity = m_boomBoom.getVelocity(regressedDistance);
+      hoodPosition = m_boomBoom.getHood(regressedDistance);
       
       m_boomBoom.runDrumShooterVelocityPID(velocity);
       m_hood.runAngleAdjustPID(hoodPosition);