SHTUFF

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

@@ -551,8 +551,8 @@ public class RobotContainer {
     Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
     // FIXME: Chassis speeds are created from joystick inputs and do not reflect
     // actual robot velocity.
-    Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0],
+    Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds().vxMetersPerSecond,
-        m_robotSwerveDrive.getChassisSpeeds()[1]);
+        m_robotSwerveDrive.getChassisSpeeds().vyMetersPerSecond);
     Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
         SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
     pathPoints.add(waypoint);

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

@@ -51,7 +51,12 @@ public class BoomBoom extends SubsystemBase {
     public Double distance, hoodExt, drumVelocity;
   }
 
+  public static class VelocityCorrectionTableEntry {
+    public Double distance, duration;
+  }
+
   private ShooterTableEntry[] m_shooterTable;
+  private VelocityCorrectionTableEntry[] m_velocityCorrectionTable;
 
   /** Creates a new BoomBoom, which has a drum shooter and angle adjuster. */
   public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
@@ -61,7 +66,7 @@ public class BoomBoom extends SubsystemBase {
 
     try {
       // This is a helper class that allows us to read a CSV file into a Java array.
-      CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
+      CSV<ShooterTableEntry> shooterCSV = new CSV<>(ShooterTableEntry::new) {
         // This is a regular expression that removes all parentheses from the header of the CSV file.
         private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)");
 
@@ -80,17 +85,50 @@ public class BoomBoom extends SubsystemBase {
           return super.headerSanitizer(parentheses.matcher(header).replaceAll(""));
         }
       };
+
+      CSV<VelocityCorrectionTableEntry> velocityCorrectionCSV = new CSV<>(VelocityCorrectionTableEntry::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.
-      m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath());
+      m_shooterTable = shooterCSV.read(new File(Filesystem.getDeployDirectory(), "ShooterData.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(m_shooterTable, RobotBase.isSimulation()))).start();
+      
+      // This is reading the CSV file into a Java array.
+      m_velocityCorrectionTable = velocityCorrectionCSV.read(new File(Filesystem.getDeployDirectory(), "VelocityCorrectionData.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(m_velocityCorrectionTable, RobotBase.isSimulation()))).start();
+
     } catch (final IOException exception) {
-      ShooterTableEntry dummyEntry = new ShooterTableEntry();
+
-      dummyEntry.distance = 0.0;
+      ShooterTableEntry dummyShooterEntry = new ShooterTableEntry();
-      dummyEntry.hoodExt = 0.0;
+      dummyShooterEntry.distance = 0.0;
-      dummyEntry.drumVelocity = 0.0;
+      dummyShooterEntry.hoodExt = 0.0;
-      m_shooterTable = new ShooterTableEntry[] { dummyEntry };
+      dummyShooterEntry.drumVelocity = 0.0;
+      m_shooterTable = new ShooterTableEntry[] { dummyShooterEntry };
       LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception);
+
+      VelocityCorrectionTableEntry dummyVelocityCorrectionEntry = new VelocityCorrectionTableEntry();
+      dummyVelocityCorrectionEntry.distance = 0.0;
+      m_velocityCorrectionTable = new VelocityCorrectionTableEntry[] { dummyVelocityCorrectionEntry };
+      LOGGER.log(Level.SEVERE, "Exception while reading velocity correction CSV table.", exception);
     }
   }
 
@@ -116,6 +154,17 @@ public class BoomBoom extends SubsystemBase {
     return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
   }
 
+  /**
+   * This is a function that takes a value (distance) and returns a value (duration) that is a linear
+   * interpolation of the two values (duration) at the two closest points in the table (m_velocityCorrectionTable)
+   * to the given value (distance).
+   * @param distance Distance in velocityCorrection table
+   * @return Duration in milliseconds
+   */
+  public Double getDuration(final Double distance) {
+    return linearInterpolate(m_velocityCorrectionTable, distance, e -> e.distance, e -> e.duration).doubleValue();
+  }
+
   /**
    * 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

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

@@ -178,8 +178,8 @@ public class SwerveDrive extends SubsystemBase {
    * Gets the current chassis speeds in m/s and rad/s.
    * @return Current chassis speeds (vx, vy, ω)
    */
-  public double[] getChassisSpeeds() {
+  public ChassisSpeeds getChassisSpeeds() {
-    return new double[] {chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond};
+    return new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond);
   }
 
   /**

src/main/java/frc4388/utility/VelocityCorrection.java

@@ -4,44 +4,40 @@
 
 package frc4388.utility;
 
-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.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc4388.robot.subsystems.BoomBoom;
 import frc4388.robot.subsystems.SwerveDrive;
 
-/** Add your docs here. */
+/** Aarav's simple VelocityCorrection class for shooting while moving.
+ * @author Aarav Shah
+ */
 public class VelocityCorrection {
 
-  SwerveDrive swerve;
+  // subsystems
-  BoomBoom boomBoom;
+  private SwerveDrive swerve;
+  private BoomBoom drum;
 
   // vectors (in ft and ft/sec)
   public Vector2D position;
-  public Vector2D cartesianVelocity;
+  public Vector2D velocity;
 
-  // find
+  // scalars (in ft and sec)
+  public double radius;
+  public double duration;
+
+  // target (in ft)
   public Vector2D target;
 
   public VelocityCorrection(SwerveDrive swerve, BoomBoom boomBoom) {
 
     this.swerve = swerve;
-    this.boomBoom = boomBoom;
+    this.drum = boomBoom;
 
-    position = new Vector2D(5, 0);//new Vector2D(this.swerve.getOdometry().getX(), this.swerve.getOdometry().getY());
+    this.position = new Vector2D(this.swerve.getOdometry().getX(), this.swerve.getOdometry().getY());
-    cartesianVelocity = new Vector2D(-2, 3);//new Vector2D(this.swerve.getChassisSpeeds().vxMetersPerSecond, this.swerve.getChassisSpeeds().vyMetersPerSecond);
+    this.velocity = new Vector2D(this.swerve.getChassisSpeeds().vxMetersPerSecond, this.swerve.getChassisSpeeds().vyMetersPerSecond);
 
-    target = getTargetPoint();
+    this.radius = this.position.magnitude();
+    this.duration = this.drum.getDuration(this.radius);
+
+    this.target = Vector2D.multiply(this.velocity, -1 * this.duration);
   }
-
-  private Vector2D getTargetPoint() {
-    double approxShotTime = 1; // TODO: get shot time from shooter tables
-    
-    Vector2D targetPoint = Vector2D.multiply(this.cartesianVelocity, -1 * approxShotTime);
-    
-    return Vector2D.round(targetPoint, 5);
-  }
-
 }