velocity correction and shooter tables time

src/main/deploy/ShooterData.csv

@@ -1,5 +1,5 @@
-Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds)
-96            ,-25.00          ,0.425
-144           ,-47.57          ,0.475
-192           ,-55.55          ,0.500
-240           ,-96.00          ,0.525
+Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Time (s)
+96            ,-25.00        ,0.425                ,0
+144           ,-47.57        ,0.                   ,0
+192           ,-55.55        ,0.500                ,0
+240           ,-96.00        ,0.525                ,0

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

@@ -25,6 +25,7 @@ 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
@@ -150,6 +151,12 @@ public class Robot extends TimedRobot {
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
     
+    VelocityCorrection vc = new VelocityCorrection(m_robotContainer.m_robotSwerveDrive, m_robotContainer.m_robotBoomBoom);
+    System.out.println("Position: " + vc.position);
+    System.out.println("Velocity: " + vc.cartesianVelocity);
+    System.out.println("Target: " + vc.target.toString());
+
+
     //SmartDashboard.putNumber("Turret Encoder Position", m_robotContainer.m_robotTurret.m_boomBoomRotateEncoder.getPosition());
     //SmartDashboard.putNumber("Hood Encoder Position", m_robotContainer.m_robotHood.m_angleEncoder.getPosition());
 

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

@@ -163,6 +163,7 @@ public class SwerveDrive extends SubsystemBase {
     updateSmartDash();
 
     SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
+    SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
 
     m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
     super.periodic();

src/main/java/frc4388/utility/Vector2D.java

@@ -0,0 +1,56 @@
+// 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.utility;
+
+import edu.wpi.first.wpilibj.drive.Vector2d;
+
+/** Aarav's good vector class (better than WPILib) */
+public class Vector2D extends Vector2d {
+
+  public double x;
+  public double y;
+
+  public Vector2D() {
+    super();
+  }
+
+  public Vector2D(double x, double y) {
+    super(x, y);
+
+    this.x = x;
+    this.y = y;
+  }
+
+  public static Vector2D add(Vector2D v1, Vector2D v2) {
+    return new Vector2D(v1.x + v2.x, v1.y + v2.y);
+  }
+
+  public static Vector2D subtract(Vector2D v1, Vector2D v2) {
+    return new Vector2D(v1.x - v2.x, v1.y - v2.y);
+  }
+
+  public static Vector2D multiply(Vector2D v1, double scalar) {
+    return new Vector2D(scalar * v1.x, scalar * v1.y);
+  }
+
+  public static Vector2D round(Vector2D v, int places) {
+    int scale = (int) Math.pow(10, places);
+
+    v = Vector2D.multiply(v, scale);
+
+    v.x = Math.round(v.x);
+    v.y = Math.round(v.y);
+    v.x = v.x / scale;
+    v.y = v.y / scale;
+
+    return v;
+  }
+
+  @Override
+  public String toString() {
+    return ("(" + this.x + ", " + this.y + ")");
+  }
+
+}

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

@@ -0,0 +1,47 @@
+// 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.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. */
+public class VelocityCorrection {
+
+  SwerveDrive swerve;
+  BoomBoom boomBoom;
+
+  // vectors (in ft and ft/sec)
+  public Vector2D position;
+  public Vector2D cartesianVelocity;
+
+  // find
+  public Vector2D target;
+
+  public VelocityCorrection(SwerveDrive swerve, BoomBoom boomBoom) {
+
+    this.swerve = swerve;
+    this.boomBoom = 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);
+
+    target = getTargetPoint();
+  }
+
+  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);
+  }
+
+}