From 09da060a431cdb4c5fb5f85e89da79fbd3dcffcc Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Mon, 23 Feb 2026 20:53:45 -0500 Subject: [PATCH 1/2] Revert "Merge branch 'main' into R2-137-create-an-elevator-current-limit" This reverts commit e029f1955c847d17a2eeba26fb0fb0b2f6f3c1d6, reversing changes made to 7a683ac829aee7261fe156ab2d6ee925ba6d0909. reverting merge --- src/main/java/frc/robot/Constants.java | 15 +- src/main/java/frc/robot/Robot.java | 39 +- src/main/java/frc/robot/RobotContainer.java | 63 +- .../java/frc/robot/subsystems/Climber.java | 30 +- .../java/frc/robot/subsystems/Intake.java | 69 +- .../java/frc/robot/subsystems/Shooter.java | 22 +- .../frc/robot/subsystems/drive/Drive.java | 2 +- src/main/java/frc/robot/util/FuelSim.java | 941 ------------------ 8 files changed, 59 insertions(+), 1122 deletions(-) delete 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 f4b67c3..05b67e2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -162,7 +162,6 @@ public class HopperConstants { public static final String MOTOR_NAME = "Hopper Roller"; // CHANGE TO PROPER RPMS !!!! - public static final double SLOW_SPEED_RPM = 0.0; public static final double FAST_SPEED_RPM = 0.0; public static final double REVERSE_SPEED_RPM = 0.0; @@ -342,8 +341,8 @@ public class ShooterRotaryConstants { public static final Translation3d OFFSET = Translation3d.kZero; - public static final Angle MIN_ANGLE = Degrees.of(0.0); - public static final Angle MAX_ANGLE = Rotations.of(45.0); + public static final Angle MIN_ANGLE = Degrees.of(-130.0); + public static final Angle MAX_ANGLE = Rotations.of(.5); public static final Angle STARTING_ANGLE = Rotations.of(0.0); public static final Distance ARM_LENGTH = Meters.of(1.0); @@ -434,12 +433,11 @@ public static CANcoderConfiguration getCANcoderConfig(boolean sim) { public static final int CANDLE_ID = 50; - public class IntakeFlywheelConstants { + public class IntakeConstants { // Constants for Intake public static final Angle MIN_ANGLE = Rotations.of(0.0); public static final Angle MAX_ANGLE = Rotations.of(1); public static final Angle STARTING_ANGLE = Rotations.of(0.0); - public static final double PICKUP_SPEED = 0.0; public static final Distance WHEEL_RADIUS = Meters.of(0.05); public static final Translation3d OFFSET = Translation3d.kZero; public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125); @@ -571,9 +569,6 @@ public static TalonFXConfiguration getFXConfig() { public class IntakePivotConstants { public static final String NAME = "Intake"; - public static final Angle PICKUP_ANGLE = Rotations.of(0.0); - public static final Angle STOW_ANGLE = Rotations.of(0.0); - public static final Angle TOLERANCE = Degrees.of(1.0); public static final AngularVelocity CRUISE_VELOCITY = RadiansPerSecond.of(10); @@ -584,8 +579,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(0.0); - public static final Angle MAX_ANGLE = Degrees.of(130.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 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 c10093b..e0f36af 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,26 +13,15 @@ package frc.robot; -<<<<<<< R2-137-create-an-elevator-current-limit -======= -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; - ->>>>>>> main import au.grapplerobotics.CanBridge; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; -<<<<<<< R2-137-create-an-elevator-current-limit import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotController; -======= -import edu.wpi.first.math.geometry.Pose3d; ->>>>>>> main 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 org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -50,11 +39,7 @@ public class Robot extends LoggedRobot { private Command autonomousCommand; private RobotContainer robotContainer; -<<<<<<< R2-137-create-an-elevator-current-limit private Alert lowBatteryAlert = new Alert("The robot is low on battery!", AlertType.kWarning); -======= - public FuelSim fuelSim = new FuelSim(); ->>>>>>> main public Robot() { CanBridge.runTCP(); @@ -195,29 +180,9 @@ public void testPeriodic() {} /** This function is called once when the robot is first started up. */ @Override - 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 - } + public void simulationInit() {} /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() { - fuelSim.updateSim(); - - Logger.recordOutput("Zero Pose", new Pose3d()); - } + public void simulationPeriodic() {} } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5880c51..2ff6a6e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,11 +23,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -<<<<<<< R2-137-create-an-elevator-current-limit import frc.lib.W8.io.absoluteencoder.AbsoluteEncoderIOCANCoder; import frc.lib.W8.io.absoluteencoder.AbsoluteEncoderIOCANCoderSim; -======= ->>>>>>> main import frc.lib.W8.io.motor.*; import frc.lib.W8.io.vision.VisionIOPhotonVision; import frc.lib.W8.io.vision.VisionIOPhotonVisionSim; @@ -36,9 +33,8 @@ import frc.lib.W8.mechanisms.rotary.RotaryMechanismReal; import frc.lib.W8.mechanisms.rotary.RotaryMechanismSim; import frc.robot.Constants.HopperConstants; -import frc.robot.Constants.IntakeFlywheelConstants; -import frc.robot.Constants.IntakeFlywheelConstants.VisionConstants; -import frc.robot.Constants.IntakePivotConstants; +import frc.robot.Constants.IntakeConstants; +import frc.robot.Constants.IntakeConstants.VisionConstants; import frc.robot.Constants.Ports; import frc.robot.Constants.ShooterFlywheelConstants; import frc.robot.Constants.ShooterRotaryConstants; @@ -68,11 +64,7 @@ public class RobotContainer { // Subsystems -<<<<<<< R2-137-create-an-elevator-current-limit private final Drive drive; -======= - public final Drive drive; ->>>>>>> main private final Hopper hopper; private final Shooter shooter; private final Intake intake; @@ -122,8 +114,8 @@ public RobotContainer() { Ports.ShooterRoller)), new RotaryMechanismReal( new MotorIOTalonFX( - ShooterRotaryConstants.NAME, - ShooterRotaryConstants.getFXConfig(), + ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.getFXConfig(), Ports.ShooterRoller), Constants.ShooterRotaryConstants.CONSTANTS, java.util.Optional.empty())); @@ -132,25 +124,20 @@ public RobotContainer() { new Intake( new FlywheelMechanismReal( new MotorIOTalonFX( - IntakeFlywheelConstants.MOTOR_NAME, - IntakeFlywheelConstants.getFXConfig(), + IntakeConstants.MOTOR_NAME, + IntakeConstants.getFXConfig(), Ports.IntakeRoller)), new RotaryMechanismReal( new MotorIOTalonFX( - IntakePivotConstants.NAME, - IntakePivotConstants.getFXConfig(), + IntakeConstants.MOTOR_NAME, + IntakeConstants.getFXConfig(), Ports.IntakeRoller), -<<<<<<< R2-137-create-an-elevator-current-limit IntakeConstants.CONSTANTS, Optional.of( new AbsoluteEncoderIOCANCoder( Ports.IntakeRoller, IntakeConstants.MOTOR_NAME + " Encoder", IntakeConstants.getCANcoderConfig(false))))); -======= - IntakePivotConstants.CONSTANTS, - Optional.empty())); ->>>>>>> main vision = new Vision( new VisionIOPhotonVision( @@ -200,8 +187,8 @@ public RobotContainer() { ShooterFlywheelConstants.TOLERANCE), new RotaryMechanismSim( new MotorIOTalonFXSim( - ShooterRotaryConstants.NAME, - ShooterRotaryConstants.getFXConfig(), + ShooterFlywheelConstants.NAME, + ShooterFlywheelConstants.getFXConfig(), Ports.ShooterRoller), ShooterRotaryConstants.DCMOTOR, ShooterRotaryConstants.MOI, @@ -213,22 +200,26 @@ public RobotContainer() { new Intake( new FlywheelMechanismSim( new MotorIOTalonFXSim( - IntakeFlywheelConstants.MOTOR_NAME, - IntakeFlywheelConstants.getFXConfig(), + IntakeConstants.MOTOR_NAME, + IntakeConstants.getFXConfig(), Ports.IntakeRoller), - IntakeFlywheelConstants.DCMOTOR, - IntakeFlywheelConstants.MOI, - IntakeFlywheelConstants.TOLERANCE), + IntakeConstants.DCMOTOR, + IntakeConstants.MOI, + IntakeConstants.TOLERANCE), new RotaryMechanismSim( new MotorIOTalonFXSim( - IntakePivotConstants.NAME, - IntakePivotConstants.getFXConfig(), + IntakeConstants.MOTOR_NAME, + IntakeConstants.getFXConfig(), Ports.IntakeRoller), - IntakePivotConstants.DCMOTOR, - IntakePivotConstants.MOI, + IntakeConstants.DCMOTOR, + IntakeConstants.MOI, false, - IntakePivotConstants.CONSTANTS, - Optional.empty())); + IntakeConstants.CONSTANTS, + Optional.of( + new AbsoluteEncoderIOCANCoderSim( + Ports.IntakeRoller, + IntakeConstants.MOTOR_NAME + " Encoder", + IntakeConstants.getCANcoderConfig(false))))); vision = new Vision( new VisionIOPhotonVisionSim( @@ -261,7 +252,7 @@ public RobotContainer() { intake = new Intake( new FlywheelMechanism() {}, - new RotaryMechanism(IntakePivotConstants.NAME, IntakePivotConstants.CONSTANTS) {}); + new RotaryMechanism(IntakeConstants.MOTOR_NAME, IntakeConstants.CONSTANTS) {}); vision = new Vision(null); break; } @@ -331,8 +322,6 @@ 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 3bc62da..233de05 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -2,28 +2,12 @@ import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Volts; -<<<<<<< R2-137-create-an-elevator-current-limit import edu.wpi.first.units.measure.Distance; -======= -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; ->>>>>>> main import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.W8.mechanisms.linear.LinearMechanism; import frc.robot.Constants.ClimberConstants; -<<<<<<< R2-137-create-an-elevator-current-limit import frc.robot.Constants.ElevatorConstants; -======= -import org.littletonrobotics.junction.Logger; ->>>>>>> main public class Climber extends SubsystemBase { private LinearMechanism _io; @@ -53,19 +37,7 @@ public boolean isAboveCurrentLimit() { } @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))); - - _io.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp()) * 0.25)); - } + public void periodic() {} 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 672bf1b..8b962c7 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -1,26 +1,23 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -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.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; 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.IntakeFlywheelConstants; +import frc.robot.Constants; +import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.IntakePivotConstants; -import org.littletonrobotics.junction.Logger; public class Intake extends SubsystemBase { private FlywheelMechanism _rollerIO; private RotaryMechanism _pivotIO; + double velocity; + double pivotAngle; public double desiredAngle; public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { @@ -32,7 +29,7 @@ public Intake(FlywheelMechanism rollerIO, RotaryMechanism pivotIO) { public void setVelocity(double velocity) { AngularVelocity angVelo = RotationsPerSecond.of(velocity); - _rollerIO.runVelocity(angVelo, IntakeFlywheelConstants.ACCELERATION, PIDSlot.SLOT_0); + _rollerIO.runVelocity(angVelo, Constants.IntakeConstants.ACCELERATION, PIDSlot.SLOT_0); } public Command setPivotAngle(Angle pivotAngle) { @@ -40,9 +37,9 @@ public Command setPivotAngle(Angle pivotAngle) { () -> _pivotIO.runPosition( pivotAngle, - IntakeFlywheelConstants.CRUISE_VELOCITY, - IntakeFlywheelConstants.ACCELERATION, - IntakeFlywheelConstants.JERK, + IntakeConstants.CRUISE_VELOCITY, + IntakeConstants.ACCELERATION, + IntakeConstants.JERK, PIDSlot.SLOT_0)); // .withName("Go To " + setpoint.toString() + " Setpoint"); } @@ -59,10 +56,19 @@ public void stop() { setVelocity(0); } - public Command intake() { - return Commands.sequence( - Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), - setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); + // public Command intake() { + // return Commands.sequence( + // Commands.run(() -> setVelocity(velocity)), Commands.run(() -> + // setPivotAngle(pivotAngle))); + // } + public void setAngle(Angle angle) { + _pivotIO.runPosition( + angle, + getVelocity(), + IntakePivotConstants.ACCELERATION, + IntakePivotConstants.JERK, + PIDSlot.SLOT_0); + desiredAngle = angle.magnitude(); } public boolean isIntendedAngle() { @@ -70,37 +76,6 @@ public boolean isIntendedAngle() { <= IntakePivotConstants.TOLERANCE.magnitude(); } - public Command stowAndStopRollers() { - return Commands.sequence( - Commands.run(() -> setVelocity(IntakeFlywheelConstants.PICKUP_SPEED)), - setStowAngle(IntakePivotConstants.STOW_ANGLE)); - } - - private Command setStowAngle(Angle stowAngle) { - return this.runOnce( - () -> - _pivotIO.runPosition( - stowAngle, - IntakePivotConstants.CRUISE_VELOCITY, - IntakePivotConstants.ACCELERATION, - IntakePivotConstants.JERK, - PIDSlot.SLOT_0)); - } - @Override - 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 - } + public void periodic() {} } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 5f89310..629e188 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -2,16 +2,11 @@ import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.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; @@ -21,7 +16,6 @@ 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 { @@ -126,18 +120,6 @@ public Command shoot(double velocity) { Commands.runOnce(() -> setFlywheelVelocity(0))); } - 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)); - } + @Override + public void periodic() {} } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index a905aef..fbc3110 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") - public ChassisSpeeds getChassisSpeeds() { + private 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 deleted file mode 100644 index 28de21f..0000000 --- a/src/main/java/frc/robot/util/FuelSim.java +++ /dev/null @@ -1,941 +0,0 @@ -// 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 e859b93b80515fc9bad96f0a1a0c1b452cfcc1e6 Mon Sep 17 00:00:00 2001 From: rhit-collinkn Date: Sat, 28 Feb 2026 13:35:32 -0500 Subject: [PATCH 2/2] put changes back --- src/main/java/frc/robot/subsystems/climber/Climber.java | 8 ++++++++ src/main/java/frc/robot/subsystems/drive/Drive.java | 4 ++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 07f4a55..80b1cff 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -24,6 +24,14 @@ private State(LinearVelocity velocity) { this.velocity = velocity; } } + public boolean isAboveCurrentLimit() { + if (_io.getSupplyCurrent().in(Units.Amps) > ClimberConstants.HARD_STOP_CURRENT_LIMIT) { + return true; + } + else { + return false; + } + } @Override public void periodic() {} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index fbc3110..5a79971 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()); } @@ -365,4 +365,4 @@ public static Translation2d[] getModuleTranslations() { new Translation2d(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } -} +} \ No newline at end of file