boilerplate vel correction

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

@@ -19,12 +19,14 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.drive.Vector2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc4388.utility.RobotTime;
+import frc4388.utility.VelocityCorrection;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -138,9 +140,19 @@ public class Robot extends TimedRobot {
     // robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
+
+    // velocity correction tests
+    VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom);
     
     //SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition());
     //SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition());
+    // System.out.println("Position: (" + vc.position.x + ", " + vc.position.y + ")");
+    // System.out.println("Velocity: (" + vc.cartesianVelocity.x + ", " + vc.cartesianVelocity.y + ")");
+    // System.out.println("Tangential Velocity: (" + vc.tangentialVelocity.x + ", " + vc.tangentialVelocity.y + ")");
+    // System.out.println("Radial Velocity: (" + vc.radialVelocity.x + ", " + vc.radialVelocity.y + ")");
+
+    SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition());
+    SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition());
 
     // odo chooser stuff
     addOdoChoices(new Pose2d(1, 1, new Rotation2d(Math.PI/6)), 

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

@@ -414,8 +414,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);

src/main/java/frc4388/robot/commands/Shoot.java

@@ -96,7 +96,7 @@ public class Shoot extends CommandBase {
    * Updates error for custom PID.
    */
   public void updateError() {
-    m_targetPoint = new Pose2d(hTargetDistanceFromHub(), vTargetDistanceFromHub(), SwerveDriveConstants.HUB_POSE.getRotation());
+    m_targetPoint = SwerveDriveConstants.HUB_POSE;
     m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
     // m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
     error = (m_targetAngle - turretDummy.get() + 360) % 360;
@@ -152,43 +152,6 @@ public class Shoot extends CommandBase {
     output = kP * proportional + kI * integral + kD * derivative;
     normOutput = output/360 * inverted;
   }
-  // TODO: horizontal velocity correction
-  public double hTargetDistanceFromHub() {
-    
-    double hVel = m_swerve.getChassisSpeeds()[0];
-    double velBeforeCorrection = m_boomBoom.getVelocity(m_distance);
-    double vDistanceFromHub = m_odoY;
-    double approxTravelTime = vDistanceFromHub / velBeforeCorrection;
-    double hTargetDistanceFromHub = hVel * approxTravelTime;
-    
-    // return hTargetDistanceFromHub;
-    return 0.0; // this is for no velocity correction
-  }
-
-  public Pose2d findTargetPoint() {
-
-    // position vector and radius
-    Translation2d position = new Translation2d(m_odoX, m_odoY);
-    double radius = position.getNorm();
-
-    // equation of circle = x^2 + y^2 = m_distance^2
-    // derivative of circle = 2x + 2y * y' = 0 --> y' = -x/y
-
-    // velocity vector (x, y)
-    Translation2d cartesianVelocity = new Translation2d(m_swerve.getChassisSpeeds()[0], m_swerve.getChassisSpeeds()[1]);
-
-    // unit tangential vector
-    Translation2d tangential = new Translation2d(0, 0);
-    // velocity vector (tangential, radial)
-    Translation2d polarVelocity = new Translation2d(0, 0);
-
-    return SwerveDriveConstants.HUB_POSE;
-  }
-
-  // TODO: vertical velocity correction
-  public double vTargetDistanceFromHub() {
-    return 0.0; // this is for no velocity correction
-  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override

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

@@ -5,6 +5,7 @@
 package frc4388.robot.subsystems;
 
 import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
+import com.ctre.phoenix.sensors.PigeonIMUConfiguration;
 import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 
 import edu.wpi.first.math.VecBuilder;
@@ -186,8 +187,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 chassisSpeeds;
   }
 
   /**