Add ballistic trajectory simulation

src/main/java/frc/game/Field.java

@@ -3,8 +3,10 @@
 import static edu.wpi.first.units.Units.*;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rectangle2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.units.measure.Distance;
 import frc.robot.Robot;
@@ -17,6 +19,7 @@ public class Field {
   private static final Distance hub_x_centerPos = Inches.of(181.56);
   private static final Distance hub_x_len = Inches.of(47);
   private static final Distance hub_y_len = Inches.of(47);
+  private static final Distance hub_z_len = Inches.of(72);
   private static final Distance trench_y_len = Inches.of(49.86);
   private static final Distance bump_y_len = Inches.of(73);
   private static final Distance depot_y_centerPos = centerField_y_pos.plus(Inches.of(75.93));
@@ -36,12 +39,14 @@ public class Field {
       (centerField_x_pos.minus(hub_x_centerPos.plus(hub_x_len.div(2)))).times(2);
   private static final Pose2d fieldCenter =
       new Pose2d(new Translation2d(centerField_x_pos, centerField_y_pos), Rotation2d.kZero);
-  private static final Pose2d blueHubCenter =
-      new Pose2d(new Translation2d(hub_x_centerPos, centerField_y_pos), Rotation2d.kZero);
-  private static final Pose2d redHubCenter =
-      new Pose2d(
-          new Translation2d(centerField_x_pos.times(2).minus(hub_x_centerPos), centerField_y_pos),
-          Rotation2d.kZero);
+  public static final Pose3d blueHubCenter =
+      new Pose3d(hub_x_centerPos, centerField_y_pos, hub_z_len, Rotation3d.kZero);
+  public static final Pose3d redHubCenter =
+      new Pose3d(
+          centerField_x_pos.times(2).minus(hub_x_centerPos),
+          centerField_y_pos,
+          hub_z_len,
+          Rotation3d.kZero);
 
   enum Region {
     BlueZone(
@@ -52,8 +57,8 @@ enum Region {
         new Rectangle2d(
             new Translation2d(field_x_len.minus(allianceZone_x_len), Inches.of(0)),
             new Translation2d(field_x_len, field_y_len))),
-    BlueHub(new Rectangle2d(blueHubCenter, hub_x_len, hub_y_len)),
-    RedHub(new Rectangle2d(redHubCenter, hub_x_len, hub_y_len)),
+    BlueHub(new Rectangle2d(blueHubCenter.toPose2d(), hub_x_len, hub_y_len)),
+    RedHub(new Rectangle2d(redHubCenter.toPose2d(), hub_x_len, hub_y_len)),
     RedLeftTrench(
         new Rectangle2d(
             new Translation2d(

src/main/java/frc/game/GameState.java

@@ -1,9 +1,8 @@
 package frc.game;
 
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import frc.game.Field.Region;
 import frc.robot.Robot;
 import java.util.List;
 import java.util.Optional;
@@ -122,10 +121,10 @@ public static void logValues() {
     Logger.recordOutput("GameState/IsMyHubActive", isMyHubActive());
   }
 
-  public static Translation2d getMyHubLocation() {
+  public static Pose3d getMyHubPose() {
     if (Robot.getAlliance() == Alliance.Red) {
-      return Region.RedHub.getCenter().getTranslation();
+      return Field.redHubCenter;
     }
-    return Region.BlueHub.getCenter().getTranslation();
+    return Field.blueHubCenter;
   }
 }

src/main/java/frc/robot/Robot.java

@@ -34,10 +34,12 @@
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
-import frc.robot.subsystems.turret.Turret;
-import frc.robot.subsystems.turret.TurretIO;
-import frc.robot.subsystems.turret.TurretIOSim;
-import frc.robot.subsystems.turret.TurretIOSpark;
+import frc.robot.subsystems.launcher.FlywheelIO;
+import frc.robot.subsystems.launcher.HoodIO;
+import frc.robot.subsystems.launcher.Launcher;
+import frc.robot.subsystems.launcher.TurretIO;
+import frc.robot.subsystems.launcher.TurretIOSim;
+import frc.robot.subsystems.launcher.TurretIOSpark;
 import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOPhotonVision;
@@ -66,7 +68,7 @@ public class Robot extends LoggedRobot {
   // Subsystems
   private Drive drive;
   private Vision vision;
-  private Turret turret;
+  private Launcher launcher;
 
   public Robot() {
     // Record metadata
@@ -110,8 +112,13 @@ public Robot() {
                 new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera),
                 new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera),
                 new VisionIOPhotonVision(cameraBackLeftName, robotToBackLeftCamera));
-        turret =
-            new Turret(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSpark());
+        launcher =
+            new Launcher(
+                drive::getPose,
+                drive::getRobotRelativeChassisSpeeds,
+                new TurretIOSpark(),
+                new FlywheelIO() {},
+                new HoodIO() {});
         break;
 
       case SIM: // Running a physics simulator
@@ -138,8 +145,13 @@ public Robot() {
                     cameraBackRightName, robotToBackRightCamera, drive::getPose),
                 new VisionIOPhotonVisionSim(
                     cameraBackLeftName, robotToBackLeftCamera, drive::getPose));
-        turret =
-            new Turret(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIOSim());
+        launcher =
+            new Launcher(
+                drive::getPose,
+                drive::getRobotRelativeChassisSpeeds,
+                new TurretIOSim(),
+                new FlywheelIO() {},
+                new HoodIO() {});
         break;
 
       case REPLAY: // Replaying a log
@@ -166,8 +178,13 @@ public Robot() {
                 new VisionIO() {},
                 new VisionIO() {},
                 new VisionIO() {});
-        turret =
-            new Turret(drive::getPose, drive::getRobotRelativeChassisSpeeds, new TurretIO() {});
+        launcher =
+            new Launcher(
+                drive::getPose,
+                drive::getRobotRelativeChassisSpeeds,
+                new TurretIO() {},
+                new FlywheelIO() {},
+                new HoodIO() {});
         break;
     }
 
@@ -183,7 +200,7 @@ public Robot() {
     SmartDashboard.putData("Field", field);
     Field.plotRegions();
 
-    turret.setDefaultCommand(Commands.run(turret::aimAtHub, turret));
+    launcher.setDefaultCommand(Commands.run(launcher::aimAtHub, launcher).withName("Aim at hub"));
   }
 
   /** This function is called periodically during all modes. */
@@ -201,6 +218,8 @@ public void robotPeriodic() {
     CommandScheduler.getInstance().run();
     SmartDashboard.putData(CommandScheduler.getInstance());
     SmartDashboard.putData(drive);
+    SmartDashboard.putData(vision);
+    SmartDashboard.putData(launcher);
 
     GameState.logValues();
 
@@ -321,7 +340,11 @@ public void bindZorroDriver(int port) {
                 () -> -zorroDriver.getRightYAxis(),
                 () -> -zorroDriver.getRightXAxis(),
                 () ->
-                    GameState.getMyHubLocation().minus(drive.getPose().getTranslation()).getAngle(),
+                    GameState.getMyHubPose()
+                        .toPose2d()
+                        .getTranslation()
+                        .minus(drive.getPose().getTranslation())
+                        .getAngle(),
                 allianceSelector::fieldRotated));
 
     // Switch to X pattern when button D is pressed
@@ -364,7 +387,11 @@ public void bindXboxDriver(int port) {
                 () -> -xboxDriver.getLeftY(),
                 () -> -xboxDriver.getLeftX(),
                 () ->
-                    GameState.getMyHubLocation().minus(drive.getPose().getTranslation()).getAngle(),
+                    GameState.getMyHubPose()
+                        .toPose2d()
+                        .getTranslation()
+                        .minus(drive.getPose().getTranslation())
+                        .getAngle(),
                 allianceSelector::fieldRotated));
 
     // Point in the direction of the commanded translation while Y button is held

src/main/java/frc/robot/subsystems/launcher/FlywheelIO.java

@@ -0,0 +1,20 @@
+package frc.robot.subsystems.launcher;
+
+import edu.wpi.first.units.measure.AngularVelocity;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface FlywheelIO {
+  @AutoLog
+  public static class FlywheelIOInputs {
+    public boolean connected = false;
+    public double velocityRadPerSec = 0.0;
+    public double appliedVolts = 0.0;
+    public double currentAmps = 0.0;
+  }
+
+  public default void updateInputs(FlywheelIOInputs inputs) {}
+
+  public default void setOpenLoop(double output) {}
+
+  public default void setVelocity(AngularVelocity angularVelocity) {}
+}

src/main/java/frc/robot/subsystems/launcher/HoodIO.java

@@ -0,0 +1,22 @@
+package frc.robot.subsystems.launcher;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.AngularVelocity;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface HoodIO {
+  @AutoLog
+  public static class HoodIOInputs {
+    public boolean connected = false;
+    public Rotation2d position = Rotation2d.kZero;
+    public double velocityRadPerSec = 0.0;
+    public double appliedVolts = 0.0;
+    public double currentAmps = 0.0;
+  }
+
+  public default void updateInputs(HoodIOInputs inputs) {}
+
+  public default void setOpenLoop(double output) {}
+
+  public default void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) {}
+}