Robot sim

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

@@ -579,7 +579,7 @@ public class IntakePivotConstants {
     public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(10);
     public static final AngularAcceleration ACCELERATION = RadiansPerSecondPerSecond.of(100);
     public static final Velocity<AngularAccelerationUnit> JERK =
-        RadiansPerSecondPerSecond.per(Second).of(0);
+        RadiansPerSecondPerSecond.per(Second).of(0.1);
 
     private static final double ROTOR_TO_SENSOR = (50.0 / 1.0);
     private static final double SENSOR_TO_MECHANISM = 1.0;
@@ -598,7 +598,9 @@ public class IntakePivotConstants {
 
     // Positional PID
     public static final Slot0Configs SLOT_0_CONFIG =
-        new Slot0Configs().withKP(10.0).withKI(2.0).withKD(8).withKS(0.07).withKV(0.1);
+        new Slot0Configs().withKP(100.0).withKI(0.0).withKD(0).withKS(0.07).withKV(0.1);
+
+    //                              ^^^ CHANGE
 
     public static TalonFXConfiguration getFXConfig() {
       TalonFXConfiguration config = new TalonFXConfiguration();

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

@@ -24,6 +24,7 @@
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.lib.Rebuilt2026.FuelSim;
@@ -43,9 +44,9 @@
  */
 public class Robot extends LoggedRobot {
   private Command autonomousCommand;
-  private RobotContainer robotContainer;
+  public static RobotContainer robotContainer;
   private Alert lowBatteryAlert = new Alert("The robot is low on battery!", AlertType.kWarning);
-  public FuelSim fuelSim = new FuelSim();
+  public static FuelSim fuelSim = new FuelSim();
 
   public Robot() {
     CanBridge.runTCP();
@@ -198,6 +199,16 @@ public void simulationInit() {
         robotContainer.drive
             ::getChassisSpeeds); // Supplier<ChassisSpeeds> of field-centric chassis speed
 
+    fuelSim.registerIntake(
+        0.4,
+        0.5,
+        -0.4,
+        0.4,
+        () -> robotContainer.intake.canIntake(),
+        () -> {
+          robotContainer.intake.simBalls++;
+        });
+
     fuelSim.start(); // enables the simulation to run (updateSim must still be called periodically)
 
     fuelSim.enableAirResistance(); // an additional drag force will be applied to fuel in physics
@@ -210,5 +221,7 @@ public void simulationPeriodic() {
     fuelSim.updateSim();
 
     Logger.recordOutput("Zero Pose", new Pose3d());
+
+    SmartDashboard.putNumber("Sim Balls", (double) robotContainer.intake.simBalls);
   }
 }

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

@@ -66,7 +66,7 @@ public class RobotContainer {
   public final Drive drive;
   private final Hopper hopper;
   private final Shooter shooter;
-  private final Intake intake;
+  public final Intake intake;
   private final BallCounter ballCounter;
   private final Vision vision;
 
@@ -314,6 +314,17 @@ private void configureButtonBindings() {
         .x()
         .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper));
 
+    controller.rightTrigger().onTrue(Commands.runOnce(() -> shooter.simShoot()));
+
+    controller
+        .leftTrigger()
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  Robot.fuelSim.clearFuel();
+                  Robot.fuelSim.spawnStartingFuel();
+                  intake.simBalls = 0;
+                }));
     controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1)));
     controller.leftBumper().onTrue(intake.intake());
     controller.rightBumper().onTrue(intake.stowAndStopRollers());

src/main/java/frc/robot/subsystems/Intake.java

@@ -1,7 +1,9 @@
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.Units.Degree;
 import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
+import static edu.wpi.first.units.Units.Volts;
 
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -21,11 +23,16 @@
 public class Intake extends SubsystemBase {
   private FlywheelMechanism _rollerIO;
   private RotaryMechanism _pivotIO;
+  double velocity;
+  double pivotAngle;
   public double desiredAngle;
+  public int simBalls;
 
   public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) {
     _rollerIO = rollerIO;
     _pivotIO = pivotIO;
+
+    simBalls = 0;
   }
 
   // Velocity of Rollers
@@ -70,6 +77,12 @@ public boolean isIntendedAngle() {
         <= IntakePivotConstants.TOLERANCE.magnitude();
   }
 
+  public boolean canIntake() {
+    return _pivotIO.getPosition().in(Degree) > (IntakePivotConstants.MAX_ANGLE.in(Degree) - 10)
+        && simBalls < 45
+        && simBalls >= 0;
+  }
+
   public Command stowAndStopRollers() {
     return Commands.sequence(
         Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)),
@@ -89,6 +102,9 @@ private Command setStowAngle(Angle stowAngle) {
 
   @Override
   public void periodic() {
+    if (_pivotIO.getPosition().in(Degree) < IntakePivotConstants.MAX_ANGLE.in(Degree))
+      _pivotIO.runVoltage(Volts.of(0.25));
+
     _pivotIO.periodic();
     Logger.recordOutput(
         "3DField/1_Intake",
@@ -101,6 +117,6 @@ public void periodic() {
             new Translation3d(Math.sin(_pivotIO.getPosition().in(Radians) * 0.1055), 0, 0),
             new Rotation3d(0, 0, 0)));
 
-    // _pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); --- Tests the pivot
+    // _pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); //--- Tests the pivot
   }
 }

src/main/java/frc/robot/subsystems/Shooter.java

@@ -1,11 +1,13 @@
 package frc.robot.subsystems;
 
 import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.Radians;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 import static edu.wpi.first.units.Units.Volts;
 
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.units.Units;
@@ -21,6 +23,7 @@
 import frc.robot.Constants.FeederConstants;
 import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.ShooterConstants;
+import frc.robot.Robot;
 import org.littletonrobotics.junction.Logger;
 
 public class Shooter extends SubsystemBase {
@@ -126,8 +129,47 @@ public Command shoot(double velocity) {
         Commands.runOnce(() -> setFlywheelVelocity(0)));
   }
 
+  public void simShoot() {
+    if (Robot.robotContainer.intake.simBalls <= 0) return;
+
+    double flywheelSpeed = 6;
+    Translation2d robotPose2d = Robot.robotContainer.drive.getPose().getTranslation();
+    double Yaw = Robot.robotContainer.drive.getPose().getRotation().getRadians();
+    Pose3d robotPose3d =
+        new Pose3d(
+            new Translation3d(robotPose2d.getX(), robotPose2d.getY(), 0),
+            new Rotation3d(robotPose2d.getAngle()));
+    Pose3d shooterPose3d =
+        new Pose3d(
+            new Translation3d(-0.0075, 0.0, 0.523),
+            new Rotation3d(0, _hood.getPosition().in(Radians), 0));
+
+    double V_xy =
+        Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians)))
+            * flywheelSpeed;
+
+    Robot.fuelSim.spawnFuel(
+        robotPose3d
+            .plus(
+                new Transform3d(
+                    shooterPose3d.getX(),
+                    shooterPose3d.getY(),
+                    shooterPose3d.getZ(),
+                    new Rotation3d(0, 0, 0)))
+            .getTranslation(),
+        new Translation3d(
+            V_xy * Math.cos(Yaw),
+            V_xy * Math.sin(Yaw),
+            Math.sin(Math.PI / 2 - (_hood.getPosition().in(Radians) + Degrees.of(12).in(Radians)))
+                * flywheelSpeed));
+    Robot.robotContainer.intake.simBalls--;
+  }
+
   public void periodic() {
     _hood.periodic();
+    // _feeder.periodic();
+    // _flywheel.periodic();
+
     double pitch =
         Math.toRadians(
             Math.abs(Math.sin(Timer.getFPGATimestamp()) * 45)); // Placeholder for position