2 / 16 / 26 added fuel sim

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

@@ -13,12 +13,20 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.*;
+
 import com.ctre.phoenix6.swerve.SwerveModuleConstants;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.generated.TunerConstants;
+import frc.robot.util.FuelSim;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -35,6 +43,7 @@
 public class Robot extends LoggedRobot {
   private Command autonomousCommand;
   private RobotContainer robotContainer;
+  public FuelSim fuelSim = new FuelSim(); // creates a new fuelSim of FuelSim
 
   public Robot() {
     // Record metadata
@@ -171,9 +180,29 @@ public void testPeriodic() {}
 
   /** This function is called once when the robot is first started up. */
   @Override
-  public void simulationInit() {}
+  public void simulationInit() {
+    fuelSim.spawnStartingFuel(); // spawns fuel in the depots and neutral zone
+
+    // Register a robot for collision with fuel
+    fuelSim.registerRobot(
+        0.8, // from left to right in meters
+        0.8, // from front to back in meters
+        Inches.of(7).in(Meters), // from floor to top of bumpers in meters
+        () -> robotContainer.drive.getPose(), // Supplier<Pose2d> of robot pose
+        robotContainer.drive
+            ::getChassisSpeeds); // Supplier<ChassisSpeeds> of field-centric chassis speed
+
+    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
+    // update step
+  }
 
   /** This function is called periodically whilst in simulation. */
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+    fuelSim.updateSim();
+
+    Logger.recordOutput("Zero Pose", new Pose3d());
+  }
 }

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

@@ -52,7 +52,7 @@
  */
 public class RobotContainer {
   // Subsystems
-  private final Drive drive;
+  public final Drive drive;
   private final Hopper hopper;
   private final Intake intake;
 

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

@@ -1,7 +1,16 @@
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Radians;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.lib.W8.mechanisms.linear.LinearMechanism;
+import frc.robot.Constants.ClimberConstants;
 
 public class Climber extends SubsystemBase {
   private LinearMechanism _io;
@@ -11,5 +20,7 @@ public Climber(LinearMechanism io) {
   }
 
   @Override
-  public void periodic() {}
+  public void periodic() {
+    Logger.recordOutput("3DField/4_Climber", new Pose3d(new Translation3d(0,0, ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)), new Rotation3d(0, 0, 0)));
+  }
 }

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

@@ -1,14 +1,23 @@
 package frc.robot.subsystems;
 
+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 org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.lib.W8.io.motor.MotorIO.PIDSlot;
 import frc.lib.W8.mechanisms.flywheel.FlywheelMechanism;
 import frc.lib.W8.mechanisms.rotary.RotaryMechanism;
 import frc.robot.Constants;
+import frc.robot.Constants.ClimberConstants;
 import frc.robot.Constants.IntakePivotConstants;
 
 public class Intake extends SubsystemBase {
@@ -25,7 +34,7 @@ public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) {
   public void setVelocity(double velocity) {
     AngularVelocity angVelo = RotationsPerSecond.of(velocity);
 
-    _rollerIO.runVelocity(angVelo, Constants.IntakeConstants.ACCELERATION, PIDSlot.SLOT_0);
+    _rollerIO.runVelocity(angVelo, Constants.IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0);
   }
 
   public AngularVelocity getVelocity() {
@@ -56,5 +65,11 @@ public boolean isIntendedAngle() {
   }
 
   @Override
-  public void periodic() {}
+  public void periodic() {
+    _pivotIO.periodic();
+    Logger.recordOutput("3DField/1_Intake", new Pose3d(new Translation3d(0.3085,0.0,0.175), new Rotation3d(0, _pivotIO.getPosition().in(Radians), 0)));
+    Logger.recordOutput("3DField/2_Hopper", new Pose3d(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));
+  }
 }

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

@@ -1,12 +1,20 @@
 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 org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -15,7 +23,8 @@
 import frc.lib.W8.mechanisms.rotary.RotaryMechanism;
 import frc.robot.Constants.FeederConstants;
 import frc.robot.Constants.FieldConstants;
-import frc.robot.Constants.ShooterConstants;
+import frc.robot.Constants.ShooterFlywheelConstants;
+import frc.robot.Constants.ShooterPivotConstants;
 
 public class Shooter extends SubsystemBase {
 
@@ -45,16 +54,16 @@ public void setFlywheelVelocity(double velocity) {
     this.desiredVelo = velocity;
     AngularVelocity angVelo = RotationsPerSecond.of(velocity);
 
-    _flywheel.runVelocity(angVelo, ShooterConstants.ACCELERATION, PIDSlot.SLOT_0);
+    _flywheel.runVelocity(angVelo, ShooterFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0);
   }
 
   public enum State {
     OFF(Units.RevolutionsPerSecond.of(0.0)),
-    IDLE(Units.RevolutionsPerSecond.of(ShooterConstants.IDLE_SPEED_RPM / 60)),
-    SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterConstants.HUB_SPEED_RPM / 60)),
-    SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterConstants.TOWER_SPEED_RPM / 60)),
-    SHOOT(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60)),
-    SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterConstants.DEFAULT_SPEED_RPM / 60));
+    IDLE(Units.RevolutionsPerSecond.of(ShooterFlywheelConstants.IDLE_SPEED_RPM / 60)),
+    SHOOT_FROM_HUB(Units.RevolutionsPerSecond.of(ShooterFlywheelConstants.HUB_SPEED_RPM / 60)),
+    SHOOT_FROM_TOWER(Units.RevolutionsPerSecond.of(ShooterFlywheelConstants.TOWER_SPEED_RPM / 60)),
+    SHOOT(Units.RevolutionsPerSecond.of(ShooterFlywheelConstants.DEFAULT_SPEED_RPM / 60)),
+    SHOOT_ON_MOVE(Units.RevolutionsPerSecond.of(ShooterFlywheelConstants.DEFAULT_SPEED_RPM / 60));
 
     private final AngularVelocity stateVelocity;
 
@@ -66,7 +75,7 @@ public enum State {
   // Checks if the flywheel is at speed and returns a boolean
   public boolean flyAtVelocity() {
     return Math.abs(desiredVelo - _flywheel.getVelocity().in(RotationsPerSecond))
-        <= ShooterConstants.FLYWHEEL_VELOCITY_TOLERANCE;
+        <= ShooterFlywheelConstants.FLYWHEEL_VELOCITY_TOLERANCE;
   }
 
   public double getHoodAngleDegrees(Translation2d robotPos) {
@@ -75,36 +84,36 @@ public double getHoodAngleDegrees(Translation2d robotPos) {
     double distance = robotPos.getDistance(FieldConstants.FIELDCENTER);
 
     double check =
-        Math.pow(ShooterConstants.EXIT_VELOCITY, 4)
-            - ShooterConstants.GRAVITY
-                * (ShooterConstants.GRAVITY * Math.pow(distance, 2)
+        Math.pow(ShooterPivotConstants.EXIT_VELOCITY, 4)
+            - ShooterPivotConstants.GRAVITY
+                * (ShooterPivotConstants.GRAVITY * Math.pow(distance, 2)
                     + 2
-                        * ShooterConstants.HEIGHT_DIFFERENCE
-                        * Math.pow(ShooterConstants.EXIT_VELOCITY, 2));
+                        * ShooterPivotConstants.HEIGHT_DIFFERENCE
+                        * Math.pow(ShooterPivotConstants.EXIT_VELOCITY, 2));
 
     if (check < 0) {
-      return ShooterConstants.IDLE_HOOD_ANGLE; // Default angle if the shot is not possible
+      return ShooterPivotConstants.IDLE_HOOD_ANGLE; // Default angle if the shot is not possible
     }
     return Math.toDegrees(
         Math.atan(
-            (ShooterConstants.EXIT_VELOCITY * ShooterConstants.EXIT_VELOCITY + Math.sqrt(check))
-                / (ShooterConstants.GRAVITY * distance)));
+            (ShooterPivotConstants.EXIT_VELOCITY * ShooterPivotConstants.EXIT_VELOCITY + Math.sqrt(check))
+                / (ShooterPivotConstants.GRAVITY * distance)));
   }
 
   // Sets hood angle
   public void setHoodAngle(double angleDegrees) {
     hoodAngle = angleDegrees;
     _hood.runPosition(
         Angle.ofBaseUnits(angleDegrees, Degrees),
-        ShooterConstants.HOOD_VELOCITY,
-        ShooterConstants.HOOD_ACCELERATION,
-        ShooterConstants.HOOD_JERK,
+        ShooterPivotConstants.HOOD_VELOCITY,
+        ShooterPivotConstants.HOOD_ACCELERATION,
+        ShooterPivotConstants.HOOD_JERK,
         PIDSlot.SLOT_0);
   }
 
   // Checks if hood is at angle
   public boolean hoodAtAngle() {
-    return Math.abs(hoodAngle - _hood.getPosition().in(Degrees)) < ShooterConstants.HOOD_TOLERANCE;
+    return Math.abs(hoodAngle - _hood.getPosition().in(Degrees)) < ShooterPivotConstants.HOOD_TOLERANCE;
   }
 
   public Command shoot(double velocity) {
@@ -121,5 +130,10 @@ public Command shoot(double velocity) {
   }
 
   @Override
-  public void periodic() {}
+  public void periodic() {
+    _hood.periodic();
+    Logger.recordOutput("3DField/3_Hood", new Pose3d(new Translation3d(-0.0075,0.0,0.523), new Rotation3d(0, _hood.getPosition().in(Radians), 0)));
+
+    _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25));
+  }
 }

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

@@ -299,7 +299,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());
   }