From 423a4672b2448e34b6f7145fad95508a9344bf5e Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sat, 21 Feb 2026 16:29:55 -0500 Subject: [PATCH 1/2] The Shooter and Climber have improper motor configs, but intake works and can be used for testing HUZZAH! --- src/main/java/frc/robot/Constants.java | 10 +- src/main/java/frc/robot/Robot.java | 32 +- src/main/java/frc/robot/RobotContainer.java | 65 +- .../java/frc/robot/subsystems/Climber.java | 20 +- .../java/frc/robot/subsystems/Intake.java | 27 +- .../java/frc/robot/subsystems/Shooter.java | 19 +- .../frc/robot/subsystems/drive/Drive.java | 2 +- src/main/java/frc/robot/util/FuelSim.java | 861 ++++++++++++++++++ 8 files changed, 984 insertions(+), 52 deletions(-) create mode 100644 src/main/java/frc/robot/util/FuelSim.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 91e0174..7dddebb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -340,8 +340,8 @@ public class ShooterRotaryConstants { public static final Translation3d OFFSET = Translation3d.kZero; - public static final Angle MIN_ANGLE = Degrees.of(-130.0); - public static final Angle MAX_ANGLE = Rotations.of(.5); + public static final Angle MIN_ANGLE = Degrees.of(0.0); + public static final Angle MAX_ANGLE = Rotations.of(45.0); public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance ARM_LENGTH = Meters.of(1.0); @@ -432,7 +432,7 @@ public static CANcoderConfiguration getCANcoderConfig(boolean sim) { public static final int CANDLE_ID = 50; - public class IntakeConstants { + public class IntakeFlywheelConstants { // Constants for Intake public static final Angle MIN_ANGLE = Rotations.of(0.0); public static final Angle MAX_ANGLE = Rotations.of(1); @@ -578,8 +578,8 @@ public class IntakePivotConstants { private static final double ROTOR_TO_SENSOR = (50.0 / 1.0); private static final double SENSOR_TO_MECHANISM = 1.0; - public static final Angle MIN_ANGLE = Degrees.of(-90.0); - public static final Angle MAX_ANGLE = Degrees.of(90.0); + public static final Angle MIN_ANGLE = Degrees.of(0.0); + public static final Angle MAX_ANGLE = Degrees.of(130.0); public static final Angle STARTING_ANGLE = Radians.zero(); public static final Distance ARM_LENGTH = Foot.one(); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index efea88d..9789541 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -16,9 +16,16 @@ 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.Pose3d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.lib.Rebuilt2026.FuelSim; import frc.robot.generated.TunerConstants; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; + import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -39,6 +46,7 @@ public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; + public FuelSim fuelSim = new FuelSim(); public Robot() { CanBridge.runTCP(); @@ -176,9 +184,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 of robot pose + robotContainer.drive + ::getChassisSpeeds); // Supplier 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()); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5657871..e3c65d3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,8 +36,9 @@ import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim; import frc.robot.Constants.ClimberConstants; import frc.robot.Constants.HopperConstants; -import frc.robot.Constants.IntakeConstants; -import frc.robot.Constants.IntakeConstants.VisionConstants; +import frc.robot.Constants.IntakeFlywheelConstants; +import frc.robot.Constants.IntakePivotConstants; +import frc.robot.Constants.IntakeFlywheelConstants.VisionConstants; import frc.robot.Constants.Ports; import frc.robot.Constants.ShooterFlywheelConstants; import frc.robot.Constants.ShooterRotaryConstants; @@ -56,6 +57,8 @@ import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import static edu.wpi.first.units.Units.Degrees; + import java.util.Optional; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -71,7 +74,7 @@ public class RobotContainer { // Subsystems - private final Drive drive; + public final Drive drive; private final Hopper hopper; private final Shooter shooter; private final Climber climber; @@ -122,8 +125,8 @@ public RobotContainer() { Ports.ShooterRoller)), new RotaryMechanismReal( new MotorIOTalonFX( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), + ShooterRotaryConstants.NAME, + ShooterRotaryConstants.getFXConfig(), Ports.ShooterRoller), Constants.ShooterRotaryConstants.CONSTANTS, java.util.Optional.empty())); @@ -140,20 +143,16 @@ public RobotContainer() { new Intake( new FlywheelMechanismReal( new MotorIOTalonFX( - IntakeConstants.MOTOR_NAME, - IntakeConstants.getFXConfig(), + IntakeFlywheelConstants.MOTOR_NAME, + IntakeFlywheelConstants.getFXConfig(), Ports.IntakeRoller)), new RotaryMechanismReal( new MotorIOTalonFX( - IntakeConstants.MOTOR_NAME, - IntakeConstants.getFXConfig(), + IntakePivotConstants.NAME, + IntakePivotConstants.getFXConfig(), Ports.IntakeRoller), - IntakeConstants.CONSTANTS, - Optional.of( - new AbsoluteEncoderIOCANCoderSim( - Ports.IntakeRoller, - IntakeConstants.MOTOR_NAME + " Encoder", - IntakeConstants.getCANcoderConfig(false))))); + IntakePivotConstants.CONSTANTS, + Optional.empty())); vision = new Vision( new VisionIOPhotonVision( @@ -203,8 +202,8 @@ public RobotContainer() { ShooterFlywheelConstants.TOLERANCE), new RotaryMechanismSim( new MotorIOTalonFXSim( - ShooterFlywheelConstants.NAME, - ShooterFlywheelConstants.getFXConfig(), + ShooterRotaryConstants.NAME, + ShooterRotaryConstants.getFXConfig(), Ports.ShooterRoller), ShooterRotaryConstants.DCMOTOR, ShooterRotaryConstants.MOI, @@ -227,26 +226,22 @@ public RobotContainer() { new Intake( new FlywheelMechanismSim( new MotorIOTalonFXSim( - IntakeConstants.MOTOR_NAME, - IntakeConstants.getFXConfig(), + IntakeFlywheelConstants.MOTOR_NAME, + IntakeFlywheelConstants.getFXConfig(), Ports.IntakeRoller), - IntakeConstants.DCMOTOR, - IntakeConstants.MOI, - IntakeConstants.TOLERANCE), + IntakeFlywheelConstants.DCMOTOR, + IntakeFlywheelConstants.MOI, + IntakeFlywheelConstants.TOLERANCE), new RotaryMechanismSim( new MotorIOTalonFXSim( - IntakeConstants.MOTOR_NAME, - IntakeConstants.getFXConfig(), + IntakePivotConstants.NAME, + IntakePivotConstants.getFXConfig(), Ports.IntakeRoller), - IntakeConstants.DCMOTOR, - IntakeConstants.MOI, + IntakePivotConstants.DCMOTOR, + IntakePivotConstants.MOI, false, - IntakeConstants.CONSTANTS, - Optional.of( - new AbsoluteEncoderIOCANCoderSim( - Ports.IntakeRoller, - IntakeConstants.MOTOR_NAME + " Encoder", - IntakeConstants.getCANcoderConfig(false))))); + IntakePivotConstants.CONSTANTS, + Optional.empty())); vision = new Vision( new VisionIOPhotonVisionSim( @@ -283,7 +278,7 @@ public RobotContainer() { intake = new Intake( new FlywheelMechanism() {}, - new RotaryMechanism(IntakeConstants.MOTOR_NAME, IntakeConstants.CONSTANTS) {}); + new RotaryMechanism(IntakePivotConstants.NAME, IntakePivotConstants.CONSTANTS) {}); vision = new Vision(null); break; } @@ -353,6 +348,10 @@ private void configureButtonBindings() { controller .x() .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); + + controller + .y() + .onTrue(Commands.runOnce(() -> intake.setVelocity(1))); } /** diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index cbfdf15..76d22aa 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,10 +2,17 @@ import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; +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.Units; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; @@ -17,7 +24,7 @@ public class Climber extends SubsystemBase { Distance goalDistance; public Climber(LinearMechanism io) { - io = _io; + _io = io; } public void Position(double position) { @@ -55,7 +62,16 @@ private State(LinearVelocity velocity) { } @Override - public void periodic() {} + public void periodic() { + _io.periodic(); + + double z = Math.abs(Math.sin(Timer.getFPGATimestamp())*0.33); // Placeholder for position + + // The z of the Translation3D should be 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing motor configs. + Logger.recordOutput("3DField/4_Climber", new Pose3d(new Translation3d(0,0, z), new Rotation3d(0, 0, 0))); + + _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); + } public void runClimber() { runClimber(); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 8b962c7..896afd8 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,16 +1,24 @@ 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.Command; 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.IntakeConstants; +import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakePivotConstants; public class Intake extends SubsystemBase { @@ -29,7 +37,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 Command setPivotAngle(Angle pivotAngle) { @@ -37,9 +45,9 @@ public Command setPivotAngle(Angle pivotAngle) { () -> _pivotIO.runPosition( pivotAngle, - IntakeConstants.CRUISE_VELOCITY, - IntakeConstants.ACCELERATION, - IntakeConstants.JERK, + IntakeFlywheelConstants.CRUISE_VELOCITY, + IntakeFlywheelConstants.ACCELERATION, + IntakeFlywheelConstants.JERK, PIDSlot.SLOT_0)); // .withName("Go To " + setpoint.toString() + " Setpoint"); } @@ -76,6 +84,11 @@ public boolean isIntendedAngle() { <= IntakePivotConstants.TOLERANCE.magnitude(); } - @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)); --- Tests the pivot + } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 629e188..5f91890 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/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; @@ -120,6 +128,13 @@ public Command shoot(double velocity) { Commands.runOnce(() -> setFlywheelVelocity(0))); } - @Override - public void periodic() {} + public void periodic() { + _hood.periodic(); + double pitch = Math.toRadians(Math.abs(Math.sin(Timer.getFPGATimestamp())*45)); // Placeholder for position + + // The pitch of the Rotation3D should be '_hood.getPosition().in(Radians)', change after fixing motor configs. + Logger.recordOutput("3DField/3_Hood", new Pose3d(new Translation3d(-0.0075,0.0,0.523), new Rotation3d(0, pitch, 0))); + + _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index fbc3110..a905aef 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/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()); } diff --git a/src/main/java/frc/robot/util/FuelSim.java b/src/main/java/frc/robot/util/FuelSim.java new file mode 100644 index 0000000..6cf2b5e --- /dev/null +++ b/src/main/java/frc/robot/util/FuelSim.java @@ -0,0 +1,861 @@ +// https://github.com/hammerheads5000/FuelSim +package frc.robot.util; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Radians; + +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.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.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import java.util.ArrayList; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +public class FuelSim { + protected static final double PERIOD = 0.02; // sec + protected static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); // m/s^2 + // Room temperature dry air density: https://en.wikipedia.org/wiki/Density_of_air#Dry_air + protected static final double AIR_DENSITY = 1.2041; // kg/m^3 + protected static final double FIELD_COR = Math.sqrt(22 / 51.5); // coefficient of restitution with the field + protected static final double FUEL_COR = 0.5; // coefficient of restitution with another fuel + protected static final double NET_COR = 0.2; // coefficient of restitution with the net + protected static final double ROBOT_COR = 0.1; // coefficient of restitution with a robot + protected static final double FUEL_RADIUS = 0.075; + protected static final double FIELD_LENGTH = 16.51; + protected static final double FIELD_WIDTH = 8.04; + protected static final double TRENCH_WIDTH = 1.265; + protected static final double TRENCH_BLOCK_WIDTH = 0.305; + protected static final double TRENCH_HEIGHT = 0.565; + protected static final double TRENCH_BAR_HEIGHT = 0.102; + protected static final double TRENCH_BAR_WIDTH = 0.152; + protected static final double FRICTION = 0.1; // proportion of horizontal vel to lose per sec while on ground + protected static final double FUEL_MASS = 0.448 * 0.45392; // kgs + protected static final double FUEL_CROSS_AREA = Math.PI * FUEL_RADIUS * FUEL_RADIUS; + // Drag coefficient of smooth sphere: https://en.wikipedia.org/wiki/Drag_coefficient#/media/File:14ilf1l.svg + protected static final double DRAG_COF = 0.47; // dimensionless + protected static final double DRAG_FORCE_FACTOR = 0.5 * AIR_DENSITY * DRAG_COF * FUEL_CROSS_AREA; + + protected static final Translation3d[] FIELD_XZ_LINE_STARTS = { + new Translation3d(0, 0, 0), + new Translation3d(3.96, 1.57, 0), + new Translation3d(3.96, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(4.61, 1.57, 0.165), + new Translation3d(4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 5.18, 1.57, 0), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(FIELD_LENGTH - 4.61, 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(3.96, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(3.96, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + protected static final Translation3d[] FIELD_XZ_LINE_ENDS = { + new Translation3d(FIELD_LENGTH, FIELD_WIDTH, 0), + new Translation3d(4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(5.18, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + protected static class Fuel { + protected Translation3d pos; + protected Translation3d vel; + + protected Fuel(Translation3d pos, Translation3d vel) { + this.pos = pos; + this.vel = vel; + } + + protected Fuel(Translation3d pos) { + this(pos, new Translation3d()); + } + + protected void update(boolean simulateAirResistance, int subticks) { + pos = pos.plus(vel.times(PERIOD / subticks)); + if (pos.getZ() > FUEL_RADIUS) { + Translation3d Fg = GRAVITY.times(FUEL_MASS); + Translation3d Fd = new Translation3d(); + + if (simulateAirResistance) { + double speed = vel.getNorm(); + if (speed > 1e-6) { + Fd = vel.times(-DRAG_FORCE_FACTOR * speed); + } + } + + Translation3d accel = Fg.plus(Fd).div(FUEL_MASS); + vel = vel.plus(accel.times(PERIOD / subticks)); + } + if (Math.abs(vel.getZ()) < 0.05 && pos.getZ() <= FUEL_RADIUS + 0.03) { + vel = new Translation3d(vel.getX(), vel.getY(), 0); + vel = vel.times(1 - FRICTION * PERIOD / subticks); + // pos = new Translation3d(pos.getX(), pos.getY(), FUEL_RADIUS); + } + handleFieldCollisions(subticks); + } + + protected void handleXZLineCollision(Translation3d lineStart, Translation3d lineEnd) { + if (pos.getY() < lineStart.getY() || pos.getY() > lineEnd.getY()) return; // not within y range + // Convert into 2D + Translation2d start2d = new Translation2d(lineStart.getX(), lineStart.getZ()); + Translation2d end2d = new Translation2d(lineEnd.getX(), lineEnd.getZ()); + Translation2d pos2d = new Translation2d(pos.getX(), pos.getZ()); + Translation2d lineVec = end2d.minus(start2d); + + // Get closest point on line + Translation2d projected = + start2d.plus(lineVec.times(pos2d.minus(start2d).dot(lineVec) / lineVec.getSquaredNorm())); + + if (projected.getDistance(start2d) + projected.getDistance(end2d) > lineVec.getNorm()) + return; // projected point not on line + double dist = pos2d.getDistance(projected); + if (dist > FUEL_RADIUS) return; // not intersecting line + // Back into 3D + Translation3d normal = new Translation3d(-lineVec.getY(), 0, lineVec.getX()).div(lineVec.getNorm()); + + // Apply collision response + pos = pos.plus(normal.times(FUEL_RADIUS - dist)); + if (vel.dot(normal) > 0) return; // already moving away from line + vel = vel.minus(normal.times((1 + FIELD_COR) * vel.dot(normal))); + } + + protected void handleFieldCollisions(int subticks) { + // floor and bumps + for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + handleXZLineCollision(FIELD_XZ_LINE_STARTS[i], FIELD_XZ_LINE_ENDS[i]); + } + + // edges + if (pos.getX() < FUEL_RADIUS && vel.getX() < 0) { + pos = pos.plus(new Translation3d(FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } else if (pos.getX() > FIELD_LENGTH - FUEL_RADIUS && vel.getX() > 0) { + pos = pos.plus(new Translation3d(FIELD_LENGTH - FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } + + if (pos.getY() < FUEL_RADIUS && vel.getY() < 0) { + pos = pos.plus(new Translation3d(0, FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } else if (pos.getY() > FIELD_WIDTH - FUEL_RADIUS && vel.getY() > 0) { + pos = pos.plus(new Translation3d(0, FIELD_WIDTH - FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } + + // hubs + handleHubCollisions(Hub.BLUE_HUB, subticks); + handleHubCollisions(Hub.RED_HUB, subticks); + + handleTrenchCollisions(); + } + + protected void handleHubCollisions(Hub hub, int subticks) { + hub.handleHubInteraction(this, subticks); + hub.fuelCollideSide(this); + + double netCollision = hub.fuelHitNet(this); + if (netCollision != 0) { + pos = pos.plus(new Translation3d(netCollision, 0, 0)); + vel = new Translation3d(-vel.getX() * NET_COR, vel.getY() * NET_COR, vel.getZ()); + } + } + + protected void handleTrenchCollisions() { + fuelCollideRectangle( + this, + new Translation3d(3.96, TRENCH_WIDTH, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, 0), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + FIELD_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + } + + protected void addImpulse(Translation3d impulse) { + vel = vel.plus(impulse); + } + } + + protected static void handleFuelCollision(Fuel a, Fuel b) { + Translation3d normal = a.pos.minus(b.pos); + double distance = normal.getNorm(); + if (distance == 0) { + normal = new Translation3d(1, 0, 0); + distance = 1; + } + normal = normal.div(distance); + double impulse = 0.5 * (1 + FUEL_COR) * (b.vel.minus(a.vel).dot(normal)); + double intersection = FUEL_RADIUS * 2 - distance; + a.pos = a.pos.plus(normal.times(intersection / 2)); + b.pos = b.pos.minus(normal.times(intersection / 2)); + a.addImpulse(normal.times(impulse)); + b.addImpulse(normal.times(-impulse)); + } + + protected static final double CELL_SIZE = 0.25; + protected static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE); + protected static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE); + + @SuppressWarnings("unchecked") + protected final ArrayList[][] grid = new ArrayList[GRID_COLS][GRID_ROWS]; + private final ArrayList> activeCells = new ArrayList<>(); + + protected void handleFuelCollisions(ArrayList fuels) { + // Clear grid + for (ArrayList cell : activeCells) { + cell.clear(); + } + activeCells.clear(); + + // Populate grid + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + if (col >= 0 && col < GRID_COLS && row >= 0 && row < GRID_ROWS) { + grid[col][row].add(fuel); + if (grid[col][row].size() == 1) { + activeCells.add(grid[col][row]); + } + } + } + + // Check collisions + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + // Check 3x3 neighbor cells + for (int i = col - 1; i <= col + 1; i++) { + for (int j = row - 1; j <= row + 1; j++) { + if (i >= 0 && i < GRID_COLS && j >= 0 && j < GRID_ROWS) { + for (Fuel other : grid[i][j]) { + if (fuel != other && fuel.pos.getDistance(other.pos) < FUEL_RADIUS * 2) { + if (fuel.hashCode() < other.hashCode()) { + handleFuelCollision(fuel, other); + } + } + } + } + } + } + } + } + + protected ArrayList fuels = new ArrayList<>(); + protected boolean running = false; + protected boolean simulateAirResistance = false; + protected Supplier robotPoseSupplier = null; + protected Supplier robotFieldSpeedsSupplier = null; + protected double robotWidth; // size along the robot's y axis + protected double robotLength; // size along the robot's x axis + protected double bumperHeight; + protected ArrayList intakes = new ArrayList<>(); + protected int subticks = 5; + + /** + * Creates a new instance of FuelSim + * @param tableKey NetworkTable to log fuel positions to as an array of {@link Translation3d} structs. + */ + public FuelSim(String tableKey) { + // Initialize grid + for (int i = 0; i < GRID_COLS; i++) { + for (int j = 0; j < GRID_ROWS; j++) { + grid[i][j] = new ArrayList(); + } + } + + fuelPublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic(tableKey + "/Fuels", Translation3d.struct) + .publish(); + } + + /** + * Creates a new instance of FuelSim with log path "/Fuel Simulation" + */ + public FuelSim() { + this("/Fuel Simulation"); + } + + /** + * Clears the field of fuel + */ + public void clearFuel() { + fuels.clear(); + } + + /** + * Spawns fuel in the neutral zone and depots + */ + public void spawnStartingFuel() { + // Center fuel + Translation3d center = new Translation3d(FIELD_LENGTH / 2, FIELD_WIDTH / 2, FUEL_RADIUS); + for (int i = 0; i < 15; i++) { + for (int j = 0; j < 6; j++) { + fuels.add(new Fuel(center.plus(new Translation3d(0.076 + 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add(new Fuel(center.plus(new Translation3d(-0.076 - 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add(new Fuel(center.plus(new Translation3d(0.076 + 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + fuels.add(new Fuel(center.plus(new Translation3d(-0.076 - 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + } + } + + // Depots + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 4; j++) { + fuels.add(new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add(new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 - 0.076 - 0.152 * i, FUEL_RADIUS))); + fuels.add(new Fuel( + new Translation3d(FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add(new Fuel( + new Translation3d(FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 - 0.076 - 0.152 * i, FUEL_RADIUS))); + } + } + + // DEBUG: Log XZ lines + // Translation3d[][] lines = new Translation3d[FIELD_XZ_LINE_STARTS.length][2]; + // for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + // lines[i][0] = FIELD_XZ_LINE_STARTS[i]; + // lines[i][1] = FIELD_XZ_LINE_ENDS[i]; + // } + + // Logger.recordOutput("Fuel Simulation/Lines (debug)", lines); + } + + protected StructArrayPublisher fuelPublisher; + + /** + * Adds array of `Translation3d`'s to NetworkTables at tableKey + "/Fuels" + */ + public void logFuels() { + fuelPublisher.set(fuels.stream().map((fuel) -> fuel.pos).toArray(Translation3d[]::new)); + } + + /** + * Start the simulation. `updateSim` must still be called every loop + */ + public void start() { + running = true; + } + + /** + * Pause the simulation. + */ + public void stop() { + running = false; + } + + /** Enables accounting for drag force in physics step **/ + public void enableAirResistance() { + simulateAirResistance = true; + } + + /** + * Sets the number of physics iterations per loop (0.02s) + * @param subticks + */ + public void setSubticks(int subticks) { + this.subticks = subticks; + } + + /** + * Registers a robot with the fuel simulator + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param bumperHeight + * @param poseSupplier + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + double width, + double length, + double bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width; + this.robotLength = length; + this.bumperHeight = bumperHeight; + } + + /** + * Registers a robot with the fuel simulator + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param bumperHeight from the ground + * @param poseSupplier + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + Distance width, + Distance length, + Distance bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width.in(Meters); + this.robotLength = length.in(Meters); + this.bumperHeight = bumperHeight.in(Meters); + } + + /** + * To be called periodically + * Will do nothing if sim is not running + */ + public void updateSim() { + if (!running) return; + + stepSim(); + } + + /** + * Run the simulation forward 1 time step (0.02s) + */ + public void stepSim() { + for (int i = 0; i < subticks; i++) { + for (Fuel fuel : fuels) { + fuel.update(this.simulateAirResistance, this.subticks); + } + + handleFuelCollisions(fuels); + + if (robotPoseSupplier != null) { + handleRobotCollisions(fuels); + handleIntakes(fuels); + } + } + + logFuels(); + } + + /** + * Adds a fuel onto the field + * @param pos Position to spawn at + * @param vel Initial velocity vector + */ + public void spawnFuel(Translation3d pos, Translation3d vel) { + fuels.add(new Fuel(pos, vel)); + } + + /** + * Spawns a fuel onto the field with a specified launch velocity and angles, accounting for robot movement + * @param launchVelocity Initial launch velocity + * @param hoodAngle Hood angle where 0 is launching horizontally and 90 degrees is launching straight up + * @param turretYaw Robot-relative turret yaw + * @param launchHeight Height of the fuel to launch at. Make sure this is higher than your robot's bumper height, or else it will collide with your robot immediately. + * @throws IllegalStateException if robot is not registered + */ + public void launchFuel(LinearVelocity launchVelocity, Angle hoodAngle, Angle turretYaw, Distance launchHeight) { + if (robotPoseSupplier == null || robotFieldSpeedsSupplier == null) { + throw new IllegalStateException("Robot must be registered before launching fuel."); + } + + Pose3d launchPose = new Pose3d(this.robotPoseSupplier.get()) + .plus(new Transform3d(new Translation3d(Meters.zero(), Meters.zero(), launchHeight), Rotation3d.kZero)); + ChassisSpeeds fieldSpeeds = this.robotFieldSpeedsSupplier.get(); + + double horizontalVel = Math.cos(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double verticalVel = Math.sin(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double xVel = horizontalVel + * Math.cos( + turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + double yVel = horizontalVel + * Math.sin( + turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + + xVel += fieldSpeeds.vxMetersPerSecond; + yVel += fieldSpeeds.vyMetersPerSecond; + + spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); + } + + protected void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) { + Translation2d relativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) + .relativeTo(robot) + .getTranslation(); + + if (fuel.pos.getZ() > bumperHeight) return; // above bumpers + double distanceToBottom = -FUEL_RADIUS - robotLength / 2 - relativePos.getX(); + double distanceToTop = -FUEL_RADIUS - robotLength / 2 + relativePos.getX(); + double distanceToRight = -FUEL_RADIUS - robotWidth / 2 - relativePos.getY(); + double distanceToLeft = -FUEL_RADIUS - robotWidth / 2 + relativePos.getY(); + + // not inside robot + if (distanceToBottom > 0 || distanceToTop > 0 || distanceToRight > 0 || distanceToLeft > 0) return; + + Translation2d posOffset; + // find minimum distance to side and send corresponding collision response + if ((distanceToBottom >= distanceToTop + && distanceToBottom >= distanceToRight + && distanceToBottom >= distanceToLeft)) { + posOffset = new Translation2d(distanceToBottom, 0); + } else if ((distanceToTop >= distanceToBottom + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToLeft)) { + posOffset = new Translation2d(-distanceToTop, 0); + } else if ((distanceToRight >= distanceToBottom + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToLeft)) { + posOffset = new Translation2d(0, distanceToRight); + } else { + posOffset = new Translation2d(0, -distanceToLeft); + } + + posOffset = posOffset.rotateBy(robot.getRotation()); + fuel.pos = fuel.pos.plus(new Translation3d(posOffset)); + Translation2d normal = posOffset.div(posOffset.getNorm()); + if (fuel.vel.toTranslation2d().dot(normal) < 0) + fuel.addImpulse( + new Translation3d(normal.times(-fuel.vel.toTranslation2d().dot(normal) * (1 + ROBOT_COR)))); + if (robotVel.dot(normal) > 0) fuel.addImpulse(new Translation3d(normal.times(robotVel.dot(normal)))); + } + + protected void handleRobotCollisions(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + ChassisSpeeds speeds = robotFieldSpeedsSupplier.get(); + Translation2d robotVel = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + + for (Fuel fuel : fuels) { + handleRobotCollision(fuel, robot, robotVel); + } + } + + protected void handleIntakes(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + for (SimIntake intake : intakes) { + for (int i = 0; i < fuels.size(); i++) { + if (intake.shouldIntake(fuels.get(i), robot)) { + fuels.remove(i); + i--; + } + } + } + } + + protected static void fuelCollideRectangle(Fuel fuel, Translation3d start, Translation3d end) { + if (fuel.pos.getZ() > end.getZ() + FUEL_RADIUS || fuel.pos.getZ() < start.getZ() - FUEL_RADIUS) + return; // above rectangle + double distanceToLeft = start.getX() - FUEL_RADIUS - fuel.pos.getX(); + double distanceToRight = fuel.pos.getX() - end.getX() - FUEL_RADIUS; + double distanceToTop = fuel.pos.getY() - end.getY() - FUEL_RADIUS; + double distanceToBottom = start.getY() - FUEL_RADIUS - fuel.pos.getY(); + + // not inside hub + if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || distanceToBottom > 0) return; + + Translation2d collision; + // find minimum distance to side and send corresponding collision response + if (fuel.pos.getX() < start.getX() + || (distanceToLeft >= distanceToRight + && distanceToLeft >= distanceToTop + && distanceToLeft >= distanceToBottom)) { + collision = new Translation2d(distanceToLeft, 0); + } else if (fuel.pos.getX() >= end.getX() + || (distanceToRight >= distanceToLeft + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToBottom)) { + collision = new Translation2d(-distanceToRight, 0); + } else if (fuel.pos.getY() > end.getY() + || (distanceToTop >= distanceToLeft + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToBottom)) { + collision = new Translation2d(0, -distanceToTop); + } else { + collision = new Translation2d(0, distanceToBottom); + } + + if (collision.getX() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(-(1 + FIELD_COR) * fuel.vel.getX(), 0, 0)); + } else if (collision.getY() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(0, -(1 + FIELD_COR) * fuel.vel.getY(), 0)); + } + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake, Runnable intakeCallback) { + intakes.add(new SimIntake(xMin, xMax, yMin, yMax, ableToIntake, intakeCallback)); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + */ + public void registerIntake(double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake) { + registerIntake(xMin, xMax, yMin, yMax, ableToIntake, () -> {}); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake(double xMin, double xMax, double yMin, double yMax, Runnable intakeCallback) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + */ + public void registerIntake(double xMin, double xMax, double yMin, double yMax) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, () -> {}); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + Distance xMin, Distance xMax, Distance yMin, Distance yMax, BooleanSupplier ableToIntake, Runnable intakeCallback) { + registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), ableToIntake, intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + */ + public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax, BooleanSupplier ableToIntake) { + registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), ableToIntake); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax, Runnable intakeCallback) { + registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + */ + public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax) { + registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters)); + } + + public static class Hub { + public static final Hub BLUE_HUB = + new Hub(new Translation2d(4.61, FIELD_WIDTH / 2), new Translation3d(5.3, FIELD_WIDTH / 2, 0.89), 1); + public static final Hub RED_HUB = new Hub( + new Translation2d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2), + new Translation3d(FIELD_LENGTH - 5.3, FIELD_WIDTH / 2, 0.89), + -1); + + protected static final double ENTRY_HEIGHT = 1.83; + protected static final double ENTRY_RADIUS = 0.56; + + protected static final double SIDE = 1.2; + + protected static final double NET_HEIGHT_MAX = 3.057; + protected static final double NET_HEIGHT_MIN = 1.5; + protected static final double NET_OFFSET = SIDE / 2 + 0.261; + protected static final double NET_WIDTH = 1.484; + + protected final Translation2d center; + protected final Translation3d exit; + protected final int exitVelXMult; + + protected int score = 0; + + protected Hub(Translation2d center, Translation3d exit, int exitVelXMult) { + this.center = center; + this.exit = exit; + this.exitVelXMult = exitVelXMult; + } + + protected void handleHubInteraction(Fuel fuel, int subticks) { + if (didFuelScore(fuel, subticks)) { + fuel.pos = exit; + fuel.vel = getDispersalVelocity(); + score++; + } + } + + protected boolean didFuelScore(Fuel fuel, int subticks) { + return fuel.pos.toTranslation2d().getDistance(center) <= ENTRY_RADIUS + && fuel.pos.getZ() <= ENTRY_HEIGHT + && fuel.pos.minus(fuel.vel.times(PERIOD / subticks)).getZ() > ENTRY_HEIGHT; + } + + protected Translation3d getDispersalVelocity() { + return new Translation3d(exitVelXMult * (Math.random() + 0.1) * 1.5, Math.random() * 2 - 1, 0); + } + + /** + * Reset this hub's score to 0 + */ + public void resetScore() { + score = 0; + } + + /** + * Get the current count of fuel scored in this hub + * @return + */ + public int getScore() { + return score; + } + + protected void fuelCollideSide(Fuel fuel) { + fuelCollideRectangle( + fuel, + new Translation3d(center.getX() - SIDE / 2, center.getY() - SIDE / 2, 0), + new Translation3d(center.getX() + SIDE / 2, center.getY() + SIDE / 2, ENTRY_HEIGHT - 0.1)); + } + + protected double fuelHitNet(Fuel fuel) { + if (fuel.pos.getZ() > NET_HEIGHT_MAX || fuel.pos.getZ() < NET_HEIGHT_MIN) return 0; + if (fuel.pos.getY() > center.getY() + NET_WIDTH / 2 || fuel.pos.getY() < center.getY() - NET_WIDTH / 2) + return 0; + if (fuel.pos.getX() > center.getX() + NET_OFFSET * exitVelXMult) { + return Math.max(0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() - FUEL_RADIUS)); + } else { + return Math.min(0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() + FUEL_RADIUS)); + } + } + } + + protected class SimIntake { + double xMin, xMax, yMin, yMax; + BooleanSupplier ableToIntake; + Runnable callback; + + protected SimIntake( + double xMin, + double xMax, + double yMin, + double yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + this.xMin = xMin; + this.xMax = xMax; + this.yMin = yMin; + this.yMax = yMax; + this.ableToIntake = ableToIntake; + this.callback = intakeCallback; + } + + protected boolean shouldIntake(Fuel fuel, Pose2d robotPose) { + if (!ableToIntake.getAsBoolean() || fuel.pos.getZ() > bumperHeight) return false; + + Translation2d fuelRelativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) + .relativeTo(robotPose) + .getTranslation(); + + boolean result = fuelRelativePos.getX() >= xMin + && fuelRelativePos.getX() <= xMax + && fuelRelativePos.getY() >= yMin + && fuelRelativePos.getY() <= yMax; + if (result) { + callback.run(); + } + return result; + } + } +} From b8fa3afac6eb6ee3041547fb1cf8934200e73989 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Mon, 23 Feb 2026 17:50:47 -0500 Subject: [PATCH 2/2] fixing spotless issue maybe? --- src/main/java/frc/robot/Robot.java | 13 +- src/main/java/frc/robot/RobotContainer.java | 20 +- .../frc/robot/subsystems/BallCounter.java | 11 +- .../java/frc/robot/subsystems/Climber.java | 16 +- .../java/frc/robot/subsystems/Intake.java | 17 +- .../java/frc/robot/subsystems/Shooter.java | 19 +- src/main/java/frc/robot/util/FuelSim.java | 1738 +++++++++-------- 7 files changed, 956 insertions(+), 878 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9789541..5ffed28 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,19 +13,18 @@ package frc.robot; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; + +import au.grapplerobotics.CanBridge; 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.Pose3d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.lib.Rebuilt2026.FuelSim; import frc.robot.generated.TunerConstants; - -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; - import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -39,10 +38,6 @@ * the package after creating this project, you must also update the build.gradle file in the * project. */ - import au.grapplerobotics.CanBridge; - - - public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e3c65d3..1895688 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.lib.W8.io.absoluteencoder.AbsoluteEncoderIOCANCoderSim; import frc.lib.W8.io.motor.*; import frc.lib.W8.io.vision.VisionIOPhotonVision; import frc.lib.W8.io.vision.VisionIOPhotonVisionSim; @@ -37,8 +36,8 @@ import frc.robot.Constants.ClimberConstants; import frc.robot.Constants.HopperConstants; import frc.robot.Constants.IntakeFlywheelConstants; -import frc.robot.Constants.IntakePivotConstants; import frc.robot.Constants.IntakeFlywheelConstants.VisionConstants; +import frc.robot.Constants.IntakePivotConstants; import frc.robot.Constants.Ports; import frc.robot.Constants.ShooterFlywheelConstants; import frc.robot.Constants.ShooterRotaryConstants; @@ -56,15 +55,10 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; - -import static edu.wpi.first.units.Units.Degrees; - import java.util.Optional; - import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import org.photonvision.PhotonPoseEstimator.PoseStrategy; - /** * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -74,7 +68,7 @@ public class RobotContainer { // Subsystems - public final Drive drive; + public final Drive drive; private final Hopper hopper; private final Shooter shooter; private final Climber climber; @@ -254,7 +248,7 @@ public RobotContainer() { break; default: - //Replayed robot, disable IO implementations + // Replayed robot, disable IO implementations drive = new Drive( new GyroIO() {}, @@ -321,7 +315,7 @@ private void configureButtonBindings() { () -> controller.getLeftX(), () -> -controller.getRightX())); - //Lock to 0° when A button is held + // Lock to 0° when A button is held controller .a() .whileTrue( @@ -331,7 +325,7 @@ private void configureButtonBindings() { () -> -controller.getLeftX(), () -> new Rotation2d())); - //Switch to X pattern when X button is pressed + // Switch to X pattern when X button is pressed controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro to 0° when B button is pressed @@ -349,9 +343,7 @@ private void configureButtonBindings() { .x() .onTrue(Commands.runOnce(() -> hopper.setGoal(HopperConstants.HOPPER_POSITION), hopper)); - controller - .y() - .onTrue(Commands.runOnce(() -> intake.setVelocity(1))); + controller.y().onTrue(Commands.runOnce(() -> intake.setVelocity(1))); } /** diff --git a/src/main/java/frc/robot/subsystems/BallCounter.java b/src/main/java/frc/robot/subsystems/BallCounter.java index 513518b..874c405 100644 --- a/src/main/java/frc/robot/subsystems/BallCounter.java +++ b/src/main/java/frc/robot/subsystems/BallCounter.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.ArrayList; -import java.util.Iterator; import java.util.List; public class BallCounter extends SubsystemBase { @@ -57,7 +56,9 @@ public void shootBall() { public boolean ballShot() { Measurement measurement = _laserCAN.getMeasurement(); - return measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT && measurement.distance_mm <= 30; + return measurement != null + && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT + && measurement.distance_mm <= 30; } public double calculateFireRate() { @@ -87,12 +88,12 @@ public void periodic() { SmartDashboard.putNumber("Balls Fired", getBallsShot()); SmartDashboard.putNumber("Ball Fire Rate (per second)", Math.round(fireRate * 100.0) / 100.0); - SmartDashboard.putNumber("Seconds Since Last Fire", Math.round(secondsSinceLastFired() * 100.0) / 100.0); + SmartDashboard.putNumber( + "Seconds Since Last Fire", Math.round(secondsSinceLastFired() * 100.0) / 100.0); if (!isBlocked && !canFire) { canFire = true; - } - else if (isBlocked && canFire) { + } else if (isBlocked && canFire) { shootBall(); canFire = false; } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 76d22aa..286256f 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -4,8 +4,6 @@ import static edu.wpi.first.units.Units.Meters; 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; @@ -18,6 +16,7 @@ import frc.lib.W8.io.motor.MotorIO.PIDSlot; import frc.lib.W8.mechanisms.linear.LinearMechanism; import frc.robot.Constants.ClimberConstants; +import org.littletonrobotics.junction.Logger; public class Climber extends SubsystemBase { private LinearMechanism _io; @@ -64,13 +63,16 @@ private State(LinearVelocity velocity) { @Override public void periodic() { _io.periodic(); - - double z = Math.abs(Math.sin(Timer.getFPGATimestamp())*0.33); // Placeholder for position - // The z of the Translation3D should be 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing motor configs. - Logger.recordOutput("3DField/4_Climber", new Pose3d(new Translation3d(0,0, z), new Rotation3d(0, 0, 0))); + double z = Math.abs(Math.sin(Timer.getFPGATimestamp()) * 0.33); // Placeholder for position + + // The z of the Translation3D should be + // 'ClimberConstants.CONVERTER.toDistance(_io.getPosition()).in(Meters)', change after fixing + // motor configs. + Logger.recordOutput( + "3DField/4_Climber", new Pose3d(new Translation3d(0, 0, z), new Rotation3d(0, 0, 0))); - _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); + _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); } public void runClimber() { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 896afd8..d101ae6 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -2,16 +2,12 @@ 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.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.io.motor.MotorIO.PIDSlot; @@ -20,6 +16,7 @@ import frc.robot.Constants; import frc.robot.Constants.IntakeFlywheelConstants; import frc.robot.Constants.IntakePivotConstants; +import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { private FlywheelMechanism _rollerIO; @@ -86,8 +83,16 @@ public boolean isIntendedAngle() { 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))); + 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)); --- Tests the pivot } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 5f91890..5f89310 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -1,12 +1,9 @@ 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; @@ -24,6 +21,7 @@ import frc.robot.Constants.FeederConstants; import frc.robot.Constants.FieldConstants; import frc.robot.Constants.ShooterConstants; +import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { @@ -130,11 +128,16 @@ public Command shoot(double velocity) { public void periodic() { _hood.periodic(); - double pitch = Math.toRadians(Math.abs(Math.sin(Timer.getFPGATimestamp())*45)); // Placeholder for position - - // The pitch of the Rotation3D should be '_hood.getPosition().in(Radians)', change after fixing motor configs. - Logger.recordOutput("3DField/3_Hood", new Pose3d(new Translation3d(-0.0075,0.0,0.523), new Rotation3d(0, pitch, 0))); + double pitch = + Math.toRadians( + Math.abs(Math.sin(Timer.getFPGATimestamp()) * 45)); // Placeholder for position + + // The pitch of the Rotation3D should be '_hood.getPosition().in(Radians)', change after fixing + // motor configs. + Logger.recordOutput( + "3DField/3_Hood", + new Pose3d(new Translation3d(-0.0075, 0.0, 0.523), new Rotation3d(0, pitch, 0))); - _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); + _hood.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); } } diff --git a/src/main/java/frc/robot/util/FuelSim.java b/src/main/java/frc/robot/util/FuelSim.java index 6cf2b5e..28de21f 100644 --- a/src/main/java/frc/robot/util/FuelSim.java +++ b/src/main/java/frc/robot/util/FuelSim.java @@ -23,839 +23,919 @@ import java.util.function.Supplier; public class FuelSim { - protected static final double PERIOD = 0.02; // sec - protected static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); // m/s^2 - // Room temperature dry air density: https://en.wikipedia.org/wiki/Density_of_air#Dry_air - protected static final double AIR_DENSITY = 1.2041; // kg/m^3 - protected static final double FIELD_COR = Math.sqrt(22 / 51.5); // coefficient of restitution with the field - protected static final double FUEL_COR = 0.5; // coefficient of restitution with another fuel - protected static final double NET_COR = 0.2; // coefficient of restitution with the net - protected static final double ROBOT_COR = 0.1; // coefficient of restitution with a robot - protected static final double FUEL_RADIUS = 0.075; - protected static final double FIELD_LENGTH = 16.51; - protected static final double FIELD_WIDTH = 8.04; - protected static final double TRENCH_WIDTH = 1.265; - protected static final double TRENCH_BLOCK_WIDTH = 0.305; - protected static final double TRENCH_HEIGHT = 0.565; - protected static final double TRENCH_BAR_HEIGHT = 0.102; - protected static final double TRENCH_BAR_WIDTH = 0.152; - protected static final double FRICTION = 0.1; // proportion of horizontal vel to lose per sec while on ground - protected static final double FUEL_MASS = 0.448 * 0.45392; // kgs - protected static final double FUEL_CROSS_AREA = Math.PI * FUEL_RADIUS * FUEL_RADIUS; - // Drag coefficient of smooth sphere: https://en.wikipedia.org/wiki/Drag_coefficient#/media/File:14ilf1l.svg - protected static final double DRAG_COF = 0.47; // dimensionless - protected static final double DRAG_FORCE_FACTOR = 0.5 * AIR_DENSITY * DRAG_COF * FUEL_CROSS_AREA; - - protected static final Translation3d[] FIELD_XZ_LINE_STARTS = { - new Translation3d(0, 0, 0), - new Translation3d(3.96, 1.57, 0), - new Translation3d(3.96, FIELD_WIDTH / 2 + 0.60, 0), - new Translation3d(4.61, 1.57, 0.165), - new Translation3d(4.61, FIELD_WIDTH / 2 + 0.60, 0.165), - new Translation3d(FIELD_LENGTH - 5.18, 1.57, 0), - new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH / 2 + 0.60, 0), - new Translation3d(FIELD_LENGTH - 4.61, 1.57, 0.165), - new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 + 0.60, 0.165), - new Translation3d(3.96, TRENCH_WIDTH, TRENCH_HEIGHT), - new Translation3d(3.96, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - }; - - protected static final Translation3d[] FIELD_XZ_LINE_ENDS = { - new Translation3d(FIELD_LENGTH, FIELD_WIDTH, 0), - new Translation3d(4.61, FIELD_WIDTH / 2 - 0.60, 0.165), - new Translation3d(4.61, FIELD_WIDTH - 1.57, 0.165), - new Translation3d(5.18, FIELD_WIDTH / 2 - 0.60, 0), - new Translation3d(5.18, FIELD_WIDTH - 1.57, 0), - new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 - 0.60, 0.165), - new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH - 1.57, 0.165), - new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH / 2 - 0.60, 0), - new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57, 0), - new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d( - 4.61 + TRENCH_BAR_WIDTH / 2, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, - TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d(FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - }; - - protected static class Fuel { - protected Translation3d pos; - protected Translation3d vel; - - protected Fuel(Translation3d pos, Translation3d vel) { - this.pos = pos; - this.vel = vel; - } - - protected Fuel(Translation3d pos) { - this(pos, new Translation3d()); - } - - protected void update(boolean simulateAirResistance, int subticks) { - pos = pos.plus(vel.times(PERIOD / subticks)); - if (pos.getZ() > FUEL_RADIUS) { - Translation3d Fg = GRAVITY.times(FUEL_MASS); - Translation3d Fd = new Translation3d(); - - if (simulateAirResistance) { - double speed = vel.getNorm(); - if (speed > 1e-6) { - Fd = vel.times(-DRAG_FORCE_FACTOR * speed); - } - } - - Translation3d accel = Fg.plus(Fd).div(FUEL_MASS); - vel = vel.plus(accel.times(PERIOD / subticks)); - } - if (Math.abs(vel.getZ()) < 0.05 && pos.getZ() <= FUEL_RADIUS + 0.03) { - vel = new Translation3d(vel.getX(), vel.getY(), 0); - vel = vel.times(1 - FRICTION * PERIOD / subticks); - // pos = new Translation3d(pos.getX(), pos.getY(), FUEL_RADIUS); - } - handleFieldCollisions(subticks); - } - - protected void handleXZLineCollision(Translation3d lineStart, Translation3d lineEnd) { - if (pos.getY() < lineStart.getY() || pos.getY() > lineEnd.getY()) return; // not within y range - // Convert into 2D - Translation2d start2d = new Translation2d(lineStart.getX(), lineStart.getZ()); - Translation2d end2d = new Translation2d(lineEnd.getX(), lineEnd.getZ()); - Translation2d pos2d = new Translation2d(pos.getX(), pos.getZ()); - Translation2d lineVec = end2d.minus(start2d); - - // Get closest point on line - Translation2d projected = - start2d.plus(lineVec.times(pos2d.minus(start2d).dot(lineVec) / lineVec.getSquaredNorm())); - - if (projected.getDistance(start2d) + projected.getDistance(end2d) > lineVec.getNorm()) - return; // projected point not on line - double dist = pos2d.getDistance(projected); - if (dist > FUEL_RADIUS) return; // not intersecting line - // Back into 3D - Translation3d normal = new Translation3d(-lineVec.getY(), 0, lineVec.getX()).div(lineVec.getNorm()); - - // Apply collision response - pos = pos.plus(normal.times(FUEL_RADIUS - dist)); - if (vel.dot(normal) > 0) return; // already moving away from line - vel = vel.minus(normal.times((1 + FIELD_COR) * vel.dot(normal))); - } - - protected void handleFieldCollisions(int subticks) { - // floor and bumps - for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { - handleXZLineCollision(FIELD_XZ_LINE_STARTS[i], FIELD_XZ_LINE_ENDS[i]); - } - - // edges - if (pos.getX() < FUEL_RADIUS && vel.getX() < 0) { - pos = pos.plus(new Translation3d(FUEL_RADIUS - pos.getX(), 0, 0)); - vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); - } else if (pos.getX() > FIELD_LENGTH - FUEL_RADIUS && vel.getX() > 0) { - pos = pos.plus(new Translation3d(FIELD_LENGTH - FUEL_RADIUS - pos.getX(), 0, 0)); - vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); - } - - if (pos.getY() < FUEL_RADIUS && vel.getY() < 0) { - pos = pos.plus(new Translation3d(0, FUEL_RADIUS - pos.getY(), 0)); - vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); - } else if (pos.getY() > FIELD_WIDTH - FUEL_RADIUS && vel.getY() > 0) { - pos = pos.plus(new Translation3d(0, FIELD_WIDTH - FUEL_RADIUS - pos.getY(), 0)); - vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); - } - - // hubs - handleHubCollisions(Hub.BLUE_HUB, subticks); - handleHubCollisions(Hub.RED_HUB, subticks); - - handleTrenchCollisions(); - } - - protected void handleHubCollisions(Hub hub, int subticks) { - hub.handleHubInteraction(this, subticks); - hub.fuelCollideSide(this); - - double netCollision = hub.fuelHitNet(this); - if (netCollision != 0) { - pos = pos.plus(new Translation3d(netCollision, 0, 0)); - vel = new Translation3d(-vel.getX() * NET_COR, vel.getY() * NET_COR, vel.getZ()); - } - } - - protected void handleTrenchCollisions() { - fuelCollideRectangle( - this, - new Translation3d(3.96, TRENCH_WIDTH, 0), - new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(3.96, FIELD_WIDTH - 1.57, 0), - new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, 0), - new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, 0), - new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), - new Translation3d( - 4.61 + TRENCH_BAR_WIDTH / 2, - TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, - TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, - FIELD_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); - } - - protected void addImpulse(Translation3d impulse) { - vel = vel.plus(impulse); - } - } - - protected static void handleFuelCollision(Fuel a, Fuel b) { - Translation3d normal = a.pos.minus(b.pos); - double distance = normal.getNorm(); - if (distance == 0) { - normal = new Translation3d(1, 0, 0); - distance = 1; - } - normal = normal.div(distance); - double impulse = 0.5 * (1 + FUEL_COR) * (b.vel.minus(a.vel).dot(normal)); - double intersection = FUEL_RADIUS * 2 - distance; - a.pos = a.pos.plus(normal.times(intersection / 2)); - b.pos = b.pos.minus(normal.times(intersection / 2)); - a.addImpulse(normal.times(impulse)); - b.addImpulse(normal.times(-impulse)); - } - - protected static final double CELL_SIZE = 0.25; - protected static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE); - protected static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE); - - @SuppressWarnings("unchecked") - protected final ArrayList[][] grid = new ArrayList[GRID_COLS][GRID_ROWS]; - private final ArrayList> activeCells = new ArrayList<>(); - - protected void handleFuelCollisions(ArrayList fuels) { - // Clear grid - for (ArrayList cell : activeCells) { - cell.clear(); - } - activeCells.clear(); - - // Populate grid - for (Fuel fuel : fuels) { - int col = (int) (fuel.pos.getX() / CELL_SIZE); - int row = (int) (fuel.pos.getY() / CELL_SIZE); - - if (col >= 0 && col < GRID_COLS && row >= 0 && row < GRID_ROWS) { - grid[col][row].add(fuel); - if (grid[col][row].size() == 1) { - activeCells.add(grid[col][row]); - } - } - } - - // Check collisions - for (Fuel fuel : fuels) { - int col = (int) (fuel.pos.getX() / CELL_SIZE); - int row = (int) (fuel.pos.getY() / CELL_SIZE); - - // Check 3x3 neighbor cells - for (int i = col - 1; i <= col + 1; i++) { - for (int j = row - 1; j <= row + 1; j++) { - if (i >= 0 && i < GRID_COLS && j >= 0 && j < GRID_ROWS) { - for (Fuel other : grid[i][j]) { - if (fuel != other && fuel.pos.getDistance(other.pos) < FUEL_RADIUS * 2) { - if (fuel.hashCode() < other.hashCode()) { - handleFuelCollision(fuel, other); - } - } - } - } - } - } - } - } - - protected ArrayList fuels = new ArrayList<>(); - protected boolean running = false; - protected boolean simulateAirResistance = false; - protected Supplier robotPoseSupplier = null; - protected Supplier robotFieldSpeedsSupplier = null; - protected double robotWidth; // size along the robot's y axis - protected double robotLength; // size along the robot's x axis - protected double bumperHeight; - protected ArrayList intakes = new ArrayList<>(); - protected int subticks = 5; - - /** - * Creates a new instance of FuelSim - * @param tableKey NetworkTable to log fuel positions to as an array of {@link Translation3d} structs. - */ - public FuelSim(String tableKey) { - // Initialize grid - for (int i = 0; i < GRID_COLS; i++) { - for (int j = 0; j < GRID_ROWS; j++) { - grid[i][j] = new ArrayList(); - } - } - - fuelPublisher = NetworkTableInstance.getDefault() - .getStructArrayTopic(tableKey + "/Fuels", Translation3d.struct) - .publish(); - } - - /** - * Creates a new instance of FuelSim with log path "/Fuel Simulation" - */ - public FuelSim() { - this("/Fuel Simulation"); - } - - /** - * Clears the field of fuel - */ - public void clearFuel() { - fuels.clear(); - } - - /** - * Spawns fuel in the neutral zone and depots - */ - public void spawnStartingFuel() { - // Center fuel - Translation3d center = new Translation3d(FIELD_LENGTH / 2, FIELD_WIDTH / 2, FUEL_RADIUS); - for (int i = 0; i < 15; i++) { - for (int j = 0; j < 6; j++) { - fuels.add(new Fuel(center.plus(new Translation3d(0.076 + 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); - fuels.add(new Fuel(center.plus(new Translation3d(-0.076 - 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); - fuels.add(new Fuel(center.plus(new Translation3d(0.076 + 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); - fuels.add(new Fuel(center.plus(new Translation3d(-0.076 - 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); - } - } - - // Depots - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 4; j++) { - fuels.add(new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 + 0.076 + 0.152 * i, FUEL_RADIUS))); - fuels.add(new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 - 0.076 - 0.152 * i, FUEL_RADIUS))); - fuels.add(new Fuel( - new Translation3d(FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 + 0.076 + 0.152 * i, FUEL_RADIUS))); - fuels.add(new Fuel( - new Translation3d(FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 - 0.076 - 0.152 * i, FUEL_RADIUS))); - } - } - - // DEBUG: Log XZ lines - // Translation3d[][] lines = new Translation3d[FIELD_XZ_LINE_STARTS.length][2]; - // for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { - // lines[i][0] = FIELD_XZ_LINE_STARTS[i]; - // lines[i][1] = FIELD_XZ_LINE_ENDS[i]; - // } - - // Logger.recordOutput("Fuel Simulation/Lines (debug)", lines); - } - - protected StructArrayPublisher fuelPublisher; - - /** - * Adds array of `Translation3d`'s to NetworkTables at tableKey + "/Fuels" - */ - public void logFuels() { - fuelPublisher.set(fuels.stream().map((fuel) -> fuel.pos).toArray(Translation3d[]::new)); - } - - /** - * Start the simulation. `updateSim` must still be called every loop - */ - public void start() { - running = true; - } - - /** - * Pause the simulation. - */ - public void stop() { - running = false; - } - - /** Enables accounting for drag force in physics step **/ - public void enableAirResistance() { - simulateAirResistance = true; - } - - /** - * Sets the number of physics iterations per loop (0.02s) - * @param subticks - */ - public void setSubticks(int subticks) { - this.subticks = subticks; - } - - /** - * Registers a robot with the fuel simulator - * @param width from left to right (y-axis) - * @param length from front to back (x-axis) - * @param bumperHeight - * @param poseSupplier - * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier - */ - public void registerRobot( - double width, - double length, - double bumperHeight, - Supplier poseSupplier, - Supplier fieldSpeedsSupplier) { - this.robotPoseSupplier = poseSupplier; - this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; - this.robotWidth = width; - this.robotLength = length; - this.bumperHeight = bumperHeight; - } - - /** - * Registers a robot with the fuel simulator - * @param width from left to right (y-axis) - * @param length from front to back (x-axis) - * @param bumperHeight from the ground - * @param poseSupplier - * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier - */ - public void registerRobot( - Distance width, - Distance length, - Distance bumperHeight, - Supplier poseSupplier, - Supplier fieldSpeedsSupplier) { - this.robotPoseSupplier = poseSupplier; - this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; - this.robotWidth = width.in(Meters); - this.robotLength = length.in(Meters); - this.bumperHeight = bumperHeight.in(Meters); - } - - /** - * To be called periodically - * Will do nothing if sim is not running - */ - public void updateSim() { - if (!running) return; - - stepSim(); - } - - /** - * Run the simulation forward 1 time step (0.02s) - */ - public void stepSim() { - for (int i = 0; i < subticks; i++) { - for (Fuel fuel : fuels) { - fuel.update(this.simulateAirResistance, this.subticks); - } - - handleFuelCollisions(fuels); - - if (robotPoseSupplier != null) { - handleRobotCollisions(fuels); - handleIntakes(fuels); - } - } - - logFuels(); - } - - /** - * Adds a fuel onto the field - * @param pos Position to spawn at - * @param vel Initial velocity vector - */ - public void spawnFuel(Translation3d pos, Translation3d vel) { - fuels.add(new Fuel(pos, vel)); - } - - /** - * Spawns a fuel onto the field with a specified launch velocity and angles, accounting for robot movement - * @param launchVelocity Initial launch velocity - * @param hoodAngle Hood angle where 0 is launching horizontally and 90 degrees is launching straight up - * @param turretYaw Robot-relative turret yaw - * @param launchHeight Height of the fuel to launch at. Make sure this is higher than your robot's bumper height, or else it will collide with your robot immediately. - * @throws IllegalStateException if robot is not registered - */ - public void launchFuel(LinearVelocity launchVelocity, Angle hoodAngle, Angle turretYaw, Distance launchHeight) { - if (robotPoseSupplier == null || robotFieldSpeedsSupplier == null) { - throw new IllegalStateException("Robot must be registered before launching fuel."); - } - - Pose3d launchPose = new Pose3d(this.robotPoseSupplier.get()) - .plus(new Transform3d(new Translation3d(Meters.zero(), Meters.zero(), launchHeight), Rotation3d.kZero)); - ChassisSpeeds fieldSpeeds = this.robotFieldSpeedsSupplier.get(); - - double horizontalVel = Math.cos(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); - double verticalVel = Math.sin(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); - double xVel = horizontalVel - * Math.cos( - turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); - double yVel = horizontalVel - * Math.sin( - turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); - - xVel += fieldSpeeds.vxMetersPerSecond; - yVel += fieldSpeeds.vyMetersPerSecond; - - spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); - } - - protected void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) { - Translation2d relativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) - .relativeTo(robot) - .getTranslation(); - - if (fuel.pos.getZ() > bumperHeight) return; // above bumpers - double distanceToBottom = -FUEL_RADIUS - robotLength / 2 - relativePos.getX(); - double distanceToTop = -FUEL_RADIUS - robotLength / 2 + relativePos.getX(); - double distanceToRight = -FUEL_RADIUS - robotWidth / 2 - relativePos.getY(); - double distanceToLeft = -FUEL_RADIUS - robotWidth / 2 + relativePos.getY(); - - // not inside robot - if (distanceToBottom > 0 || distanceToTop > 0 || distanceToRight > 0 || distanceToLeft > 0) return; - - Translation2d posOffset; - // find minimum distance to side and send corresponding collision response - if ((distanceToBottom >= distanceToTop - && distanceToBottom >= distanceToRight - && distanceToBottom >= distanceToLeft)) { - posOffset = new Translation2d(distanceToBottom, 0); - } else if ((distanceToTop >= distanceToBottom - && distanceToTop >= distanceToRight - && distanceToTop >= distanceToLeft)) { - posOffset = new Translation2d(-distanceToTop, 0); - } else if ((distanceToRight >= distanceToBottom - && distanceToRight >= distanceToTop - && distanceToRight >= distanceToLeft)) { - posOffset = new Translation2d(0, distanceToRight); - } else { - posOffset = new Translation2d(0, -distanceToLeft); - } - - posOffset = posOffset.rotateBy(robot.getRotation()); - fuel.pos = fuel.pos.plus(new Translation3d(posOffset)); - Translation2d normal = posOffset.div(posOffset.getNorm()); - if (fuel.vel.toTranslation2d().dot(normal) < 0) - fuel.addImpulse( - new Translation3d(normal.times(-fuel.vel.toTranslation2d().dot(normal) * (1 + ROBOT_COR)))); - if (robotVel.dot(normal) > 0) fuel.addImpulse(new Translation3d(normal.times(robotVel.dot(normal)))); - } - - protected void handleRobotCollisions(ArrayList fuels) { - Pose2d robot = robotPoseSupplier.get(); - ChassisSpeeds speeds = robotFieldSpeedsSupplier.get(); - Translation2d robotVel = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - - for (Fuel fuel : fuels) { - handleRobotCollision(fuel, robot, robotVel); - } - } - - protected void handleIntakes(ArrayList fuels) { - Pose2d robot = robotPoseSupplier.get(); - for (SimIntake intake : intakes) { - for (int i = 0; i < fuels.size(); i++) { - if (intake.shouldIntake(fuels.get(i), robot)) { - fuels.remove(i); - i--; + protected static final double PERIOD = 0.02; // sec + protected static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); // m/s^2 + // Room temperature dry air density: https://en.wikipedia.org/wiki/Density_of_air#Dry_air + protected static final double AIR_DENSITY = 1.2041; // kg/m^3 + protected static final double FIELD_COR = + Math.sqrt(22 / 51.5); // coefficient of restitution with the field + protected static final double FUEL_COR = 0.5; // coefficient of restitution with another fuel + protected static final double NET_COR = 0.2; // coefficient of restitution with the net + protected static final double ROBOT_COR = 0.1; // coefficient of restitution with a robot + protected static final double FUEL_RADIUS = 0.075; + protected static final double FIELD_LENGTH = 16.51; + protected static final double FIELD_WIDTH = 8.04; + protected static final double TRENCH_WIDTH = 1.265; + protected static final double TRENCH_BLOCK_WIDTH = 0.305; + protected static final double TRENCH_HEIGHT = 0.565; + protected static final double TRENCH_BAR_HEIGHT = 0.102; + protected static final double TRENCH_BAR_WIDTH = 0.152; + protected static final double FRICTION = + 0.1; // proportion of horizontal vel to lose per sec while on ground + protected static final double FUEL_MASS = 0.448 * 0.45392; // kgs + protected static final double FUEL_CROSS_AREA = Math.PI * FUEL_RADIUS * FUEL_RADIUS; + // Drag coefficient of smooth sphere: + // https://en.wikipedia.org/wiki/Drag_coefficient#/media/File:14ilf1l.svg + protected static final double DRAG_COF = 0.47; // dimensionless + protected static final double DRAG_FORCE_FACTOR = 0.5 * AIR_DENSITY * DRAG_COF * FUEL_CROSS_AREA; + + protected static final Translation3d[] FIELD_XZ_LINE_STARTS = { + new Translation3d(0, 0, 0), + new Translation3d(3.96, 1.57, 0), + new Translation3d(3.96, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(4.61, 1.57, 0.165), + new Translation3d(4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 5.18, 1.57, 0), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(FIELD_LENGTH - 4.61, 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(3.96, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(3.96, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, + FIELD_WIDTH - 1.57, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + protected static final Translation3d[] FIELD_XZ_LINE_ENDS = { + new Translation3d(FIELD_LENGTH, FIELD_WIDTH, 0), + new Translation3d(4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(5.18, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + protected static class Fuel { + protected Translation3d pos; + protected Translation3d vel; + + protected Fuel(Translation3d pos, Translation3d vel) { + this.pos = pos; + this.vel = vel; + } + + protected Fuel(Translation3d pos) { + this(pos, new Translation3d()); + } + + protected void update(boolean simulateAirResistance, int subticks) { + pos = pos.plus(vel.times(PERIOD / subticks)); + if (pos.getZ() > FUEL_RADIUS) { + Translation3d Fg = GRAVITY.times(FUEL_MASS); + Translation3d Fd = new Translation3d(); + + if (simulateAirResistance) { + double speed = vel.getNorm(); + if (speed > 1e-6) { + Fd = vel.times(-DRAG_FORCE_FACTOR * speed); + } + } + + Translation3d accel = Fg.plus(Fd).div(FUEL_MASS); + vel = vel.plus(accel.times(PERIOD / subticks)); + } + if (Math.abs(vel.getZ()) < 0.05 && pos.getZ() <= FUEL_RADIUS + 0.03) { + vel = new Translation3d(vel.getX(), vel.getY(), 0); + vel = vel.times(1 - FRICTION * PERIOD / subticks); + // pos = new Translation3d(pos.getX(), pos.getY(), FUEL_RADIUS); + } + handleFieldCollisions(subticks); + } + + protected void handleXZLineCollision(Translation3d lineStart, Translation3d lineEnd) { + if (pos.getY() < lineStart.getY() || pos.getY() > lineEnd.getY()) + return; // not within y range + // Convert into 2D + Translation2d start2d = new Translation2d(lineStart.getX(), lineStart.getZ()); + Translation2d end2d = new Translation2d(lineEnd.getX(), lineEnd.getZ()); + Translation2d pos2d = new Translation2d(pos.getX(), pos.getZ()); + Translation2d lineVec = end2d.minus(start2d); + + // Get closest point on line + Translation2d projected = + start2d.plus(lineVec.times(pos2d.minus(start2d).dot(lineVec) / lineVec.getSquaredNorm())); + + if (projected.getDistance(start2d) + projected.getDistance(end2d) > lineVec.getNorm()) + return; // projected point not on line + double dist = pos2d.getDistance(projected); + if (dist > FUEL_RADIUS) return; // not intersecting line + // Back into 3D + Translation3d normal = + new Translation3d(-lineVec.getY(), 0, lineVec.getX()).div(lineVec.getNorm()); + + // Apply collision response + pos = pos.plus(normal.times(FUEL_RADIUS - dist)); + if (vel.dot(normal) > 0) return; // already moving away from line + vel = vel.minus(normal.times((1 + FIELD_COR) * vel.dot(normal))); + } + + protected void handleFieldCollisions(int subticks) { + // floor and bumps + for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + handleXZLineCollision(FIELD_XZ_LINE_STARTS[i], FIELD_XZ_LINE_ENDS[i]); + } + + // edges + if (pos.getX() < FUEL_RADIUS && vel.getX() < 0) { + pos = pos.plus(new Translation3d(FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } else if (pos.getX() > FIELD_LENGTH - FUEL_RADIUS && vel.getX() > 0) { + pos = pos.plus(new Translation3d(FIELD_LENGTH - FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } + + if (pos.getY() < FUEL_RADIUS && vel.getY() < 0) { + pos = pos.plus(new Translation3d(0, FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } else if (pos.getY() > FIELD_WIDTH - FUEL_RADIUS && vel.getY() > 0) { + pos = pos.plus(new Translation3d(0, FIELD_WIDTH - FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } + + // hubs + handleHubCollisions(Hub.BLUE_HUB, subticks); + handleHubCollisions(Hub.RED_HUB, subticks); + + handleTrenchCollisions(); + } + + protected void handleHubCollisions(Hub hub, int subticks) { + hub.handleHubInteraction(this, subticks); + hub.fuelCollideSide(this); + + double netCollision = hub.fuelHitNet(this); + if (netCollision != 0) { + pos = pos.plus(new Translation3d(netCollision, 0, 0)); + vel = new Translation3d(-vel.getX() * NET_COR, vel.getY() * NET_COR, vel.getZ()); + } + } + + protected void handleTrenchCollisions() { + fuelCollideRectangle( + this, + new Translation3d(3.96, TRENCH_WIDTH, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, 0), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d( + FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + FIELD_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + } + + protected void addImpulse(Translation3d impulse) { + vel = vel.plus(impulse); + } + } + + protected static void handleFuelCollision(Fuel a, Fuel b) { + Translation3d normal = a.pos.minus(b.pos); + double distance = normal.getNorm(); + if (distance == 0) { + normal = new Translation3d(1, 0, 0); + distance = 1; + } + normal = normal.div(distance); + double impulse = 0.5 * (1 + FUEL_COR) * (b.vel.minus(a.vel).dot(normal)); + double intersection = FUEL_RADIUS * 2 - distance; + a.pos = a.pos.plus(normal.times(intersection / 2)); + b.pos = b.pos.minus(normal.times(intersection / 2)); + a.addImpulse(normal.times(impulse)); + b.addImpulse(normal.times(-impulse)); + } + + protected static final double CELL_SIZE = 0.25; + protected static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE); + protected static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE); + + @SuppressWarnings("unchecked") + protected final ArrayList[][] grid = new ArrayList[GRID_COLS][GRID_ROWS]; + + private final ArrayList> activeCells = new ArrayList<>(); + + protected void handleFuelCollisions(ArrayList fuels) { + // Clear grid + for (ArrayList cell : activeCells) { + cell.clear(); + } + activeCells.clear(); + + // Populate grid + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + if (col >= 0 && col < GRID_COLS && row >= 0 && row < GRID_ROWS) { + grid[col][row].add(fuel); + if (grid[col][row].size() == 1) { + activeCells.add(grid[col][row]); + } + } + } + + // Check collisions + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + // Check 3x3 neighbor cells + for (int i = col - 1; i <= col + 1; i++) { + for (int j = row - 1; j <= row + 1; j++) { + if (i >= 0 && i < GRID_COLS && j >= 0 && j < GRID_ROWS) { + for (Fuel other : grid[i][j]) { + if (fuel != other && fuel.pos.getDistance(other.pos) < FUEL_RADIUS * 2) { + if (fuel.hashCode() < other.hashCode()) { + handleFuelCollision(fuel, other); } + } } - } - } - - protected static void fuelCollideRectangle(Fuel fuel, Translation3d start, Translation3d end) { - if (fuel.pos.getZ() > end.getZ() + FUEL_RADIUS || fuel.pos.getZ() < start.getZ() - FUEL_RADIUS) - return; // above rectangle - double distanceToLeft = start.getX() - FUEL_RADIUS - fuel.pos.getX(); - double distanceToRight = fuel.pos.getX() - end.getX() - FUEL_RADIUS; - double distanceToTop = fuel.pos.getY() - end.getY() - FUEL_RADIUS; - double distanceToBottom = start.getY() - FUEL_RADIUS - fuel.pos.getY(); - - // not inside hub - if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || distanceToBottom > 0) return; - - Translation2d collision; - // find minimum distance to side and send corresponding collision response - if (fuel.pos.getX() < start.getX() - || (distanceToLeft >= distanceToRight - && distanceToLeft >= distanceToTop - && distanceToLeft >= distanceToBottom)) { - collision = new Translation2d(distanceToLeft, 0); - } else if (fuel.pos.getX() >= end.getX() - || (distanceToRight >= distanceToLeft - && distanceToRight >= distanceToTop - && distanceToRight >= distanceToBottom)) { - collision = new Translation2d(-distanceToRight, 0); - } else if (fuel.pos.getY() > end.getY() - || (distanceToTop >= distanceToLeft - && distanceToTop >= distanceToRight - && distanceToTop >= distanceToBottom)) { - collision = new Translation2d(0, -distanceToTop); - } else { - collision = new Translation2d(0, distanceToBottom); - } - - if (collision.getX() != 0) { - fuel.pos = fuel.pos.plus(new Translation3d(collision)); - fuel.vel = fuel.vel.plus(new Translation3d(-(1 + FIELD_COR) * fuel.vel.getX(), 0, 0)); - } else if (collision.getY() != 0) { - fuel.pos = fuel.pos.plus(new Translation3d(collision)); - fuel.vel = fuel.vel.plus(new Translation3d(0, -(1 + FIELD_COR) * fuel.vel.getY(), 0)); - } - } - - /** - * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param ableToIntake Should a return a boolean whether the intake is active - * @param intakeCallback Function to call when a fuel is intaked - */ - public void registerIntake( - double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake, Runnable intakeCallback) { - intakes.add(new SimIntake(xMin, xMax, yMin, yMax, ableToIntake, intakeCallback)); - } - - /** - * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param ableToIntake Should a return a boolean whether the intake is active - */ - public void registerIntake(double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake) { - registerIntake(xMin, xMax, yMin, yMax, ableToIntake, () -> {}); - } - - /** - * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param intakeCallback Function to call when a fuel is intaked - */ - public void registerIntake(double xMin, double xMax, double yMin, double yMax, Runnable intakeCallback) { - registerIntake(xMin, xMax, yMin, yMax, () -> true, intakeCallback); - } - - /** - * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - */ - public void registerIntake(double xMin, double xMax, double yMin, double yMax) { - registerIntake(xMin, xMax, yMin, yMax, () -> true, () -> {}); - } - - /** - * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param ableToIntake Should a return a boolean whether the intake is active - * @param intakeCallback Function to call when a fuel is intaked - */ - public void registerIntake( - Distance xMin, Distance xMax, Distance yMin, Distance yMax, BooleanSupplier ableToIntake, Runnable intakeCallback) { - registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), ableToIntake, intakeCallback); + } + } + } + } + } + + protected ArrayList fuels = new ArrayList<>(); + protected boolean running = false; + protected boolean simulateAirResistance = false; + protected Supplier robotPoseSupplier = null; + protected Supplier robotFieldSpeedsSupplier = null; + protected double robotWidth; // size along the robot's y axis + protected double robotLength; // size along the robot's x axis + protected double bumperHeight; + protected ArrayList intakes = new ArrayList<>(); + protected int subticks = 5; + + /** + * Creates a new instance of FuelSim + * + * @param tableKey NetworkTable to log fuel positions to as an array of {@link Translation3d} + * structs. + */ + public FuelSim(String tableKey) { + // Initialize grid + for (int i = 0; i < GRID_COLS; i++) { + for (int j = 0; j < GRID_ROWS; j++) { + grid[i][j] = new ArrayList(); + } + } + + fuelPublisher = + NetworkTableInstance.getDefault() + .getStructArrayTopic(tableKey + "/Fuels", Translation3d.struct) + .publish(); + } + + /** Creates a new instance of FuelSim with log path "/Fuel Simulation" */ + public FuelSim() { + this("/Fuel Simulation"); + } + + /** Clears the field of fuel */ + public void clearFuel() { + fuels.clear(); + } + + /** Spawns fuel in the neutral zone and depots */ + public void spawnStartingFuel() { + // Center fuel + Translation3d center = new Translation3d(FIELD_LENGTH / 2, FIELD_WIDTH / 2, FUEL_RADIUS); + for (int i = 0; i < 15; i++) { + for (int j = 0; j < 6; j++) { + fuels.add( + new Fuel( + center.plus(new Translation3d(0.076 + 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus(new Translation3d(-0.076 - 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus(new Translation3d(0.076 + 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus( + new Translation3d(-0.076 - 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + } + } + + // Depots + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 4; j++) { + fuels.add( + new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 - 0.076 - 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel( + new Translation3d( + FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel( + new Translation3d( + FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 - 0.076 - 0.152 * i, FUEL_RADIUS))); + } + } + + // DEBUG: Log XZ lines + // Translation3d[][] lines = new Translation3d[FIELD_XZ_LINE_STARTS.length][2]; + // for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + // lines[i][0] = FIELD_XZ_LINE_STARTS[i]; + // lines[i][1] = FIELD_XZ_LINE_ENDS[i]; + // } + + // Logger.recordOutput("Fuel Simulation/Lines (debug)", lines); + } + + protected StructArrayPublisher fuelPublisher; + + /** Adds array of `Translation3d`'s to NetworkTables at tableKey + "/Fuels" */ + public void logFuels() { + fuelPublisher.set(fuels.stream().map((fuel) -> fuel.pos).toArray(Translation3d[]::new)); + } + + /** Start the simulation. `updateSim` must still be called every loop */ + public void start() { + running = true; + } + + /** Pause the simulation. */ + public void stop() { + running = false; + } + + /** Enables accounting for drag force in physics step * */ + public void enableAirResistance() { + simulateAirResistance = true; + } + + /** + * Sets the number of physics iterations per loop (0.02s) + * + * @param subticks + */ + public void setSubticks(int subticks) { + this.subticks = subticks; + } + + /** + * Registers a robot with the fuel simulator + * + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param bumperHeight + * @param poseSupplier + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + double width, + double length, + double bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width; + this.robotLength = length; + this.bumperHeight = bumperHeight; + } + + /** + * Registers a robot with the fuel simulator + * + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param bumperHeight from the ground + * @param poseSupplier + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + Distance width, + Distance length, + Distance bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width.in(Meters); + this.robotLength = length.in(Meters); + this.bumperHeight = bumperHeight.in(Meters); + } + + /** To be called periodically Will do nothing if sim is not running */ + public void updateSim() { + if (!running) return; + + stepSim(); + } + + /** Run the simulation forward 1 time step (0.02s) */ + public void stepSim() { + for (int i = 0; i < subticks; i++) { + for (Fuel fuel : fuels) { + fuel.update(this.simulateAirResistance, this.subticks); + } + + handleFuelCollisions(fuels); + + if (robotPoseSupplier != null) { + handleRobotCollisions(fuels); + handleIntakes(fuels); + } + } + + logFuels(); + } + + /** + * Adds a fuel onto the field + * + * @param pos Position to spawn at + * @param vel Initial velocity vector + */ + public void spawnFuel(Translation3d pos, Translation3d vel) { + fuels.add(new Fuel(pos, vel)); + } + + /** + * Spawns a fuel onto the field with a specified launch velocity and angles, accounting for robot + * movement + * + * @param launchVelocity Initial launch velocity + * @param hoodAngle Hood angle where 0 is launching horizontally and 90 degrees is launching + * straight up + * @param turretYaw Robot-relative turret yaw + * @param launchHeight Height of the fuel to launch at. Make sure this is higher than your robot's + * bumper height, or else it will collide with your robot immediately. + * @throws IllegalStateException if robot is not registered + */ + public void launchFuel( + LinearVelocity launchVelocity, Angle hoodAngle, Angle turretYaw, Distance launchHeight) { + if (robotPoseSupplier == null || robotFieldSpeedsSupplier == null) { + throw new IllegalStateException("Robot must be registered before launching fuel."); + } + + Pose3d launchPose = + new Pose3d(this.robotPoseSupplier.get()) + .plus( + new Transform3d( + new Translation3d(Meters.zero(), Meters.zero(), launchHeight), + Rotation3d.kZero)); + ChassisSpeeds fieldSpeeds = this.robotFieldSpeedsSupplier.get(); + + double horizontalVel = Math.cos(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double verticalVel = Math.sin(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double xVel = + horizontalVel + * Math.cos(turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + double yVel = + horizontalVel + * Math.sin(turretYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + + xVel += fieldSpeeds.vxMetersPerSecond; + yVel += fieldSpeeds.vyMetersPerSecond; + + spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); + } + + protected void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) { + Translation2d relativePos = + new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero).relativeTo(robot).getTranslation(); + + if (fuel.pos.getZ() > bumperHeight) return; // above bumpers + double distanceToBottom = -FUEL_RADIUS - robotLength / 2 - relativePos.getX(); + double distanceToTop = -FUEL_RADIUS - robotLength / 2 + relativePos.getX(); + double distanceToRight = -FUEL_RADIUS - robotWidth / 2 - relativePos.getY(); + double distanceToLeft = -FUEL_RADIUS - robotWidth / 2 + relativePos.getY(); + + // not inside robot + if (distanceToBottom > 0 || distanceToTop > 0 || distanceToRight > 0 || distanceToLeft > 0) + return; + + Translation2d posOffset; + // find minimum distance to side and send corresponding collision response + if ((distanceToBottom >= distanceToTop + && distanceToBottom >= distanceToRight + && distanceToBottom >= distanceToLeft)) { + posOffset = new Translation2d(distanceToBottom, 0); + } else if ((distanceToTop >= distanceToBottom + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToLeft)) { + posOffset = new Translation2d(-distanceToTop, 0); + } else if ((distanceToRight >= distanceToBottom + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToLeft)) { + posOffset = new Translation2d(0, distanceToRight); + } else { + posOffset = new Translation2d(0, -distanceToLeft); + } + + posOffset = posOffset.rotateBy(robot.getRotation()); + fuel.pos = fuel.pos.plus(new Translation3d(posOffset)); + Translation2d normal = posOffset.div(posOffset.getNorm()); + if (fuel.vel.toTranslation2d().dot(normal) < 0) + fuel.addImpulse( + new Translation3d( + normal.times(-fuel.vel.toTranslation2d().dot(normal) * (1 + ROBOT_COR)))); + if (robotVel.dot(normal) > 0) + fuel.addImpulse(new Translation3d(normal.times(robotVel.dot(normal)))); + } + + protected void handleRobotCollisions(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + ChassisSpeeds speeds = robotFieldSpeedsSupplier.get(); + Translation2d robotVel = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + + for (Fuel fuel : fuels) { + handleRobotCollision(fuel, robot, robotVel); + } + } + + protected void handleIntakes(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + for (SimIntake intake : intakes) { + for (int i = 0; i < fuels.size(); i++) { + if (intake.shouldIntake(fuels.get(i), robot)) { + fuels.remove(i); + i--; + } + } + } + } + + protected static void fuelCollideRectangle(Fuel fuel, Translation3d start, Translation3d end) { + if (fuel.pos.getZ() > end.getZ() + FUEL_RADIUS || fuel.pos.getZ() < start.getZ() - FUEL_RADIUS) + return; // above rectangle + double distanceToLeft = start.getX() - FUEL_RADIUS - fuel.pos.getX(); + double distanceToRight = fuel.pos.getX() - end.getX() - FUEL_RADIUS; + double distanceToTop = fuel.pos.getY() - end.getY() - FUEL_RADIUS; + double distanceToBottom = start.getY() - FUEL_RADIUS - fuel.pos.getY(); + + // not inside hub + if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || distanceToBottom > 0) + return; + + Translation2d collision; + // find minimum distance to side and send corresponding collision response + if (fuel.pos.getX() < start.getX() + || (distanceToLeft >= distanceToRight + && distanceToLeft >= distanceToTop + && distanceToLeft >= distanceToBottom)) { + collision = new Translation2d(distanceToLeft, 0); + } else if (fuel.pos.getX() >= end.getX() + || (distanceToRight >= distanceToLeft + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToBottom)) { + collision = new Translation2d(-distanceToRight, 0); + } else if (fuel.pos.getY() > end.getY() + || (distanceToTop >= distanceToLeft + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToBottom)) { + collision = new Translation2d(0, -distanceToTop); + } else { + collision = new Translation2d(0, distanceToBottom); + } + + if (collision.getX() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(-(1 + FIELD_COR) * fuel.vel.getX(), 0, 0)); + } else if (collision.getY() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(0, -(1 + FIELD_COR) * fuel.vel.getY(), 0)); + } + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + double xMin, + double xMax, + double yMin, + double yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + intakes.add(new SimIntake(xMin, xMax, yMin, yMax, ableToIntake, intakeCallback)); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + */ + public void registerIntake( + double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake) { + registerIntake(xMin, xMax, yMin, yMax, ableToIntake, () -> {}); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + double xMin, double xMax, double yMin, double yMax, Runnable intakeCallback) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + */ + public void registerIntake(double xMin, double xMax, double yMin, double yMax) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, () -> {}); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + Distance xMin, + Distance xMax, + Distance yMin, + Distance yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + registerIntake( + xMin.in(Meters), + xMax.in(Meters), + yMin.in(Meters), + yMax.in(Meters), + ableToIntake, + intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + */ + public void registerIntake( + Distance xMin, Distance xMax, Distance yMin, Distance yMax, BooleanSupplier ableToIntake) { + registerIntake( + xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), ableToIntake); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + Distance xMin, Distance xMax, Distance yMin, Distance yMax, Runnable intakeCallback) { + registerIntake( + xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + */ + public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax) { + registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters)); + } + + public static class Hub { + public static final Hub BLUE_HUB = + new Hub( + new Translation2d(4.61, FIELD_WIDTH / 2), + new Translation3d(5.3, FIELD_WIDTH / 2, 0.89), + 1); + public static final Hub RED_HUB = + new Hub( + new Translation2d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2), + new Translation3d(FIELD_LENGTH - 5.3, FIELD_WIDTH / 2, 0.89), + -1); + + protected static final double ENTRY_HEIGHT = 1.83; + protected static final double ENTRY_RADIUS = 0.56; + + protected static final double SIDE = 1.2; + + protected static final double NET_HEIGHT_MAX = 3.057; + protected static final double NET_HEIGHT_MIN = 1.5; + protected static final double NET_OFFSET = SIDE / 2 + 0.261; + protected static final double NET_WIDTH = 1.484; + + protected final Translation2d center; + protected final Translation3d exit; + protected final int exitVelXMult; + + protected int score = 0; + + protected Hub(Translation2d center, Translation3d exit, int exitVelXMult) { + this.center = center; + this.exit = exit; + this.exitVelXMult = exitVelXMult; + } + + protected void handleHubInteraction(Fuel fuel, int subticks) { + if (didFuelScore(fuel, subticks)) { + fuel.pos = exit; + fuel.vel = getDispersalVelocity(); + score++; + } + } + + protected boolean didFuelScore(Fuel fuel, int subticks) { + return fuel.pos.toTranslation2d().getDistance(center) <= ENTRY_RADIUS + && fuel.pos.getZ() <= ENTRY_HEIGHT + && fuel.pos.minus(fuel.vel.times(PERIOD / subticks)).getZ() > ENTRY_HEIGHT; + } + + protected Translation3d getDispersalVelocity() { + return new Translation3d( + exitVelXMult * (Math.random() + 0.1) * 1.5, Math.random() * 2 - 1, 0); + } + + /** Reset this hub's score to 0 */ + public void resetScore() { + score = 0; } /** - * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param ableToIntake Should a return a boolean whether the intake is active + * Get the current count of fuel scored in this hub + * + * @return */ - public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax, BooleanSupplier ableToIntake) { - registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), ableToIntake); - } - - /** - * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param intakeCallback Function to call when a fuel is intaked - */ - public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax, Runnable intakeCallback) { - registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters), intakeCallback); - } - - /** - * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - */ - public void registerIntake(Distance xMin, Distance xMax, Distance yMin, Distance yMax) { - registerIntake(xMin.in(Meters), xMax.in(Meters), yMin.in(Meters), yMax.in(Meters)); - } - - public static class Hub { - public static final Hub BLUE_HUB = - new Hub(new Translation2d(4.61, FIELD_WIDTH / 2), new Translation3d(5.3, FIELD_WIDTH / 2, 0.89), 1); - public static final Hub RED_HUB = new Hub( - new Translation2d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2), - new Translation3d(FIELD_LENGTH - 5.3, FIELD_WIDTH / 2, 0.89), - -1); - - protected static final double ENTRY_HEIGHT = 1.83; - protected static final double ENTRY_RADIUS = 0.56; - - protected static final double SIDE = 1.2; - - protected static final double NET_HEIGHT_MAX = 3.057; - protected static final double NET_HEIGHT_MIN = 1.5; - protected static final double NET_OFFSET = SIDE / 2 + 0.261; - protected static final double NET_WIDTH = 1.484; - - protected final Translation2d center; - protected final Translation3d exit; - protected final int exitVelXMult; - - protected int score = 0; - - protected Hub(Translation2d center, Translation3d exit, int exitVelXMult) { - this.center = center; - this.exit = exit; - this.exitVelXMult = exitVelXMult; - } - - protected void handleHubInteraction(Fuel fuel, int subticks) { - if (didFuelScore(fuel, subticks)) { - fuel.pos = exit; - fuel.vel = getDispersalVelocity(); - score++; - } - } - - protected boolean didFuelScore(Fuel fuel, int subticks) { - return fuel.pos.toTranslation2d().getDistance(center) <= ENTRY_RADIUS - && fuel.pos.getZ() <= ENTRY_HEIGHT - && fuel.pos.minus(fuel.vel.times(PERIOD / subticks)).getZ() > ENTRY_HEIGHT; - } - - protected Translation3d getDispersalVelocity() { - return new Translation3d(exitVelXMult * (Math.random() + 0.1) * 1.5, Math.random() * 2 - 1, 0); - } - - /** - * Reset this hub's score to 0 - */ - public void resetScore() { - score = 0; - } - - /** - * Get the current count of fuel scored in this hub - * @return - */ - public int getScore() { - return score; - } - - protected void fuelCollideSide(Fuel fuel) { - fuelCollideRectangle( - fuel, - new Translation3d(center.getX() - SIDE / 2, center.getY() - SIDE / 2, 0), - new Translation3d(center.getX() + SIDE / 2, center.getY() + SIDE / 2, ENTRY_HEIGHT - 0.1)); - } - - protected double fuelHitNet(Fuel fuel) { - if (fuel.pos.getZ() > NET_HEIGHT_MAX || fuel.pos.getZ() < NET_HEIGHT_MIN) return 0; - if (fuel.pos.getY() > center.getY() + NET_WIDTH / 2 || fuel.pos.getY() < center.getY() - NET_WIDTH / 2) - return 0; - if (fuel.pos.getX() > center.getX() + NET_OFFSET * exitVelXMult) { - return Math.max(0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() - FUEL_RADIUS)); - } else { - return Math.min(0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() + FUEL_RADIUS)); - } - } - } - - protected class SimIntake { - double xMin, xMax, yMin, yMax; - BooleanSupplier ableToIntake; - Runnable callback; - - protected SimIntake( - double xMin, - double xMax, - double yMin, - double yMax, - BooleanSupplier ableToIntake, - Runnable intakeCallback) { - this.xMin = xMin; - this.xMax = xMax; - this.yMin = yMin; - this.yMax = yMax; - this.ableToIntake = ableToIntake; - this.callback = intakeCallback; - } - - protected boolean shouldIntake(Fuel fuel, Pose2d robotPose) { - if (!ableToIntake.getAsBoolean() || fuel.pos.getZ() > bumperHeight) return false; - - Translation2d fuelRelativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) - .relativeTo(robotPose) - .getTranslation(); - - boolean result = fuelRelativePos.getX() >= xMin - && fuelRelativePos.getX() <= xMax - && fuelRelativePos.getY() >= yMin - && fuelRelativePos.getY() <= yMax; - if (result) { - callback.run(); - } - return result; - } - } + public int getScore() { + return score; + } + + protected void fuelCollideSide(Fuel fuel) { + fuelCollideRectangle( + fuel, + new Translation3d(center.getX() - SIDE / 2, center.getY() - SIDE / 2, 0), + new Translation3d( + center.getX() + SIDE / 2, center.getY() + SIDE / 2, ENTRY_HEIGHT - 0.1)); + } + + protected double fuelHitNet(Fuel fuel) { + if (fuel.pos.getZ() > NET_HEIGHT_MAX || fuel.pos.getZ() < NET_HEIGHT_MIN) return 0; + if (fuel.pos.getY() > center.getY() + NET_WIDTH / 2 + || fuel.pos.getY() < center.getY() - NET_WIDTH / 2) return 0; + if (fuel.pos.getX() > center.getX() + NET_OFFSET * exitVelXMult) { + return Math.max( + 0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() - FUEL_RADIUS)); + } else { + return Math.min( + 0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() + FUEL_RADIUS)); + } + } + } + + protected class SimIntake { + double xMin, xMax, yMin, yMax; + BooleanSupplier ableToIntake; + Runnable callback; + + protected SimIntake( + double xMin, + double xMax, + double yMin, + double yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + this.xMin = xMin; + this.xMax = xMax; + this.yMin = yMin; + this.yMax = yMax; + this.ableToIntake = ableToIntake; + this.callback = intakeCallback; + } + + protected boolean shouldIntake(Fuel fuel, Pose2d robotPose) { + if (!ableToIntake.getAsBoolean() || fuel.pos.getZ() > bumperHeight) return false; + + Translation2d fuelRelativePos = + new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) + .relativeTo(robotPose) + .getTranslation(); + + boolean result = + fuelRelativePos.getX() >= xMin + && fuelRelativePos.getX() <= xMax + && fuelRelativePos.getY() >= yMin + && fuelRelativePos.getY() <= yMax; + if (result) { + callback.run(); + } + return result; + } + } }