diff --git a/comp-bot/src/main/java/frc/robot/Robot.java b/comp-bot/src/main/java/frc/robot/Robot.java index a4998dbc8..6b8c3443a 100644 --- a/comp-bot/src/main/java/frc/robot/Robot.java +++ b/comp-bot/src/main/java/frc/robot/Robot.java @@ -1,9 +1,15 @@ package frc.robot; +import static edu.wpi.first.units.Units.Meter; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Radian; + import com.team581.Base581Robot; import com.team581.GlobalConfig; import com.team581.controller.ControllerBindings; +import com.team581.math.MathHelpers; import com.team581.math.PoseErrorTolerance; +import com.team581.simkit.FuelSim; import com.team581.trailblazer.Trailblazer; import com.team581.trailblazer.followers.PidPathFollower; import com.team581.trailblazer.trackers.HeuristicPathTracker; @@ -11,8 +17,13 @@ import com.team581.util.FmsUtil; import dev.doglog.DogLog; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.autos.Autos; +import frc.robot.autos.BumpCrossingFollower; import frc.robot.cluster_map.ClusterMap; import frc.robot.config.FeatureFlags; import frc.robot.conveyor.Conveyor; @@ -41,16 +52,6 @@ public class Robot extends Base581Robot { private final Hardware hardware = new Hardware(); - private final Trailblazer trailblazer = - new Trailblazer( - new HeuristicPathTracker(new PoseErrorTolerance(0.5, 10)), - new PidPathFollower( - new PIDController(3.5, 0, 0), - new PIDController( - Swerve.ORIGINAL_HEADING_PID.getP(), - Swerve.ORIGINAL_HEADING_PID.getI(), - Swerve.ORIGINAL_HEADING_PID.getD()))); - private final Limelight shooterLimelight = new Limelight("shooter", LimelightState.TAGS, CameraConfigs.SHOOTER); private final Limelight leftLimelight = @@ -61,9 +62,25 @@ public class Robot extends Base581Robot { new Limelight("ground", LimelightState.CLUSTER_MAP, CameraConfigs.GROUND); private final HealthManager health = new HealthManager(shooterLimelight, leftLimelight, rightLimelight, groundLimelight); + + private final Imu imu = new Imu(hardware.drivetrain); + + private final Trailblazer trailblazer = + new Trailblazer( + new HeuristicPathTracker(new PoseErrorTolerance(0.5, 10)), + new BumpCrossingFollower( + new PidPathFollower( + new PIDController(3.5, 0, 0), + new PIDController( + Swerve.ORIGINAL_HEADING_PID.getP(), + Swerve.ORIGINAL_HEADING_PID.getI(), + Swerve.ORIGINAL_HEADING_PID.getD())), + imu.bumpCrossingTracker)); + private final Swerve swerve = new Swerve(hardware.drivetrain, health, hardware.driverController, trailblazer); - private final Imu imu = new Imu(swerve.drivetrain); + + public FuelSim fuelSim = new FuelSim("FuelSim"); private final ShooterHood shooterHood = new ShooterHood(hardware.shooterHoodMotor); @@ -138,6 +155,12 @@ public Robot() { throw new RuntimeException("Failed to write field obstacles SVG", e); } } + + if (RobotBase.isSimulation()) { + initFuelSim(); + } + + FieldUtil.debugLogFieldZones(); } @Override @@ -151,6 +174,58 @@ public void robotPeriodic() { } } + @Override + public void simulationPeriodic() { + if (hopperManager.isShooting()) { + fuelSim.launchFuel( + LinearVelocity.ofBaseUnits( + MathHelpers.rpmToLinearVelocity(shooter.getAverageRPM(), Units.inchesToMeters(1.6)), + MetersPerSecond), + Angle.ofBaseUnits(Math.toRadians(90 - shooterHood.getAngle()), Radian), + Angle.ofBaseUnits(Math.PI, Radian), + Distance.ofBaseUnits( + Units.inchesToMeters(20), + Meter)); // Spawns a fuel onto the field at the robot's position with a specified + // launch velocity and angles, accounting for robot movement (robot must be + // registered) + } + fuelSim.updateSim(); + } + + private void initFuelSim() { + fuelSim.setMaxAdditions(100); + + // Register a robot for collision with fuel + fuelSim.registerRobot( + Units.inchesToMeters(34.5), // from left to right in meters + Units.inchesToMeters(33), // from front to back in meters + Units.inchesToMeters(4.5), // from floor to top of bumpers in meters + Units.inchesToMeters(21), + Units.inchesToMeters(-12), + Units.inchesToMeters(24.5), + () -> localization.getPose(), // Supplier of robot pose + () -> swerve.getFieldRelativeSpeeds()); // Supplier of field-centric chassis + // speeds + + // Register an intake to remove fuel from the field as a rectangular bounding box + fuelSim.registerIntake( + Units.inchesToMeters(16.5), + Units.inchesToMeters(25), + -Units.inchesToMeters(17.25), + Units.inchesToMeters(17.25), // robot-centric coordinates for bounding box in meters + () -> { + return hopperManager.isIntaking(); + }); // (optional) Runnable called whenever a fuel is intaked + + fuelSim.setSubticks( + 5); // sets the number of physics iterations to perform per 20ms loop. Default = 5 + + 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 + } + @Override protected void configureBindings() { var driver = diff --git a/comp-bot/src/main/java/frc/robot/autos/BumpCrossingFollower.java b/comp-bot/src/main/java/frc/robot/autos/BumpCrossingFollower.java new file mode 100644 index 000000000..77741b484 --- /dev/null +++ b/comp-bot/src/main/java/frc/robot/autos/BumpCrossingFollower.java @@ -0,0 +1,72 @@ +package frc.robot.autos; + +import com.team581.mechanisms.imu.BumpCrossingState; +import com.team581.mechanisms.imu.BumpCrossingTracker; +import com.team581.trailblazer.AutoPoint; +import com.team581.trailblazer.followers.PathFollower; +import com.team581.trailblazer.segments.AutoSegment; +import com.team581.util.FmsUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +/** + * Wraps a {@link PathFollower} but injects custom velocities to ensure consistent bump crossing + * behavior. + */ +public class BumpCrossingFollower implements PathFollower { + private static final double CROSSING_UPHILL_LINEAR_VELOCITY = 3; + private static final double CROSSING_DOWNHILL_LINEAR_VELOCITY = 1; + + private final PathFollower follower; + + // TODO: Actually use this + @SuppressWarnings("unused") + private final BumpCrossingTracker bumpCrossingTracker; + + public BumpCrossingFollower(PathFollower follower, BumpCrossingTracker bumpCrossingTracker) { + this.follower = follower; + this.bumpCrossingTracker = bumpCrossingTracker; + } + + @Override + public ChassisSpeeds calculateSpeeds( + ChassisSpeeds currentSpeeds, + Pose2d currentPose, + Pose2d targetPose, + AutoPoint currentPoint, + AutoSegment segment, + int currentPointIndex) { + var originalSpeeds = + follower.calculateSpeeds( + currentSpeeds, currentPose, targetPose, currentPoint, segment, currentPointIndex); + + var state = resolveState(); + + if (state == BumpCrossingState.NOT_ON_BUMP) { + return originalSpeeds; + } + + var unsignedXVelocity = + state == BumpCrossingState.CROSSING_UPHILL + ? CROSSING_UPHILL_LINEAR_VELOCITY + : CROSSING_DOWNHILL_LINEAR_VELOCITY; + + var xVelocity = FmsUtil.isRedAlliance() ? unsignedXVelocity : -unsignedXVelocity; + + // We preserve the Y and angular speeds to ensure we stay centered with the bump and maintain + // the right heading + return new ChassisSpeeds( + xVelocity, originalSpeeds.vyMetersPerSecond, originalSpeeds.omegaRadiansPerSecond); + } + + @Override + public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) { + follower.reset(currentPose, currentSpeeds); + } + + // TODO: We need to decide what state of crossing the bump (if any) we're in via + // BumpCrossingTracker + private BumpCrossingState resolveState() { + return BumpCrossingState.NOT_ON_BUMP; + } +} diff --git a/comp-bot/src/main/java/frc/robot/cluster_map/ClusterMap.java b/comp-bot/src/main/java/frc/robot/cluster_map/ClusterMap.java index 80cf02c01..0d0c64940 100644 --- a/comp-bot/src/main/java/frc/robot/cluster_map/ClusterMap.java +++ b/comp-bot/src/main/java/frc/robot/cluster_map/ClusterMap.java @@ -1,5 +1,8 @@ package frc.robot.cluster_map; +import static com.google.common.collect.ImmutableMap.toImmutableMap; +import static java.util.Map.Entry.comparingByValue; + import com.team581.GlobalConfig; import com.team581.math.GamePieceDetectionCalculator; import com.team581.math.MathHelpers; @@ -25,6 +28,7 @@ import frc.robot.vision.limelight.LimelightState; import java.util.ArrayList; import java.util.Arrays; +import java.util.Map; import java.util.Optional; public class ClusterMap extends StateMachineSubsystem { @@ -94,40 +98,27 @@ public ClusterMap(Localization localization, Swerve swerve, Limelight limelight) this.limelight = limelight; } + /** Returns the lane with the most detected balls, ignoring the trench. */ public Lane getBestClusterLane() { - if (clusterMap.isEmpty()) { - return Lane.NONE; - } - var robotPose = localization.getPose(); - int[] ballsPerLane = new int[Lane.values().length]; - - for (ClusterMapElement element : clusterMap) { - Pose2d elementPose = new Pose2d(element.clusterTranslation(), Rotation2d.kZero); - Lane lane = laneSystem.getLane(elementPose, robotPose); - - if (lane != Lane.NONE && lane != Lane.TRENCH) { - ballsPerLane[lane.ordinal()] = - (int) (ballsPerLane[lane.ordinal()] + element.detectionSize()); - } - } - - Lane bestLane = Lane.NONE; - int maxBalls = 0; - - // Find the lane with the highest count - for (Lane lane : Lane.values()) { - if (lane == Lane.NONE || lane == Lane.TRENCH) continue; - - int count = ballsPerLane[lane.ordinal()]; - if (count > maxBalls) { - maxBalls = count; - bestLane = lane; - } - } - - return bestLane; + return clusterMap.stream() + // Sum up total detected balls per lane + .collect( + toImmutableMap( + element -> + laneSystem.getLane( + new Pose2d(element.clusterTranslation(), Rotation2d.kZero), robotPose), + element -> (int) element.detectionSize(), + Integer::sum)) + .entrySet() + .stream() + // Only consider actual scoring lanes + .filter(entry -> entry.getKey() != Lane.NONE && entry.getKey() != Lane.TRENCH) + // Pick the lane with the highest ball count + .max(comparingByValue()) + .map(Map.Entry::getKey) + .orElse(Lane.NONE); } public Optional getBestClusterPose() { diff --git a/comp-bot/src/main/java/frc/robot/config/RobotKind.java b/comp-bot/src/main/java/frc/robot/config/RobotKind.java index 7d2c29c88..284fdba1d 100644 --- a/comp-bot/src/main/java/frc/robot/config/RobotKind.java +++ b/comp-bot/src/main/java/frc/robot/config/RobotKind.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.RobotController; public enum RobotKind { + PRACTICE_BOT, COMP_BOT; public static final boolean IS_COMP_BOT = fromSerialNumber() == COMP_BOT; @@ -10,6 +11,7 @@ public enum RobotKind { /** Returns the RobotKind by matching the serial number to a known RobotKind. */ public static RobotKind fromSerialNumber() { return switch (RobotController.getSerialNumber()) { + case "0322443D" -> PRACTICE_BOT; default -> COMP_BOT; }; } diff --git a/comp-bot/src/main/java/frc/robot/deploy/Deploy.java b/comp-bot/src/main/java/frc/robot/deploy/Deploy.java index 14abf32f9..ec19b75b5 100644 --- a/comp-bot/src/main/java/frc/robot/deploy/Deploy.java +++ b/comp-bot/src/main/java/frc/robot/deploy/Deploy.java @@ -136,7 +136,7 @@ protected void afterTransition(DeployState newState) { differentialMechanism.setControl( voltageRequest.withOutput(DeployConfig.HOMING_VOLTAGE_OUTWARD), voltageDifferentialRequest.withOutput(0)); - case HOPPER_COMPACTION_IN, HOPPER_COMPACTION_WAITING -> { + case SCORE_COMPACTION, SCORE_COMPACTION_WAITING -> { differentialMechanism.setControl( motionMagicTorqueCurrentFOCAverageRequest.withPosition(clamp(newState.getLength())), positionDifferentialRequest.withPosition(0.0)); @@ -167,14 +167,21 @@ public boolean atGoal() { public void waitHopperCompactionRequest() { switch (getState()) { case UNHOMED, HOME_INWARD, HOME_OUTWARD -> {} - default -> setStateFromRequest(DeployState.HOPPER_COMPACTION_WAITING); + default -> setStateFromRequest(DeployState.SCORE_COMPACTION_WAITING); + } + } + + public void feedCompactionRequest() { + switch (getState()) { + case UNHOMED, HOME_INWARD, HOME_OUTWARD -> {} + default -> setStateFromRequest(DeployState.FEED_COMPACTION); } } public void hopperCompactionRequest() { switch (getState()) { case UNHOMED, HOME_INWARD, HOME_OUTWARD -> {} - default -> setStateFromRequest(DeployState.HOPPER_COMPACTION_IN); + default -> setStateFromRequest(DeployState.SCORE_COMPACTION); } } diff --git a/comp-bot/src/main/java/frc/robot/deploy/DeployConfig.java b/comp-bot/src/main/java/frc/robot/deploy/DeployConfig.java index d4e86dd30..79f76ee03 100644 --- a/comp-bot/src/main/java/frc/robot/deploy/DeployConfig.java +++ b/comp-bot/src/main/java/frc/robot/deploy/DeployConfig.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.configs.TorqueCurrentConfigs; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.robot.config.RobotKind; public class DeployConfig { public static final double MAX_LENGTH = 11.8; @@ -23,7 +24,7 @@ public class DeployConfig { private static final Slot0Configs AVERAGE_GAINS = new Slot0Configs() - .withKP(8.0) + .withKP(RobotKind.IS_COMP_BOT ? 0 : 8.0) .withKI(0) .withKD(0.0) .withKG(0.0) diff --git a/comp-bot/src/main/java/frc/robot/deploy/DeployState.java b/comp-bot/src/main/java/frc/robot/deploy/DeployState.java index e403544bc..dbd41ff55 100644 --- a/comp-bot/src/main/java/frc/robot/deploy/DeployState.java +++ b/comp-bot/src/main/java/frc/robot/deploy/DeployState.java @@ -7,8 +7,10 @@ public enum DeployState { INTAKE(DeployConfig.MAX_LENGTH), STOW(1.0), // placeholder values - HOPPER_COMPACTION_WAITING(DeployConfig.MAX_LENGTH - 3.5), - HOPPER_COMPACTION_IN(1.0), + SCORE_COMPACTION_WAITING(DeployConfig.MAX_LENGTH - 3.5), + SCORE_COMPACTION(1.0), + FEED_COMPACTION(5.915), + SUPER_COMPACTION_IN(1.0), HOME_INWARD(DeployConfig.MIN_LENGTH), diff --git a/comp-bot/src/main/java/frc/robot/feeder/Feeder.java b/comp-bot/src/main/java/frc/robot/feeder/Feeder.java index 7a62cb979..c8418c484 100644 --- a/comp-bot/src/main/java/frc/robot/feeder/Feeder.java +++ b/comp-bot/src/main/java/frc/robot/feeder/Feeder.java @@ -3,6 +3,7 @@ import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.team581.math.MathHelpers; import com.team581.mechanisms.PowerManaged; import com.team581.util.state_machines.StateMachineSubsystem; import dev.doglog.DogLog; @@ -15,6 +16,8 @@ public class Feeder extends StateMachineSubsystem implements PowerM private final NeutralOut neutralRequest = new NeutralOut(); private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(true); + private double averageCurrent = 0.0; + public Feeder(TalonFX topMotor, TalonFX bottomMotor) { super(SubsystemPriority.FEEDER, FeederState.IDLE); topMotor.getConfigurator().apply(FeederConfig.TOP_MOTOR_CONFIG); @@ -62,6 +65,15 @@ protected void collectInputs() { DogLog.log("Feeder/Top/VelocityRPM", topMotor.getVelocity().getValueAsDouble() * 60.0); DogLog.log("Feeder/Bottom/VelocityRPM", bottomMotor.getVelocity().getValueAsDouble() * 60.0); DogLog.log("Feeder/Voltage", getState().getVoltage()); + + averageCurrent = + MathHelpers.average( + topMotor.getStatorCurrent().getValueAsDouble(), + bottomMotor.getStatorCurrent().getValueAsDouble()); + } + + public double getAverageCurrent() { + return averageCurrent; } @Override diff --git a/comp-bot/src/main/java/frc/robot/generated/CompTunerConstants.java b/comp-bot/src/main/java/frc/robot/generated/CompTunerConstants.java index e0a7f9509..1b3b81b12 100644 --- a/comp-bot/src/main/java/frc/robot/generated/CompTunerConstants.java +++ b/comp-bot/src/main/java/frc/robot/generated/CompTunerConstants.java @@ -84,7 +84,8 @@ public class CompTunerConstants { // This may need to be tuned to your individual robot private static final double kCoupleRatio = 5.4; - private static final double kDriveGearRatio = 6.821052631578947; + // Swerve X2c X1 with 11T pinion + private static final double kDriveGearRatio = 6.98; private static final double kSteerGearRatio = 12.1; private static final Distance kWheelRadius = Inches.of(2); diff --git a/comp-bot/src/main/java/frc/robot/robot_manager/RobotManager.java b/comp-bot/src/main/java/frc/robot/robot_manager/RobotManager.java index 96abf4891..7ab6ac6ab 100644 --- a/comp-bot/src/main/java/frc/robot/robot_manager/RobotManager.java +++ b/comp-bot/src/main/java/frc/robot/robot_manager/RobotManager.java @@ -386,13 +386,13 @@ protected void whileInState(RobotState state) { case SCORE -> { shooterHood.scoreRequest(scoringParameters.distance()); swerve.scoreRequest(scoringParameters); - hopperManager.shootRequest(hubActivity.shouldSuperScore()); + hopperManager.scoreRequest(hubActivity.shouldSuperScore()); smartScoringPowerManagerRequest(); } case FORCE_SCORE -> { shooter.scoreRequest(scoringParameters.distance()); shooterHood.scoreRequest(scoringParameters.distance()); - hopperManager.shootRequest(hubActivity.shouldSuperScore()); + hopperManager.scoreRequest(hubActivity.shouldSuperScore()); } case PREPARE_FEED -> { smartHoodPrepareFeedRequest(); @@ -402,7 +402,7 @@ protected void whileInState(RobotState state) { case FEED -> { shooterHood.feedRequest(feedingParameters.distance()); swerve.feedRequest(feedingParameters); - hopperManager.shootRequest(); + hopperManager.feedRequest(); } // Fallback states @@ -419,14 +419,14 @@ protected void whileInState(RobotState state) { // Automatically update scoring parameters with preset pose shooterHood.scoreRequest(scoringParameters.distance()); swerve.scoreRequest(scoringParameters); - hopperManager.shootRequest(hubActivity.shouldSuperScore()); + hopperManager.scoreRequest(hubActivity.shouldSuperScore()); } case PREPARE_FALLBACK_FEED -> { swerve.feedRequest(fallbackFeedingParameters); } case FALLBACK_FEED -> { swerve.feedRequest(fallbackFeedingParameters); - hopperManager.shootRequest(); + hopperManager.feedRequest(); } case WARMUP_SCORE -> { shooter.prepareScoreRequest(scoringParameters.distance()); @@ -662,6 +662,7 @@ protected void collectInputs() { || !FieldUtil.isFeedPathObstructed( robotPose.getTranslation(), feedLocation.getTranslation()); + shooter.updateHopperState(hopperManager.feeder.getAverageCurrent(), hopperManager.isFull()); DogLog.log("RobotManager/Scoring/IsInSafeScoringLocation", isInSafeScoringLocation); DogLog.log("RobotManager/Feeding/IsInSafeFeedingLocation", isInSafeFeedingLocation); } diff --git a/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperManager.java b/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperManager.java index f24302af7..9756616df 100644 --- a/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperManager.java +++ b/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperManager.java @@ -35,6 +35,7 @@ public class HopperManager extends StateMachineSubsystem { private boolean driverWantsEject = false; private boolean operatorWantsStow = false; private boolean towerSensorRaw = false; + private boolean ballFilling = false; private boolean shouldSuperScore = false; @@ -48,6 +49,12 @@ public class HopperManager extends StateMachineSubsystem { private HopperCapacity hopperCapacity = HopperCapacity.LOW; + public enum HopperBallPosition { + CLOSE_TO_SHOOTER, + AT_SENSOR, + BELOW_SENSOR, + } + private final Timer canRangeUpdateTimer = new Timer(); public HopperManager( @@ -69,6 +76,27 @@ public HopperManager( canRangeUpdateTimer.start(); } + private HopperBallPosition getShotPosition() { + if (towerSensorDebounced) { + if (ballFilling) { + return HopperBallPosition.AT_SENSOR; + } + + return HopperBallPosition.CLOSE_TO_SHOOTER; + } + + return HopperBallPosition.BELOW_SENSOR; + } + + public double getFeederToShooterTime() { + return switch (getShotPosition()) { + // TODO: Validate this + case CLOSE_TO_SHOOTER -> 0.25; + case AT_SENSOR -> 0.2; + case BELOW_SENSOR -> 0.3; + }; + } + @Override protected HopperState getNextState(HopperState currentState) { return switch (getState()) { @@ -160,17 +188,33 @@ protected void afterTransition(HopperState newState) { conveyor.shootRequest(); feeder.shootRequest(); } - case SHOOT -> { + case SCORE -> { // Don't move deploy back to intake if it's already compacting from a previous SHOOT cycle - if (deploy.getState() != DeployState.HOPPER_COMPACTION_IN - && deploy.getState() != DeployState.HOPPER_COMPACTION_WAITING) { + if (deploy.getState() != DeployState.SCORE_COMPACTION + && deploy.getState() != DeployState.SCORE_COMPACTION_WAITING) { deploy.intakeRequest(); } intake.shootRequest(); conveyor.initialShotRequest(); feeder.shootRequest(); } - case SHOOT_AND_INTAKE -> { + case SCORE_AND_INTAKE -> { + deploy.intakeRequest(); + intake.intakeRequest(); + conveyor.shootRequest(); + feeder.shootRequest(); + } + + case FEED -> { + // Don't move deploy back to intake if it's already compacting from a previous SHOOT cycle + if (deploy.getState() != DeployState.FEED_COMPACTION) { + deploy.intakeRequest(); + } + intake.shootRequest(); + conveyor.initialShotRequest(); + feeder.shootRequest(); + } + case FEED_AND_INTAKE -> { deploy.intakeRequest(); intake.intakeRequest(); conveyor.shootRequest(); @@ -192,7 +236,7 @@ protected void whileInState(HopperState state) { switch (state) { default -> {} - case SHOOT -> { + case SCORE -> { if (shouldSuperScore) { deploy.superCompactionRequest(); intake.shootRequest(); @@ -205,6 +249,13 @@ protected void whileInState(HopperState state) { deploy.waitHopperCompactionRequest(); } } + case FEED -> { + if (timeout(HopperManagerConfig.HOPPER_COMPACTION_DELAY.getAsDouble())) { + deploy.feedCompactionRequest(); + intake.idleRequest(); + conveyor.shootRequest(); + } + } } if (previousCanRangeDistance != hopperDistance) { @@ -252,19 +303,35 @@ private HopperState resolveScoreState() { } if (driverWantsIntake) { - return HopperState.SHOOT_AND_INTAKE; + return HopperState.SCORE_AND_INTAKE; + } + + return HopperState.SCORE; + } + + private HopperState resolveFeedState() { + if (driverWantsEject) { + return HopperState.EJECTING; + } + + if (driverWantsIntake) { + return HopperState.FEED_AND_INTAKE; } - return HopperState.SHOOT; + return HopperState.FEED; } - public void shootRequest(boolean shouldSuperScore) { + public void scoreRequest(boolean shouldSuperScore) { this.shouldSuperScore = shouldSuperScore; setState(resolveScoreState()); } - public void shootRequest() { - shootRequest(false); + public void scoreRequest() { + scoreRequest(false); + } + + public void feedRequest() { + setState(resolveFeedState()); } public boolean isIntaking() { @@ -273,7 +340,13 @@ public boolean isIntaking() { public boolean isShooting() { // You need to actually be in a shooting state - if (getState() != HopperState.SHOOT && getState() != HopperState.SHOOT_AND_INTAKE) { + if (getState() != HopperState.SCORE + && getState() != HopperState.SCORE_AND_INTAKE + && getState() != HopperState.FEED + && getState() != HopperState.FEED_AND_INTAKE) { + if (RobotBase.isSimulation()) { + return false; + } return true; } diff --git a/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperManagerConfig.java b/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperManagerConfig.java index 54659628a..4716f231b 100644 --- a/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperManagerConfig.java +++ b/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperManagerConfig.java @@ -11,7 +11,7 @@ public class HopperManagerConfig { public static final CANrangeConfiguration CAN_RANGE_CONFIG = new CANrangeConfiguration() .withFovParams( - new FovParamsConfigs().withFOVRangeX(10.0).withFOVRangeY(10.0).withFOVCenterY(-11.8)) + new FovParamsConfigs().withFOVRangeX(10.0).withFOVRangeY(10.0).withFOVCenterY(-6.0)) .withToFParams(new ToFParamsConfigs().withUpdateMode(UpdateModeValue.ShortRange100Hz)); public static final DoubleSubscriber HOPPER_COMPACTION_DELAY = DogLog.tunable("HopperManager/CompactionDelay", 0.7); diff --git a/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperState.java b/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperState.java index 0d110c47d..d1331d4b1 100644 --- a/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperState.java +++ b/comp-bot/src/main/java/frc/robot/robot_manager/hopper_manager/HopperState.java @@ -6,8 +6,10 @@ public enum HopperState { INTAKING(true), EJECTING(false), UNJAMMING(false), - SHOOT(false), - SHOOT_AND_INTAKE(false); + SCORE(false), + SCORE_AND_INTAKE(false), + FEED(false), + FEED_AND_INTAKE(false); public final boolean canBallFill; diff --git a/comp-bot/src/main/java/frc/robot/shooter/Shooter.java b/comp-bot/src/main/java/frc/robot/shooter/Shooter.java index 77e373d0b..1e7349be2 100644 --- a/comp-bot/src/main/java/frc/robot/shooter/Shooter.java +++ b/comp-bot/src/main/java/frc/robot/shooter/Shooter.java @@ -51,6 +51,9 @@ private static double distanceToFeedingRpm(double distance) { private double topRightMotorRpm = 0; private double bottomLeftMotorRpm = 0; private double bottomRightMotorRpm = 0; + private double feederCurrent = 0.0; + private double feederBasedFeedForward = 0.0; + private boolean hopperFull = false; private boolean atGoal = false; private boolean atGoalDebounced = false; @@ -125,6 +128,11 @@ public void setTurboMode(boolean useTurboMode) { turboMode = useTurboMode; } + public void updateHopperState(double feederCurrent, boolean hopperFull) { + this.feederCurrent = feederCurrent; + this.hopperFull = hopperFull || !DSOptions.USE_CANRANGE.getAsBoolean(); + } + @Override protected void whileInState(ShooterState state) { DogLog.log("Shooter/TopLeft/RPM", topLeftMotorRpm); @@ -133,6 +141,8 @@ protected void whileInState(ShooterState state) { DogLog.log("Shooter/BottomRight/RPM", bottomRightMotorRpm); DogLog.log("Shooter/GoalShootingRPM", shootingRpm); DogLog.log("Shooter/GoalFeedingRPM", feedingRpm); + DogLog.log("Shooter/FeederCurrent", feederCurrent); + DogLog.log("Shooter/FeederBasedFeedForward", feederBasedFeedForward); DogLog.log("Shooter/AtGoal", atGoal()); DogLog.log("Shooter/TopRight/Voltage", topRightMotor.getMotorVoltage().getValueAsDouble()); DogLog.log( @@ -163,39 +173,35 @@ protected void whileInState(ShooterState state) { } case PREPARE_SCORE -> { var setpoint = shootingRpm / 60.0; - topRightMotor.setControl(velocityRequest.withVelocity(setpoint).withFeedForward(0.0)); + topRightMotor.setControl( + velocityRequest.withVelocity(setpoint).withFeedForward(feederBasedFeedForward)); DogLog.log("Shooter/RpmSetpoint", shootingRpm); } case SCORE -> { var setpoint = shootingRpm / 60.0; topRightMotor.setControl( - velocityRequest - .withVelocity(setpoint) - .withFeedForward( - turboMode - ? ShooterConfig.TURBO_MODE_FF_CURRENT.get() - : ShooterConfig.ACTIVE_SHOT_FF_CURRENT.get())); + velocityRequest.withVelocity(setpoint).withFeedForward(feederBasedFeedForward)); DogLog.log("Shooter/RpmSetpoint", shootingRpm); } case PREPARE_FEED -> { var setpoint = feedingRpm / 60.0; - topRightMotor.setControl(velocityRequest.withVelocity(setpoint).withFeedForward(0.0)); + topRightMotor.setControl( + velocityRequest.withVelocity(setpoint).withFeedForward(feederBasedFeedForward)); DogLog.log("Shooter/RpmSetpoint", feedingRpm); } case FEED -> { var setpoint = feedingRpm / 60.0; topRightMotor.setControl( - velocityRequest - .withVelocity(setpoint) - .withFeedForward( - turboMode - ? ShooterConfig.TURBO_MODE_FF_CURRENT.get() - : ShooterConfig.ACTIVE_SHOT_FF_CURRENT.get())); + velocityRequest.withVelocity(setpoint).withFeedForward(feederBasedFeedForward)); DogLog.log("Shooter/RpmSetpoint", feedingRpm); } } } + public double getAverageRPM() { + return (topLeftMotorRpm + topRightMotorRpm + bottomLeftMotorRpm + bottomRightMotorRpm) / 4.0; + } + @Override protected void collectInputs() { shootingRpm = Math.min(ShooterConfig.MAX_SAFE_RPM, distanceToScoringRpm(scoreDistance)); @@ -216,6 +222,29 @@ protected void collectInputs() { atGoal = calculateAtGoal(); atGoalDebounced = atGoalDebouncer.calculate(atGoal); + + switch (getState()) { + case SCORE, FEED -> { + if (!timeout(0.7) && hopperFull) { + feederBasedFeedForward = + Math.max( + ShooterConfig.FEEDER_CURRENT_TO_SHOOTER_FEED_FORWARD.get(feederCurrent), + ShooterConfig.FULL_HOPPER_INITIAL_FF.getAsDouble()); + } else if (!timeout(0.1)) { + feederBasedFeedForward = + Math.max( + ShooterConfig.FEEDER_CURRENT_TO_SHOOTER_FEED_FORWARD.get(feederCurrent), + ShooterConfig.LOW_HOPPER_INITIAL_FFF.getAsDouble()); + } else { + feederBasedFeedForward = + ShooterConfig.FEEDER_CURRENT_TO_SHOOTER_FEED_FORWARD.get(feederCurrent); + } + } + default -> { + feederBasedFeedForward = + ShooterConfig.FEEDER_CURRENT_TO_SHOOTER_FEED_FORWARD.get(feederCurrent); + } + } } public boolean atGoal() { @@ -230,26 +259,12 @@ private boolean calculateAtGoal() { return switch (getState()) { case IDLE -> false; case PREPARE_SCORE -> - MathUtil.isNear(topLeftMotorRpm, shootingRpm, ShooterConfig.RPM_TOLERANCE) - && MathUtil.isNear(topRightMotorRpm, shootingRpm, ShooterConfig.RPM_TOLERANCE) - && MathUtil.isNear(bottomLeftMotorRpm, shootingRpm, ShooterConfig.RPM_TOLERANCE) - && MathUtil.isNear(bottomRightMotorRpm, shootingRpm, ShooterConfig.RPM_TOLERANCE); + MathUtil.isNear(topRightMotorRpm, shootingRpm, ShooterConfig.RPM_TOLERANCE); case SCORE -> MathUtil.isNear( - topLeftMotorRpm, shootingRpm, ShooterConfig.RPM_TOLERANCE_ACTIVELY_SHOOTING) - && MathUtil.isNear( - topRightMotorRpm, shootingRpm, ShooterConfig.RPM_TOLERANCE_ACTIVELY_SHOOTING) - && MathUtil.isNear( - bottomLeftMotorRpm, shootingRpm, ShooterConfig.RPM_TOLERANCE_ACTIVELY_SHOOTING) - && MathUtil.isNear( - bottomRightMotorRpm, shootingRpm, ShooterConfig.RPM_TOLERANCE_ACTIVELY_SHOOTING); + topRightMotorRpm, shootingRpm, ShooterConfig.RPM_TOLERANCE_ACTIVELY_SHOOTING); case PREPARE_FEED, FEED -> - MathUtil.isNear(topLeftMotorRpm, feedingRpm, ShooterConfig.RPM_TOLERANCE_FEEDING) - && MathUtil.isNear(topRightMotorRpm, feedingRpm, ShooterConfig.RPM_TOLERANCE_FEEDING) - && MathUtil.isNear( - bottomLeftMotorRpm, feedingRpm, ShooterConfig.RPM_TOLERANCE_FEEDING) - && MathUtil.isNear( - bottomRightMotorRpm, feedingRpm, ShooterConfig.RPM_TOLERANCE_FEEDING); + MathUtil.isNear(topRightMotorRpm, feedingRpm, ShooterConfig.RPM_TOLERANCE_FEEDING); default -> true; }; } diff --git a/comp-bot/src/main/java/frc/robot/shooter/ShooterConfig.java b/comp-bot/src/main/java/frc/robot/shooter/ShooterConfig.java index 5fe3960db..202f3bb88 100644 --- a/comp-bot/src/main/java/frc/robot/shooter/ShooterConfig.java +++ b/comp-bot/src/main/java/frc/robot/shooter/ShooterConfig.java @@ -19,10 +19,11 @@ import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.DoubleSubscriber; +import frc.robot.config.RobotKind; import java.util.Map; public class ShooterConfig { - public static final double RPM_TOLERANCE = 100.0; + public static final double RPM_TOLERANCE = 50.0; public static final double RPM_TOLERANCE_ACTIVELY_SHOOTING = 300.0; public static final double RPM_TOLERANCE_FEEDING = 500.0; @@ -39,6 +40,14 @@ public class ShooterConfig { public static final Transform2d SHOOTER_TO_ROBOT = new Transform2d(0.0, 0.0, Rotation2d.k180deg); + public static final InterpolatingDoubleTreeMap FEEDER_CURRENT_TO_SHOOTER_FEED_FORWARD = + TunableInterpolatingDoubleTreeMap.ofEntries( + "Shooter/FeederCurrentToShooterFeedForward", + Map.entry(0.0, 0.0), + Map.entry(50.0, 40.0), + Map.entry(20.0, 5.0), + Map.entry(30.0, 15.0)); + public static final InterpolatingDoubleTreeMap DISTANCE_TO_SCORE_RPM = TunableInterpolatingDoubleTreeMap.ofEntries( "Shooter/DistanceToScoreRPM", @@ -88,7 +97,11 @@ public class ShooterConfig { createMotorConfig() .withMotorOutput( new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) - .withSlot0(new Slot0Configs().withKP(14.0).withKS(6.4).withKV(0.13)); + .withSlot0( + new Slot0Configs() + .withKP(RobotKind.IS_COMP_BOT ? 0 : 14.0) + .withKS(RobotKind.IS_COMP_BOT ? 0 : 6.4) + .withKV(RobotKind.IS_COMP_BOT ? 0 : 0.13)); public static final TalonFXConfiguration BOTTOM_LEFT_MOTOR_CONFIG = createMotorConfig() .withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)) @@ -99,8 +112,10 @@ public class ShooterConfig { new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive)) .withSlot0(new Slot0Configs().withKP(0.0).withKV(0.0)); - public static DoubleSubscriber ACTIVE_SHOT_FF_CURRENT = - DogLog.tunable("Shooter/ActiveShotFFCurrent", 20.0); + public static DoubleSubscriber FULL_HOPPER_INITIAL_FF = + DogLog.tunable("Shooter/FullHopperInitialFF", 30.0); + public static DoubleSubscriber LOW_HOPPER_INITIAL_FFF = + DogLog.tunable("Shooter/LowHopperInitialFF", 20.0); public static DoubleSubscriber TURBO_MODE_FF_CURRENT = DogLog.tunable("Shooter/TurboShotFFCurrent", 30.0); diff --git a/comp-bot/src/main/java/frc/robot/shooter_hood/ShooterHoodConfig.java b/comp-bot/src/main/java/frc/robot/shooter_hood/ShooterHoodConfig.java index 59e89fc88..a64ef3ca4 100644 --- a/comp-bot/src/main/java/frc/robot/shooter_hood/ShooterHoodConfig.java +++ b/comp-bot/src/main/java/frc/robot/shooter_hood/ShooterHoodConfig.java @@ -11,6 +11,7 @@ import com.team581.math.PolynomialRegression; import com.team581.util.tuning.TunableInterpolatingDoubleTreeMap; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import frc.robot.config.RobotKind; import java.util.Map; public class ShooterHoodConfig { @@ -44,7 +45,8 @@ public class ShooterHoodConfig { new MotorOutputConfigs() .withNeutralMode(NeutralModeValue.Coast) .withInverted(InvertedValue.Clockwise_Positive)) - .withSlot0(new Slot0Configs().withKP(300.0).withKV(0).withKS(0)); + .withSlot0( + new Slot0Configs().withKP(RobotKind.IS_COMP_BOT ? 0 : 300.0).withKV(0).withKS(0)); // TODO: Update interpolating map numbers later public static final InterpolatingDoubleTreeMap DISTANCE_TO_SCORE = diff --git a/shared/src/main/java/com/team581/math/MathHelpers.java b/shared/src/main/java/com/team581/math/MathHelpers.java index 3cd78ee63..a33f7eb6a 100644 --- a/shared/src/main/java/com/team581/math/MathHelpers.java +++ b/shared/src/main/java/com/team581/math/MathHelpers.java @@ -280,6 +280,10 @@ public static double roundTo(double value, double numDigits) { return Math.round(value * factor * (1 + EPSILON)) / factor; } + public static final double rpmToLinearVelocity(double rpm, double radiusMeters) { + return rpm * radiusMeters * (Math.PI / 30.0); + } + public static double signedSqrt(double value) { return Math.copySign(Math.sqrt(Math.abs(value)), value); } diff --git a/shared/src/main/java/com/team581/mechanisms/imu/BumpCrossingState.java b/shared/src/main/java/com/team581/mechanisms/imu/BumpCrossingState.java new file mode 100644 index 000000000..e40991cc1 --- /dev/null +++ b/shared/src/main/java/com/team581/mechanisms/imu/BumpCrossingState.java @@ -0,0 +1,7 @@ +package com.team581.mechanisms.imu; + +public enum BumpCrossingState { + NOT_ON_BUMP, + CROSSING_UPHILL, + CROSSING_DOWNHILL; +} diff --git a/shared/src/main/java/com/team581/simkit/FuelSim.java b/shared/src/main/java/com/team581/simkit/FuelSim.java new file mode 100644 index 000000000..43e4fe180 --- /dev/null +++ b/shared/src/main/java/com/team581/simkit/FuelSim.java @@ -0,0 +1,984 @@ +package com.team581.simkit; + +import static com.google.common.base.Preconditions.checkState; +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 edu.wpi.first.wpilibj.Timer; +import java.util.ArrayList; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +public class FuelSim { + 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; + } + + /** Get the current count of fuel scored in this hub */ + public int getScore() { + return score; + } + + /** Reset this hub's score to 0 */ + public void resetScore() { + score = 0; + } + + 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 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 Translation3d getDispersalVelocity() { + return new Translation3d( + exitVelXMult * (Math.random() + 0.1) * 1.5, Math.random() * 2 - 1, 0); + } + + protected void handleHubInteraction(Fuel fuel, int subticks) { + if (didFuelScore(fuel, subticks)) { + fuel.pos = exit; + fuel.vel = getDispersalVelocity(); + score++; + } + } + } + + protected static class Fuel { + protected Translation3d pos; + protected Translation3d vel; + + protected Fuel(Translation3d pos) { + this(pos, new Translation3d()); + } + + protected Fuel(Translation3d pos, Translation3d vel) { + this.pos = pos; + this.vel = vel; + } + + protected void addImpulse(Translation3d impulse) { + vel = vel.plus(impulse); + } + + 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 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 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 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; + } + } + + 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.465; // 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 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); + + 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)); + } + } + + 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)); + } + + @SuppressWarnings("unchecked") + protected final ArrayList[][] grid = new ArrayList[GRID_COLS][GRID_ROWS]; + + private final ArrayList> activeCells = new ArrayList<>(); + protected ArrayList fuels = new ArrayList<>(); + protected boolean spawnedStartingFuel = false; + + protected int maxAdditions = Integer.MAX_VALUE; + 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 shooterHeight; + protected double shooterForwardOffset; + protected double shooterWidth; + protected double bumperHeight; + protected ArrayList intakes = new ArrayList<>(); + protected int subticks = 5; + + protected double loggingFreqHz = 10; + + protected Timer loggingTimer = new Timer(); + + protected StructArrayPublisher fuelPublisher; + + /** Creates a new instance of FuelSim with log path "/Fuel Simulation" */ + public FuelSim() { + this("/Fuel Simulation"); + } + + /** + * 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(); + } + + /** Clears the field of fuel */ + public void clearFuel() { + fuels.clear(); + } + + /** Enables accounting for drag force in physics step * */ + public void enableAirResistance() { + simulateAirResistance = true; + } + + /** + * 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 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 shotYaw, Distance launchHeight) { + checkState( + robotPoseSupplier != null && robotFieldSpeedsSupplier != null, + "Robot must be registered before launching fuel."); + + Pose3d launchPose = + new Pose3d(this.robotPoseSupplier.get()) + .plus( + new Transform3d( + new Translation3d( + Distance.ofBaseUnits(shooterForwardOffset, Meters), + Distance.ofBaseUnits((shooterWidth / 2) * (1 - Math.random() * 2), Meters), + 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(shotYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + double yVel = + horizontalVel * Math.sin(shotYaw.plus(launchPose.getRotation().getMeasureZ()).in(Radians)); + + xVel += fieldSpeeds.vxMetersPerSecond; + yVel += fieldSpeeds.vyMetersPerSecond; + + spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); + } + + /** Adds array of `Translation3d`'s to NetworkTables at tableKey + "/Fuels" */ + public void logFuels() { + fuelPublisher.set(fuels.stream().map((fuel) -> fuel.pos).toArray(Translation3d[]::new)); + } + + /** + * 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)); + } + + /** + * 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 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 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(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 + */ + 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 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 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 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 fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + Distance width, + Distance length, + Distance bumperHeight, + Distance launchHeight, + Distance shooterForwardOffset, + Distance shooterWidth, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width.in(Meters); + this.robotLength = length.in(Meters); + this.shooterHeight = launchHeight.in(Meters); + this.shooterForwardOffset = shooterForwardOffset.in(Meters); + this.shooterWidth = shooterWidth.in(Meters); + this.bumperHeight = bumperHeight.in(Meters); + } + + /** + * Registers a robot with the fuel simulator + * + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + double width, + double length, + double bumperHeight, + double launchHeight, + double shooterForwardOffset, + double shooterWidth, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width; + this.robotLength = length; + this.shooterHeight = launchHeight; + this.shooterForwardOffset = shooterForwardOffset; + this.shooterWidth = shooterWidth; + this.bumperHeight = bumperHeight; + } + + /** + * Sets the frequency to publish fuel translations to NetworkTables Used to improve performance in + * AdvantageScope + * + * @param loggingFreqHz update frequency in hertz + */ + public void setLoggingFrequency(double loggingFreqHz) { + this.loggingFreqHz = loggingFreqHz; + } + + /** + * Sets the maximum number of fuels allowed on the field + * + * @param maxSize maximum number of fuels + */ + public void setMaxAdditions(int maxSize) { + this.maxAdditions = maxSize; + } + + /** + * Sets the number of physics iterations per loop (0.02s) + * + * @param subticks physics iteration per loop (default: 5) + */ + public void setSubticks(int subticks) { + this.subticks = subticks; + } + + /** + * Adds a fuel onto the field + * + * @param pos Position to spawn at + * @param vel Initial velocity vector + */ + public void spawnFuel(Translation3d pos, Translation3d vel) { + if (fuels.size() >= (spawnedStartingFuel ? 408 : 0) + maxAdditions) { + for (int i = 0; i < fuels.size(); i++) { + Fuel f = fuels.get(i); + if (f.pos.getZ() < FUEL_RADIUS + 0.05) { + fuels.remove(i); + break; + } + } + } + fuels.add(new Fuel(pos, vel)); + } + + /** 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))); + } + } + + spawnedStartingFuel = true; + } + + /** Start the simulation. `updateSim` must still be called every loop */ + public void start() { + running = true; + loggingTimer.restart(); + } + + /** 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); + } + } + + if (loggingTimer.advanceIfElapsed(1.0 / loggingFreqHz)) { + logFuels(); + } + } + + /** Pause the simulation. */ + public void stop() { + running = false; + loggingTimer.stop(); + } + + /** To be called periodically Will do nothing if sim is not running */ + public void updateSim() { + if (!running) return; + + stepSim(); + } + + 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 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 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); + } + } +} diff --git a/shared/src/main/java/com/team581/util/FeedLocation.java b/shared/src/main/java/com/team581/util/FeedLocation.java index 3ff9839e4..0f6d9ad6f 100644 --- a/shared/src/main/java/com/team581/util/FeedLocation.java +++ b/shared/src/main/java/com/team581/util/FeedLocation.java @@ -6,16 +6,25 @@ public enum FeedLocation { LEFT(FieldUtil.FEED_LEFT_POSE), - RIGHT(FieldUtil.FEED_RIGHT_POSE); + RIGHT(FieldUtil.FEED_RIGHT_POSE), + BACKUP_LEFT(FieldUtil.BACKUP_FEED_LEFT_POSE), + BACKUP_RIGHT(FieldUtil.BACKUP_FEED_RIGHT_POSE); public static FeedLocation closest(Pose2d robot) { var yDistanceToLeft = Math.abs(robot.getY() - FieldUtil.FEED_LEFT_POSE.getY()); var yDistanceToRight = Math.abs(robot.getY() - FieldUtil.FEED_RIGHT_POSE.getY()); var closestFeedLocation = Math.min(yDistanceToLeft, yDistanceToRight); if (closestFeedLocation == yDistanceToLeft) { + if (FieldUtil.isFeedPathObstructed( + robot.getTranslation(), FieldUtil.FEED_LEFT_POSE.getTranslation())) { + return FeedLocation.BACKUP_LEFT; + } return FeedLocation.LEFT; } - + if (FieldUtil.isFeedPathObstructed( + robot.getTranslation(), FieldUtil.FEED_RIGHT_POSE.getTranslation())) { + return FeedLocation.BACKUP_RIGHT; + } return FeedLocation.RIGHT; } diff --git a/shared/src/main/java/com/team581/util/FieldUtil.java b/shared/src/main/java/com/team581/util/FieldUtil.java index be7b68d2a..891beb305 100644 --- a/shared/src/main/java/com/team581/util/FieldUtil.java +++ b/shared/src/main/java/com/team581/util/FieldUtil.java @@ -28,6 +28,10 @@ public class FieldUtil { public static final Point FEED_LEFT_POSE = Point.ofRed(new Pose2d(15.75, 3.0, Rotation2d.kZero)); public static final Point FEED_RIGHT_POSE = Point.ofRed(new Pose2d(15.75, FIELD_WIDTH_Y - 3.0, Rotation2d.kZero)); + public static final Point BACKUP_FEED_LEFT_POSE = + Point.ofRed(new Pose2d(15.15, 1.0, Rotation2d.kZero)); + public static final Point BACKUP_FEED_RIGHT_POSE = + Point.ofRed(new Pose2d(15.15, 7.1, Rotation2d.kZero)); private static final double BLUE_STARTING_LINE_X = Units.inchesToMeters(156.61); private static final double RED_STARTING_LINE_X = FIELD_LENGTH_X - BLUE_STARTING_LINE_X; @@ -436,6 +440,12 @@ public static Pose2d clampPoseToAllianceZone(Pose2d robot) { // Logs all trench/bump zone corners & points; only needs to run once public static void debugLogFieldZones() { + // Feed poses + DogLog.log("FieldUtil/Feeding/FEED_LEFT_POSE", FEED_LEFT_POSE); + DogLog.log("FieldUtil/Feeding/BACKUP_LEFT_POSE", BACKUP_FEED_LEFT_POSE); + DogLog.log("FieldUtil/Feeding/FEED_RIGHT_POSE", FEED_RIGHT_POSE); + DogLog.log("FieldUtil/Feeding/BACKUP_RIGHT_POSE", BACKUP_FEED_RIGHT_POSE); + // Trenches DogLog.log( "FieldUtil/BlueOutpost/Trenches/Corner1",