Merge branch 'testRoboReveal' of https://github.com/Team4388/2022NoWayHome into testRoboReveal

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/RobotContainer.java

@@ -79,6 +79,7 @@ import frc4388.robot.subsystems.VisionOdometry;
 import frc4388.utility.LEDPatterns;
 import frc4388.utility.ListeningSendableChooser;
 import frc4388.utility.PathPlannerUtil;
+import frc4388.utility.Vector2D;
 import frc4388.utility.PathPlannerUtil.Path.Waypoint;
 import frc4388.utility.controller.DeadbandedXboxController;
 

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

@@ -47,14 +47,12 @@ public class BoomBoom extends SubsystemBase {
   // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later
 
   public static class ShooterTableEntry {
-    public Double distance, hoodExt, drumVelocity;
+    public Double distance, hoodExt, drumVelocity, duration;
   }
 
   private ShooterTableEntry[] m_shooterTable;
-  /*
-  * Creates new BoomBoom subsystem, has drum shooter and angle adjuster
-  */
-  /** Creates a new BoomBoom. */
+
+  /** Creates a new BoomBoom, which has a drum shooter and angle adjuster. */
   public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
     m_shooterFalconLeft = shooterFalconLeft;
     m_shooterFalconRight = shooterFalconRight;
@@ -94,20 +92,39 @@ public class BoomBoom extends SubsystemBase {
     }
   }
 
+  /**
+   * This is a function that takes a value (distance) and returns a value (drumVelocity) that is a
+   * linear interpolation of the two values (drumVelocity) at the two closest points in the table
+   * (m_shooterTable) to the given value (distance). 
+   * @param distance Distance in shooter table
+   * @return Drum Velocity in units per 100 ms
+   */
   public Double getVelocity(final Double distance) {
-    // This is a function that takes a value (distance) and returns a value (drumVelocity) that is a
-    // linear interpolation of the two values (drumVelocity) at the two closest points in the table
-    // (m_shooterTable) to the given value (distance).
     return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
   }
 
+  /**
+   * This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear
+   * interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable)
+   * to the given value (distance).
+   * @param distance Distance in shooter table
+   * @return Hood extension in units
+   */
   public Double getHood(final Double distance) {
-    // This is a function that takes a value (distance) and returns a value (hoodExt) that is a linear
-    // interpolation of the two values (hoodExt) at the two closest points in the table (m_shooterTable)
-    // to the given value (distance).
     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_shooterTable)
+   * to the given value (distance).
+   * @param distance Distance in shooter table
+   * @return Shot duration in seconds
+   */
+  public Double getDuration(final Double distance) {
+    return linearInterpolate(m_shooterTable, 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

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