velocity correction and shooter tables time

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();