projectile sim

AdvKit2024/src/main/java/frc/robot/Constants.java

@@ -318,7 +318,8 @@ public static class ShooterConstants {
     public static boolean HAS_STOPPED_REVVING = false;
     public static boolean IS_AMP = false;
 
-    public static final double ShooterSpeed = 28.06308713961776; // in ft/s
+    public static final LinearVelocity ShooterSpeed =
+        FeetPerSecond.of(28.06308713961776); // in ft/s
 
     public static final AngularVelocity SPK_TOP_RPM = RPM.of(3000);
     public static final AngularVelocity SPK_BOTTOM_RPM = RPM.of(5000);

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

@@ -16,7 +16,7 @@
 import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import org.ironmaple.simulation.SimulatedArena;
+import org.ironmaple.simulation.seasonspecific.crescendo2024.Arena2024Crescendo;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -160,7 +160,7 @@ public void simulationInit() {}
   /** This function is called periodically whilst in simulation. */
   @Override
   public void simulationPeriodic() {
-    SimulatedArena.getInstance().simulationPeriodic();
+    Arena2024Crescendo.getInstance().simulationPeriodic();
     robotContainer.displaySimFieldToAdvantageScope();
   }
 }

AdvKit2024/src/main/java/frc/robot/RobotContainer.java

@@ -34,7 +34,6 @@
 import frc.robot.subsystems.drive.module.ModuleIOTalonFXReal;
 import frc.robot.subsystems.drive.module.ModuleIOTalonFXSim;
 import frc.robot.subsystems.index.Index;
-import frc.robot.subsystems.index.Index.IndexState;
 import frc.robot.subsystems.index.IndexIO;
 import frc.robot.subsystems.index.IndexIOSim;
 import frc.robot.subsystems.index.IndexIOSparkFlex;
@@ -57,8 +56,8 @@
 import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOLimelight;
 import frc.robot.subsystems.vision.VisionIOPhotonVisionSim;
-import org.ironmaple.simulation.SimulatedArena;
 import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
+import org.ironmaple.simulation.seasonspecific.crescendo2024.Arena2024Crescendo;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -111,7 +110,7 @@ public RobotContainer() {
         // Sim robot, instantiate physics sim IO implementations
         driveSimulation =
             new SwerveDriveSimulation(Drive.MAPLE_SIM_CONFIG, new Pose2d(3, 1.5, new Rotation2d()));
-        SimulatedArena.getInstance().addDriveTrainSimulation(driveSimulation);
+        Arena2024Crescendo.getInstance().addDriveTrainSimulation(driveSimulation);
         drive =
             new Drive(
                 new GyroIOSim(driveSimulation.getGyroSimulation()),
@@ -130,7 +129,14 @@ public RobotContainer() {
             new Intake(
                 new IntakeIOSim(driveSimulation, () -> shooter.getState() != ShooterState.IDLE));
         pivot = new Pivot(new PivotIOSim());
-        index = new Index(new IndexIOSim());
+        index =
+            new Index(
+                new IndexIOSim(
+                    intake.noteInIntake,
+                    driveSimulation::getSimulatedDriveTrainPose,
+                    drive::getChassisSpeeds,
+                    pivot::getPivotAngle,
+                    Arena2024Crescendo.getInstance()));
         break;
 
       default:
@@ -203,29 +209,34 @@ private void configureButtonBindings() {
     // Center in-place on apriltag target
     controller.rightBumper().whileTrue(DriveCommands.centerOnTarget(drive, vision));
 
-    // intake mechanism
+    // run intake mechanism while held
     controller
         .rightTrigger()
         .whileTrue(
             Commands.parallel(
-                shooter.setState(ShooterState.IDLE),
-                pivot.setState(PivotState.INTAKE),
-                intake.setState(IntakeState.INTAKE),
-                index.setState(IndexState.INTAKE)))
+                shooter.setVelocityState(ShooterState.IDLE),
+                pivot.setPositionState(PivotState.INTAKE),
+                intake.setVelocityState(IntakeState.INTAKE),
+                index.setVelocityState(IndexIO.IndexState.INTAKE)))
         .onFalse(
-            Commands.parallel(intake.setState(IntakeState.IDLE), index.setState(IndexState.IDLE)));
+            Commands.parallel(
+                intake.setVelocityState(IntakeState.IDLE),
+                index.setVelocityState(IndexIO.IndexState.IDLE)));
+    // shut off intake mechanism when note enters intake
     intake.noteInIntake.onTrue(
-        Commands.parallel(intake.setState(IntakeState.IDLE), index.setState(IndexState.IDLE)));
+        Commands.parallel(
+            intake.setVelocityState(IntakeState.IDLE),
+            index.setVelocityState(IndexIO.IndexState.IDLE)));
 
     // manual override pivot angle
 
     // idle shooter
-    controller.y().onTrue(shooter.setState(ShooterState.IDLE));
+    controller.y().onTrue(shooter.setVelocityState(ShooterState.IDLE));
 
-    // pivot positions
-    controller.a().onTrue(pivot.setState(PivotState.SUBWOOFER));
-    controller.leftBumper().onTrue(pivot.setState(PivotState.AMP));
-    controller.x().onTrue(pivot.setState(PivotState.SHUTTLE));
+    // move to preset pivot positions
+    controller.a().onTrue(pivot.setPositionState(PivotState.SUBWOOFER));
+    controller.leftBumper().onTrue(pivot.setPositionState(PivotState.AMP));
+    controller.x().onTrue(pivot.setPositionState(PivotState.SHUTTLE));
 
     controller
         .start()
@@ -237,15 +248,15 @@ private void configureButtonBindings() {
     // hold to rev shooter, let go to shoot
     controller
         .leftTrigger()
-        .onTrue(shooter.setState(ShooterState.SPEAKER))
+        .onTrue(shooter.setVelocityState(ShooterState.SPEAKER))
         .onFalse(
             index
-                .setState(IndexState.SHOOT)
-                .andThen(Commands.waitSeconds(2.0))
+                .setVelocityState(IndexIO.IndexState.SHOOT)
+                .andThen(Commands.waitSeconds(1.0)) // account for delays in shooting
                 .andThen(
                     index
-                        .setState(IndexState.IDLE)
-                        .alongWith(shooter.setState(ShooterState.IDLE))));
+                        .setVelocityState(IndexIO.IndexState.IDLE)
+                        .alongWith(shooter.setVelocityState(ShooterState.IDLE))));
 
     // controller.povLeft()
     // controller.povRight()
@@ -263,8 +274,9 @@ public Command getAutonomousCommand() {
   public void resetSimulationField() {
     if (Constants.currentMode != Constants.Mode.SIM) return;
 
-    // driveSimulation.setSimulationWorldPose(new Pose2d(3, 1.5, new Rotation2d()));
-    SimulatedArena.getInstance().resetFieldForAuto();
+    driveSimulation.setSimulationWorldPose(new Pose2d(3, 1.5, new Rotation2d()));
+    System.out.println("Resetting simulation field");
+    Arena2024Crescendo.getInstance().resetFieldForAuto();
   }
 
   public void displaySimFieldToAdvantageScope() {
@@ -273,6 +285,6 @@ public void displaySimFieldToAdvantageScope() {
     Logger.recordOutput(
         "FieldSimulation/RobotPosition", driveSimulation.getSimulatedDriveTrainPose());
     Logger.recordOutput(
-        "FieldSimulation/Notes", SimulatedArena.getInstance().getGamePiecesArrayByType("Pose"));
+        "FieldSimulation/Notes", Arena2024Crescendo.getInstance().getGamePiecesArrayByType("Note"));
   }
 }

AdvKit2024/src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -325,7 +325,7 @@ private SwerveModulePosition[] getModulePositions() {
 
   /** Returns the measured chassis speeds of the robot. */
   @AutoLogOutput(key = "SwerveChassisSpeeds/Measured")
-  private ChassisSpeeds getChassisSpeeds() {
+  public ChassisSpeeds getChassisSpeeds() {
     return kinematics.toChassisSpeeds(getModuleStates());
   }
 

AdvKit2024/src/main/java/frc/robot/subsystems/index/Index.java

@@ -1,8 +1,7 @@
 package frc.robot.subsystems.index;
 
 import static edu.wpi.first.units.Units.Volts;
-import static frc.robot.Constants.IndexConstants.INDEX_AMP_PCT;
-import static frc.robot.Constants.IndexConstants.INDEX_RUN_PCT;
+import static frc.robot.Constants.IndexConstants.*;
 
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
@@ -11,19 +10,9 @@
 import org.littletonrobotics.junction.Logger;
 
 public class Index extends SubsystemBase {
-  public static enum IndexState {
-    IDLE,
-    SHOOT,
-    INTAKE,
-    EXTAKE,
-    AMP,
-  }
-
   private final IndexIO io;
   private final IndexIOInputsAutoLogged inputs = new IndexIOInputsAutoLogged();
 
-  private IndexState state = IndexState.IDLE;
-
   Alert indexMotorDisconnected = new Alert("Disconnected Index Motor.", AlertType.kError);
 
   public Index(IndexIO io) {
@@ -39,14 +28,14 @@ public void periodic() {
     indexMotorDisconnected.set(!inputs.indexMotorConnected);
   }
 
-  public Command setState(IndexState state) {
-    return runOnce(() -> this.state = state);
+  public Command setVelocityState(IndexIO.IndexState state) {
+    return runOnce(() -> io.setIndexState(state));
   }
 
   private Command manageOutput() {
     return run(
         () -> {
-          switch (state) {
+          switch (inputs.indexState) {
             case IDLE:
               io.setOutputVolts(Volts.of(0.0));
               break;
@@ -62,6 +51,9 @@ private Command manageOutput() {
             case AMP:
               io.setOutputVolts(Volts.of(INDEX_AMP_PCT * 12.0));
               break;
+            default:
+              setVelocityState(IndexIO.IndexState.IDLE);
+              break;
           }
         });
   }

AdvKit2024/src/main/java/frc/robot/subsystems/index/IndexIO.java

@@ -11,9 +11,20 @@ public static class IndexIOInputs {
     public double indexMotorVelocityRadPerSec = 0.0;
     public double indexMotorAppliedVolts = 0.0;
     public double indexMotorCurrentAmps = 0.0;
+    public IndexState indexState = IndexState.IDLE;
+  }
+
+  enum IndexState {
+    IDLE,
+    SHOOT,
+    INTAKE,
+    EXTAKE,
+    AMP,
   }
 
   public default void updateInputs(IndexIOInputs inputs) {}
 
   public default void setOutputVolts(Voltage volts) {}
+
+  public default void setIndexState(IndexState state) {}
 }

AdvKit2024/src/main/java/frc/robot/subsystems/index/IndexIOSim.java

@@ -1,12 +1,25 @@
 package frc.robot.subsystems.index;
 
+import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.Volts;
+import static frc.robot.subsystems.index.IndexIO.IndexState.*;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+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.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import java.util.function.Supplier;
+import org.ironmaple.simulation.SimulatedArena;
+import org.ironmaple.simulation.seasonspecific.crescendo2024.NoteOnFly;
+import org.littletonrobotics.junction.Logger;
 
 public class IndexIOSim implements IndexIO {
   DCMotorSim indexMotorSim;
@@ -15,10 +28,34 @@ public class IndexIOSim implements IndexIO {
 
   private Voltage voltsToApply = Volts.of(0.0);
 
-  public IndexIOSim() {
+  Trigger noteInIntake;
+
+  private NoteOnFly noteOnFly;
+
+  Supplier<Pose2d> robotSimulationWorldPose;
+  Supplier<ChassisSpeeds> chassisSpeedsFieldRelative;
+  double velocityRPM;
+  Supplier<Angle> pivotAngle;
+  SimulatedArena simulatedArena;
+
+  private IndexState state = IndexState.IDLE;
+
+  public IndexIOSim(
+      Trigger noteInIntake,
+      Supplier<Pose2d> robotSimulationWorldPose,
+      Supplier<ChassisSpeeds> chassisSpeedsFieldRelative,
+      Supplier<Angle> pivotAngle,
+      SimulatedArena simulatedArena) {
     indexMotorSim =
         new DCMotorSim(
             LinearSystemId.createDCMotorSystem(INDEX_GEARBOX, 0.001, 1.0), INDEX_GEARBOX);
+
+    this.noteInIntake = noteInIntake;
+
+    this.robotSimulationWorldPose = robotSimulationWorldPose;
+    this.chassisSpeedsFieldRelative = chassisSpeedsFieldRelative;
+    this.pivotAngle = pivotAngle;
+    this.simulatedArena = simulatedArena;
   }
 
   @Override
@@ -32,10 +69,64 @@ public void updateInputs(IndexIOInputs inputs) {
     inputs.indexMotorVelocityRadPerSec = indexMotorSim.getAngularVelocityRadPerSec();
     inputs.indexMotorAppliedVolts = voltsToApply.in(Volts);
     inputs.indexMotorCurrentAmps = Math.abs(indexMotorSim.getCurrentDrawAmps());
+    inputs.indexState = state;
+
+    if ((state == SHOOT || state == EXTAKE || state == AMP) && noteInIntake.getAsBoolean()) {
+      launchNoteWithTrajectory();
+    }
   }
 
   @Override
   public void setOutputVolts(Voltage volts) {
     voltsToApply = volts;
   }
+
+  public void launchNoteWithTrajectory() {
+    if (state == SHOOT) velocityRPM = 6000;
+    else if (state == EXTAKE) velocityRPM = 2000;
+    else velocityRPM = -500;
+    noteOnFly =
+        new NoteOnFly(
+            // Specify the position of the chassis when the note is launched
+            robotSimulationWorldPose.get().getTranslation(),
+            // Specify the translation of the shooter from the robot center (in the shooter’s
+            // reference frame)
+            new Translation2d(0.2, 0),
+            // Specify the field-relative speed of the chassis, adding it to the initial velocity of
+            // the projectile
+            chassisSpeedsFieldRelative.get(),
+            // The shooter facing direction is the same as the robot’s facing direction
+            robotSimulationWorldPose.get().getRotation().plus(Rotation2d.k180deg),
+            // Initial height of the flying note
+            0.7,
+            // The launch speed is proportional to the RPM; assumed to be 16 meters/second at 6000
+            // RPM
+            velocityRPM / 6000 * 16,
+            // The angle at which the note is launched
+            pivotAngle.get().in(Radians));
+    simulatedArena.addGamePieceProjectile(noteOnFly);
+    noteOnFly.enableBecomeNoteOnFieldAfterTouchGround();
+    noteOnFly
+        // Configure callbacks to visualize the flight trajectory of the projectile
+        .withProjectileTrajectoryDisplayCallBack(
+        // Callback for when the note will eventually hit the target (if configured)
+        (pose3ds) -> {
+          Logger.recordOutput("Index/NoteProjectileSuccessfulShot", pose3ds.toArray(Pose3d[]::new));
+          System.out.println("asdkjajbdhsadg");
+        },
+        // Callback for when the note will eventually miss the target, or if no target is configured
+        (pose3ds) -> {
+          Logger.recordOutput(
+              "Index/NoteProjectileUnsuccessfulShot", pose3ds.toArray(Pose3d[]::new));
+          System.out.println("asdsadsada");
+        });
+    if (noteInIntake.getAsBoolean()) {
+      noteOnFly.launch();
+    }
+  }
+
+  @Override
+  public void setIndexState(IndexState state) {
+    this.state = state;
+  }
 }

AdvKit2024/src/main/java/frc/robot/subsystems/index/IndexIOSparkFlex.java

@@ -23,6 +23,8 @@ public class IndexIOSparkFlex implements IndexIO {
 
   Debouncer indexMotorConnectedDebounce = new Debouncer(0.5);
 
+  private IndexState state = IndexState.IDLE;
+
   public IndexIOSparkFlex() {
     SparkFlexConfig config = new SparkFlexConfig();
     config
@@ -56,10 +58,16 @@ public void updateInputs(IndexIOInputs inputs) {
         indexMotor::getOutputCurrent,
         current -> inputs.indexMotorCurrentAmps = current);
     inputs.indexMotorConnected = indexMotorConnectedDebounce.calculate(!sparkStickyFault);
+    inputs.indexState = state;
   }
 
   @Override
   public void setOutputVolts(Voltage volts) {
     indexMotor.setVoltage(volts);
   }
+
+  @Override
+  public void setIndexState(IndexState state) {
+    this.state = state;
+  }
 }