From 088d693bd929034883b1f93fa51fc6d2ab055c96 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 16 Mar 2022 18:54:59 -0600 Subject: [PATCH] SHTUFF --- .../java/frc4388/robot/RobotContainer.java | 4 +- .../frc4388/robot/subsystems/BoomBoom.java | 63 ++++++++++++++++--- .../frc4388/robot/subsystems/SwerveDrive.java | 4 +- .../frc4388/utility/VelocityCorrection.java | 42 ++++++------- 4 files changed, 79 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ebd9715..510fcd5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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], - m_robotSwerveDrive.getChassisSpeeds()[1]); + Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds().vxMetersPerSecond, + 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); diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index c81fd38..bef2d23 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/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 csv = new CSV<>(ShooterTableEntry::new) { + CSV 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 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; - dummyEntry.hoodExt = 0.0; - dummyEntry.drumVelocity = 0.0; - m_shooterTable = new ShooterTableEntry[] { dummyEntry }; + + ShooterTableEntry dummyShooterEntry = new ShooterTableEntry(); + dummyShooterEntry.distance = 0.0; + dummyShooterEntry.hoodExt = 0.0; + 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 diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 262c950..cb7cb9b 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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() { - return new double[] {chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond}; + public ChassisSpeeds getChassisSpeeds() { + return new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond); } /** diff --git a/src/main/java/frc4388/utility/VelocityCorrection.java b/src/main/java/frc4388/utility/VelocityCorrection.java index cc11d0d..c4e32aa 100644 --- a/src/main/java/frc4388/utility/VelocityCorrection.java +++ b/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; - BoomBoom boomBoom; + // subsystems + 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()); - cartesianVelocity = new Vector2D(-2, 3);//new Vector2D(this.swerve.getChassisSpeeds().vxMetersPerSecond, this.swerve.getChassisSpeeds().vyMetersPerSecond); + this.position = new Vector2D(this.swerve.getOdometry().getX(), this.swerve.getOdometry().getY()); + 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); - } - }