diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index 23e0db94..a43d0437 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index a8ea08de..31fd213b 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -15,7 +15,7 @@ "robotMOI": 5.018, "robotTrackwidth": 0.546, "driveWheelRadius": 0.049, - "driveGearing": 6.12, + "driveGearing": 7.03, "maxDriveSpeed": 4.4, "driveMotorType": "krakenX60FOC", "driveCurrentLimit": 100.0, diff --git a/src/main/java/frc/trigon/lib b/src/main/java/frc/trigon/lib index 6b6c7adb..9d24e820 160000 --- a/src/main/java/frc/trigon/lib +++ b/src/main/java/frc/trigon/lib @@ -1 +1 @@ -Subproject commit 6b6c7adbf9d1111fa67fdc0b801e0520044bc7f8 +Subproject commit 9d24e8203d6665b1a88361f773028b1d27fb8145 diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 7de344c1..dc27ee49 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -5,12 +5,12 @@ package frc.trigon.robot; -import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.lib.utilities.flippable.Flippable; import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.commandfactories.autonomous.GeneralAutonomousCommands; import frc.trigon.robot.commands.commandfactories.FuelIntakeCommands; import frc.trigon.robot.commands.commandfactories.GeneralCommands; import frc.trigon.robot.commands.commandfactories.ShootingCommands; @@ -38,7 +38,6 @@ import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.turret.Turret; import frc.trigon.robot.subsystems.turret.TurretCommands; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; public class RobotContainer { public static final RobotPoseEstimator ROBOT_POSE_ESTIMATOR = new RobotPoseEstimator(); @@ -54,11 +53,9 @@ public class RobotContainer { public static final Shooter SHOOTER = new Shooter(); public static final Spindexer SPINDEXER = new Spindexer(); public static final Turret TURRET = new Turret(); - private LoggedDashboardChooser autoChooser; public RobotContainer() { initializeGeneralSystems(); - buildAutoChooser(); configureBindings(); } @@ -66,13 +63,13 @@ public RobotContainer() { * @return the command to run in autonomous mode */ public Command getAutonomousCommand() { - return autoChooser.get(); + return GeneralAutonomousCommands.getAutonomousCommand(); } private void configureBindings() { bindDefaultCommands(); bindControllerCommands(); -// configureSysIDBindings(TURRET); +// configureSysIDBindings(SWERVE); } private void bindDefaultCommands() { @@ -103,7 +100,7 @@ private void bindControllerCommands() { OperatorConstants.SET_FIXED_SHOOTING_POSITION_LEFT_CORNER_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.LEFT_CORNER)); OperatorConstants.SET_FIXED_SHOOTING_POSITION_CLOSE_TO_TOWER_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.CLOSE_TO_TOWER)); OperatorConstants.SET_FIXED_SHOOTING_POSITION_CLOSE_TO_OUTPOST_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.CLOSE_TO_OUTPOST)); - OperatorConstants.SHORT_EJECTION_TRIGGER.whileTrue(ShootingCommands.getShortEjectFuelCommand()); + OperatorConstants.SHORT_EJECTION_TRIGGER.whileTrue(TurretCommands.getDebuggingCommand()); } private void configureSysIDBindings(MotorSubsystem subsystem) { @@ -125,8 +122,4 @@ private void initializeGeneralSystems() { AutonomousConstants.init(); ShootingLookupTable3D.init(); } - - private void buildAutoChooser() { - autoChooser = new LoggedDashboardChooser<>("AutoChooser", AutoBuilder.buildAutoChooser()); - } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java deleted file mode 100644 index 51976806..00000000 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java +++ /dev/null @@ -1,77 +0,0 @@ -package frc.trigon.robot.commands.commandclasses; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.*; -import frc.trigon.lib.utilities.flippable.FlippableRotation2d; -import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.commands.commandfactories.GeneralCommands; -import frc.trigon.robot.constants.AutonomousConstants; -import frc.trigon.robot.misc.objectdetection.ObjectPoseEstimator; -import frc.trigon.robot.subsystems.swerve.SwerveCommands; -import org.littletonrobotics.junction.Logger; - -import java.util.function.Supplier; - -public class GamePieceAutoDriveCommand extends ParallelCommandGroup { - private static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = RobotContainer.OBJECT_POSE_ESTIMATOR; - private Translation2d distanceFromTrackedGamePiece; - - public GamePieceAutoDriveCommand() { - addCommands( - getTrackGamePieceCommand(), - GeneralCommands.getContinuousConditionalCommand( - getDriveToGamePieceCommand(() -> distanceFromTrackedGamePiece), - GeneralCommands.getFieldRelativeDriveCommand(), - () -> OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null && shouldMoveTowardsGamePiece(distanceFromTrackedGamePiece) - ) - ); - } - - private Command getTrackGamePieceCommand() { - return new RunCommand(() -> { - distanceFromTrackedGamePiece = calculateDistanceFromTrackedGamePiece(); - }); - } - - public static Translation2d calculateDistanceFromTrackedGamePiece() { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - final Translation2d trackedObjectPositionOnField = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot(); - if (trackedObjectPositionOnField == null) - return null; - - final Translation2d robotToGamePiece = robotPose.getTranslation().minus(trackedObjectPositionOnField); - var distanceFromTrackedGamePiece = robotToGamePiece.rotateBy(robotPose.getRotation().unaryMinus()); - Logger.recordOutput("GamePieceAutoDrive/DistanceFromTrackedGamePiece", distanceFromTrackedGamePiece); - Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position); - return distanceFromTrackedGamePiece; - } - - public static Command getDriveToGamePieceCommand(Supplier distanceFromTrackedGamePiece) { - return new SequentialCommandGroup( - new InstantCommand(() -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().getX())), - SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()), - () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()), - GamePieceAutoDriveCommand::calculateTargetAngle - ) - ); - } - - public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrackedGamePiece) { - return distanceFromTrackedGamePiece != null && - (distanceFromTrackedGamePiece.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open - } - - public static FlippableRotation2d calculateTargetAngle() { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - final Translation2d trackedObjectFieldRelativePosition = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot(); - if (trackedObjectFieldRelativePosition == null) - return null; - - final Translation2d objectDistanceFromRobot = trackedObjectFieldRelativePosition.minus(robotPose.getTranslation()); - final Rotation2d angularDistanceToObject = new Rotation2d(Math.atan2(objectDistanceFromRobot.getY(), objectDistanceFromRobot.getX())); - return new FlippableRotation2d(angularDistanceToObject, false); - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index fce03ab1..9536eab2 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -1,16 +1,17 @@ package frc.trigon.robot.commands.commandclasses; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.lib.hardware.RobotHardwareStats; import frc.trigon.lib.utilities.flippable.Flippable; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.misc.shootingphysics.ShootingCalculations; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.Logger; @@ -21,26 +22,26 @@ * A command class to assist in the intaking of a game piece. */ public class IntakeAssistCommand extends ParallelCommandGroup { - private final ProfiledPIDController + private final PIDController xPIDController = RobotHardwareStats.isSimulation() ? - new ProfiledPIDController(5, 0, 0, new TrapezoidProfile.Constraints(5, 15)) : - new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), + new PIDController(5, 0, 0) : + new PIDController(2.4, 0, 0), yPIDController = RobotHardwareStats.isSimulation() ? - new ProfiledPIDController(3, 0, 0.01, new TrapezoidProfile.Constraints(5, 15)) : - new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)), + new PIDController(0.3, 0, 0) : + new PIDController(0.26, 0, 0), thetaPIDController = RobotHardwareStats.isSimulation() ? - new ProfiledPIDController(0.1, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); + new PIDController(0.1, 0, 0) : + new PIDController(2.4, 0, 0); private Translation2d distanceFromTrackedGamePiece; /** - * Creates a new intake assist command that assists the driver with intaking a game piece based on the powers. + * Creates a new intake assist command that assists the driver with intaking a game piece. * - * @param xAssistPower the amount of assist to apply to the X-axis (Forward and backwards) - * @param yAssistPower the amount of assist to apply to the Y-axis (Sideways) - * @param thetaAssistPower the amount of assist to apply to the rotation + * @param shouldAssistX should apply to the X-axis (Forward and backwards) + * @param shouldAssistY should apply to the Y-axis (Sideways) + * @param shouldAssistTheta should apply to the rotation */ - public IntakeAssistCommand(double xAssistPower, double yAssistPower, double thetaAssistPower) { + public IntakeAssistCommand(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta) { addCommands( new RunCommand(this::trackGamePiece), new SequentialCommandGroup( @@ -49,28 +50,28 @@ public IntakeAssistCommand(double xAssistPower, double yAssistPower, double thet new WaitUntilCommand(this::hasNoTrackedGamePiece) ).repeatedly(), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> calculateTranslationPower(true, xAssistPower), - () -> calculateTranslationPower(false, yAssistPower), - () -> calculateThetaPower(thetaAssistPower) + () -> calculateTranslationPower(true, shouldAssistX), + () -> calculateTranslationPower(false, shouldAssistY), + () -> calculateThetaPower(shouldAssistTheta) ) ); } private void resetPIDControllers() { - xPIDController.reset(distanceFromTrackedGamePiece.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().getX()); - yPIDController.reset(distanceFromTrackedGamePiece.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().getY()); - thetaPIDController.reset(distanceFromTrackedGamePiece.getAngle().getRadians(), RobotContainer.SWERVE.getRotationalVelocityRadiansPerSecond()); + xPIDController.reset(); + yPIDController.reset(); + thetaPIDController.reset(); - xPIDController.setGoal(0); - yPIDController.setGoal(0); - thetaPIDController.setGoal(0); + xPIDController.setSetpoint(0); + yPIDController.setSetpoint(0); + thetaPIDController.setSetpoint(0); } - private double calculateTranslationPower(boolean isXAxis, double assistScalar) { + private double calculateTranslationPower(boolean isXAxis, boolean shouldAssist) { final Translation2d selfRelativeJoystickValue = getSelfRelativeJoystickPosition(); - final double joystickValue = isXAxis ? selfRelativeJoystickValue.getX() : selfRelativeJoystickValue.getY(); + final double joystickValue = CommandConstants.calculateDriveStickAxisValue(isXAxis ? selfRelativeJoystickValue.getX() : selfRelativeJoystickValue.getY()); - if (hasNoTrackedGamePiece()) + if (!shouldAssist || hasNoTrackedGamePiece()) return joystickValue; final double @@ -78,27 +79,25 @@ private double calculateTranslationPower(boolean isXAxis, double assistScalar) { xPIDController.calculate(distanceFromTrackedGamePiece.getX()) : yPIDController.calculate(distanceFromTrackedGamePiece.getY())); - return calculateAssistPower(assistScalar, pidOutput, joystickValue); + return calculateAssistPower(pidOutput, joystickValue); } - private double calculateThetaPower(double assistScalar) { - final double joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); + private double calculateThetaPower(boolean shouldAssist) { + final double joystickValue = CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getRightX()); - if (hasNoTrackedGamePiece()) + if (!shouldAssist || hasNoTrackedGamePiece()) return joystickValue; - return calculateThetaAssistPower(assistScalar, joystickValue, distanceFromTrackedGamePiece.getAngle().unaryMinus()); + return calculateThetaAssistPower(joystickValue, distanceFromTrackedGamePiece.getAngle().unaryMinus()); } - private double calculateThetaAssistPower(double assistScalar, double joystickValue, Rotation2d thetaOffset) { + private double calculateThetaAssistPower(double joystickValue, Rotation2d thetaOffset) { final double pidOutput = clampToSwerveOutputRange(thetaPIDController.calculate(thetaOffset.getRadians())); - return calculateAssistPower(assistScalar, pidOutput, joystickValue); + return calculateAssistPower(pidOutput, joystickValue); } - private double calculateAssistPower(double assistScalar, double pidOutput, double joystickValue) { - if (assistScalar == 0) - return joystickValue; - return (joystickValue * (1 - assistScalar)) + (pidOutput * assistScalar); + private double calculateAssistPower(double pidOutput, double joystickValue) { + return joystickValue + pidOutput; } private boolean hasNoTrackedGamePiece() { @@ -165,9 +164,10 @@ private boolean isGamePieceWithinAssistFOV(Translation2d gamePiecePosition, Pose } private boolean isDrivingTowardsGamePiece(Translation2d gamePieceFieldRelativePosition, Pose2d robotPose) { - final Translation2d fieldRelativeDistanceFromGamePiece = gamePieceFieldRelativePosition.minus(robotPose.getTranslation()); - - final double velocityTowardsGamePiece = calculateVelocityTowardsGamePiece(fieldRelativeDistanceFromGamePiece); + final Translation2d velocityRelativeToGamePiece = ShootingCalculations.calculateVelocityRelativeToPoint(gamePieceFieldRelativePosition, robotPose.getTranslation(), getFieldRelativeJoystickPosition()); + if (velocityRelativeToGamePiece.getX() < Math.abs(velocityRelativeToGamePiece.getY())) + return false; + final double velocityTowardsGamePiece = velocityRelativeToGamePiece.getX() * SwerveConstants.MAXIMUM_SPEED_METERS_PER_SECOND; return velocityTowardsGamePiece > OperatorConstants.MINIMUM_VELOCITY_TOWARDS_GAME_PIECE_FOR_INTAKE_ASSIST_METERS_PER_SECOND; } @@ -175,18 +175,6 @@ private double calculateMaximumAssistAngleDegrees(double gamePieceDistance) { return OperatorConstants.INTAKE_ASSIST_MAXIMUM_ASSISTABLE_ANGLE_FORMULA.applyAsDouble(gamePieceDistance); } - private double calculateVelocityTowardsGamePiece(Translation2d fieldRelativeDistanceFromGamePiece) { - final Translation2d fieldRelativeJoystickPosition = getFieldRelativeJoystickPosition(); - - final Rotation2d joystickAngle = fieldRelativeJoystickPosition.getAngle(); - final Rotation2d fieldRelativeGamePieceAngleFromRobot = fieldRelativeDistanceFromGamePiece.getAngle(); - final Rotation2d angularOffset = joystickAngle.minus(fieldRelativeGamePieceAngleFromRobot); - - final double desiredRobotVelocity = fieldRelativeJoystickPosition.getNorm() * SwerveConstants.MAXIMUM_SPEED_METERS_PER_SECOND; - - return angularOffset.getCos() * desiredRobotVelocity; - } - private double clampToSwerveOutputRange(double value) { return MathUtil.clamp(value, -1, 1); } diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java index aa76f81f..f95525e4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.commands.commandfactories.AutonomousCommands; +import frc.trigon.robot.commands.commandfactories.autonomous.GeneralAutonomousCommands; import org.littletonrobotics.junction.Logger; import java.util.function.Supplier; @@ -44,7 +44,7 @@ public boolean runsWhenDisabled() { private Command getUpdateAutoStartPoseCommand() { return new InstantCommand(() -> { - this.autoStartPose = AutonomousCommands.getAutoStartPose(autoName.get()); + this.autoStartPose = GeneralAutonomousCommands.getAutoStartPose(autoName.get()); Logger.recordOutput("PathPlanner/AutoStartPose", autoStartPose); }); } diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java new file mode 100644 index 00000000..2f11f300 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveCommand.java @@ -0,0 +1,353 @@ +package frc.trigon.robot.commands.commandclasses.gamepieceautodrive; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.lib.utilities.flippable.FlippableRotation2d; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.misc.objectdetection.ObjectPoseEstimator; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; + +import java.util.ArrayList; +import java.util.List; +import java.util.concurrent.atomic.AtomicReference; + +public class GamePieceAutoDriveCommand extends ParallelCommandGroup { + private static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = RobotContainer.OBJECT_POSE_ESTIMATOR; + private final AtomicReference currentTargetCluster = new AtomicReference<>(); + // Raw approach heading (optionally rate-limited). NOT wall-clamped — the + // clamp is applied only in getTargetHeading() so the position clamp can + // still see the unclamped heading. + private final AtomicReference commandedHeading = new AtomicReference<>(); + // Keeps the last heading that was actually valid so that brief vision gaps + // (cluster == null for a cycle) don't send null to the swerve and release + // heading control, letting the robot spin freely. + private volatile FlippableRotation2d lastValidHeading = null; + /** When true, drive speed ramps down and rotation is rate-limited near the piece. */ + private final boolean slowIntake; + + public GamePieceAutoDriveCommand(boolean slowIntake) { + this.slowIntake = slowIntake; + addCommands( + createTargetUpdateCommand(), + createDriveCommand() + ); + } + + private Command createTargetUpdateCommand() { + return new RunCommand(() -> { + GamePieceCluster prev = currentTargetCluster.get(); + GamePieceCluster bestCluster = findBestCluster(); + currentTargetCluster.set(bestCluster); + if (prev == null && bestCluster != null) { + AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(); + AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.reset(); + } + updateCommandedHeading(); + }); + } + + private Command createDriveCommand() { + return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + this::getXControllerOutput, + this::getYControllerOutput, + this::getTargetHeading + ); + } + + // ========================================================================= + // HEADING + // ========================================================================= + + private void updateCommandedHeading() { + GamePieceCluster cluster = currentTargetCluster.get(); + if (cluster == null) { + // Don't null out — keep last heading so the swerve maintains + // orientation during brief vision gaps instead of spinning freely. + return; + } + + FlippableRotation2d target = new FlippableRotation2d(cluster.getApproachHeading(), false); + + if (!slowIntake || !isWithinIntakeDistance()) { + commandedHeading.set(target); + lastValidHeading = target; + return; + } + + // Rate-limit inside the intake zone. + FlippableRotation2d prev = commandedHeading.get(); + if (prev == null) { + commandedHeading.set(target); + lastValidHeading = target; + return; + } + + double maxDeltaDeg = GamePieceAutoDriveConstants.MAX_INTAKE_ROTATION_RATE_DEG_PER_SEC * 0.02; + double deltaDeg = Math.IEEEremainder(target.get().getDegrees() - prev.get().getDegrees(), 360.0); + if (Math.abs(deltaDeg) > maxDeltaDeg) { + deltaDeg = Math.signum(deltaDeg) * maxDeltaDeg; + } + + FlippableRotation2d result = new FlippableRotation2d( + Rotation2d.fromDegrees(prev.get().getDegrees() + deltaDeg), false); + commandedHeading.set(result); + lastValidHeading = result; + } + + /** + * What actually goes to the swerve. The wall clamp is applied HERE so + * that commandedHeading stays clean for the position clamp to read. + */ + private FlippableRotation2d getTargetHeading() { + FlippableRotation2d heading = commandedHeading.get(); + if (heading == null) heading = lastValidHeading; + return clampHeadingForWall(heading); + } + + /** + * Prevents the intake from hitting the wall by blocking headings that + * would swing the intake tip past the effective wall. + * + * ── Geometry (intake at BACK, i.e. 180° in robot frame) ── + * The intake tip is (ROBOT_HALF_WIDTH + INTAKE_REACH) behind the robot + * centre. In world frame: + * tipX = robotX – intakeLength * cos(heading) + * + * Require tipX >= effectiveWallX: + * cos(heading) <= (robotX – effectiveWallX) / intakeLength = cosMax + * + * cos(θ) <= cosMax is satisfied when |θ| >= acos(cosMax). + * So the FORBIDDEN zone is |heading| < acos(cosMax) (near 0°, where the + * intake points toward the wall). + * The ALLOWED zone is everything else (near ±180°, intake away from wall). + * + * If the heading is in the forbidden zone it is pushed to the nearest + * boundary (± acos(cosMax)), which keeps it as close to the desired + * approach as possible while staying safe. + */ + private FlippableRotation2d clampHeadingForWall(FlippableRotation2d heading) { + if (heading == null) return null; + + Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + double intakeLength = GamePieceAutoDriveConstants.ROBOT_HALF_WIDTH + + GamePieceAutoDriveConstants.INTAKE_REACH; + + double cosMax = (robotPose.getX() - getEffectiveWallX()) / intakeLength; + + // Robot is far enough that even heading = 0° (intake straight at wall) + // keeps the tip clear. No clamping needed. + if (cosMax >= 1.0) return heading; + + // Robot is so close that NO heading is safe. Face 180° (intake + // maximally away from wall) as best-effort emergency. + if (cosMax < -1.0) { + return new FlippableRotation2d(Rotation2d.fromDegrees(180), false); + } + + // Normal case: forbidden zone is |heading| < minAbsAngle. + double minAbsAngleDeg = Math.toDegrees(Math.acos(cosMax)); + double headingDeg = Math.IEEEremainder(heading.get().getDegrees(), 360.0); + + if (Math.abs(headingDeg) < minAbsAngleDeg) { + // In the forbidden zone — push to the nearest allowed boundary. + // signum preserves which side of 0° we were on so the robot doesn't + // snap across; exact 0° defaults to +minAbsAngle. + headingDeg = (headingDeg >= 0) ? minAbsAngleDeg : -minAbsAngleDeg; + } + + return new FlippableRotation2d(Rotation2d.fromDegrees(headingDeg), false); + } + + // ========================================================================= + // DRIVE OUTPUTS + // ========================================================================= + + private double getXControllerOutput() { + if (!shouldDrive()) return 0.0; + Translation2d error = getTranslationError(); + if (error == null) return 0.0; + double output = AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(-error.getX()); + return output * getIntakeSpeedScale(); + } + + private double getYControllerOutput() { + if (!shouldDrive()) return 0.0; + Translation2d error = getTranslationError(); + if (error == null) return 0.0; + double output = AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(-error.getY()); + return output * getIntakeSpeedScale(); + } + + /** + * Drive is active as long as there is a reachable target and the robot + * hasn't closed to intake distance. No emergency "past-the-wall stop" + * here: if the robot has overshot the clamped target, the target is now + * behind it and the PID error naturally drives it back. Stopping would + * lock it in the unsafe position. + */ + private boolean shouldDrive() { + Translation2d error = getTranslationError(); + if (error == null) return false; + if (error.getNorm() <= AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS) + return false; + return true; + } + + /** + * Linear ramp from 1.0 (at INTAKE_SLOWDOWN_DISTANCE) down to + * INTAKE_DRIVE_SPEED_SCALE (at distance 0). Only active when slowIntake + * is enabled. + */ + private double getIntakeSpeedScale() { + if (!slowIntake) return 1.0; + + Translation2d error = getTranslationError(); + if (error == null) return 1.0; + + double dist = error.getNorm(); + double slowdownDist = GamePieceAutoDriveConstants.INTAKE_SLOWDOWN_DISTANCE_METERS; + if (dist >= slowdownDist) return 1.0; + + double t = dist / slowdownDist; + return GamePieceAutoDriveConstants.INTAKE_DRIVE_SPEED_SCALE + + t * (1.0 - GamePieceAutoDriveConstants.INTAKE_DRIVE_SPEED_SCALE); + } + + private boolean isWithinIntakeDistance() { + Translation2d error = getTranslationError(); + return error != null + && error.getNorm() < GamePieceAutoDriveConstants.INTAKE_SLOWDOWN_DISTANCE_METERS; + } + + // ========================================================================= + // WALL SAFETY & TARGET CLAMPING + // ========================================================================= + + /** + * The real wall shifted inward by the safety margin. All geometry + * checks use this as "the wall" so the robot + intake never get closer + * than WALL_SAFETY_MARGIN_METERS to the actual wall. + */ + private static double getEffectiveWallX() { + return GamePieceAutoDriveConstants.ALLIANCE_WALL_X_METERS + + 0.05; + } + + /** + * Cluster centroid clamped in X so the robot is safe at ANY heading. + * + * Uses the worst-case heading (0° = intake pointing directly at the wall, + * because the intake is at the back). This is the global maximum of minX + * across all headings, so it guarantees safety regardless of what heading + * the robot passes through while rotating. Only pieces near the wall are + * affected; pieces further out are unclamped. + * + * If the robot has overshot (is closer than the clamped target), the + * target is behind it and the PID drives it back automatically. + */ + private Translation2d getEffectiveTarget() { + GamePieceCluster cluster = currentTargetCluster.get(); + if (cluster == null) return null; + + Translation2d target = cluster.getCentroid(); + // Worst case heading for intake-at-back is 0° (intake straight at wall). + double minX = getMinSafeRobotX(Rotation2d.fromDegrees(0)); + + if (target.getX() < minX) { + target = new Translation2d(minX, target.getY()); + } + return target; + } + + /** + * Minimum robot-centre X that keeps the body and intake clear of the + * effective wall, for a given heading. + * + * ── Body ── + * Square, half-width ROBOT_HALF_WIDTH. The farthest any corner extends + * in –X from centre = halfWidth × (|cos θ| + |sin θ|). + * + * ── Intake (at BACK = 180° in robot frame) ── + * Tip is intakeLength behind centre. In world frame: + * tipX = robotX – intakeLength × cos(θ) + * The tip extends toward –X (the wall) when cos(θ) > 0, i.e. |θ| < 90°. + * The backward (toward-wall) extent is therefore max(0, intakeLength × cos(θ)). + */ + private double getMinSafeRobotX(Rotation2d heading) { + double cosH = Math.abs(heading.getCos()); + double sinH = Math.abs(heading.getSin()); + + double bodyBackwardExtent = GamePieceAutoDriveConstants.ROBOT_HALF_WIDTH * (cosH + sinH); + + // Intake at back: extends toward the wall when cos(heading) > 0. + double intakeBackwardExtent = Math.max(0.0, + (GamePieceAutoDriveConstants.ROBOT_HALF_WIDTH + GamePieceAutoDriveConstants.INTAKE_REACH) + * heading.getCos()); + + return getEffectiveWallX() + Math.max(bodyBackwardExtent, intakeBackwardExtent); + } + + /** + * Robot-frame translation error to the effective (wall-clamped) target. + */ + private Translation2d getTranslationError() { + Translation2d effectiveTarget = getEffectiveTarget(); + if (effectiveTarget == null) return null; + + Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + Translation2d fieldRelativeError = effectiveTarget.minus(robotPose.getTranslation()); + return fieldRelativeError.rotateBy(robotPose.getRotation().unaryMinus()); + } + + // ========================================================================= + // VISION LOGIC + // ========================================================================= + + private GamePieceCluster findBestCluster() { + List allObjects = OBJECT_POSE_ESTIMATOR.getObjectsOnField(); + Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + + if (allObjects.isEmpty()) return null; + + GamePieceCluster bestCluster = null; + double maxScore = -Double.MAX_VALUE; + + for (Translation2d seed : allObjects) { + if (isOutOfBounds(seed)) continue; + + List clusterPieces = getNeighbors(seed, allObjects); + if (clusterPieces.isEmpty()) continue; + + GamePieceCluster cluster = new GamePieceCluster(clusterPieces, robotPose); + double score = calculateClusterScore(clusterPieces.size(), cluster.getDistanceToRobot()); + + if (score > maxScore) { + maxScore = score; + bestCluster = cluster; + } + } + return bestCluster; + } + + private List getNeighbors(Translation2d seed, List allObjects) { + List neighbors = new ArrayList<>(); + for (Translation2d other : allObjects) { + if (isOutOfBounds(other)) continue; + if (seed.getDistance(other) <= GamePieceAutoDriveConstants.CLUSTER_RADIUS_METERS) + neighbors.add(other); + } + return neighbors; + } + + private boolean isOutOfBounds(Translation2d piece) { + return piece.getX() > GamePieceAutoDriveConstants.MAX_COLLECTION_X_METERS; + } + + private double calculateClusterScore(int count, double distance) { + return (count * GamePieceAutoDriveConstants.SCORE_WEIGHT_COUNT) + - (distance * GamePieceAutoDriveConstants.SCORE_PENALTY_DISTANCE); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java new file mode 100644 index 00000000..6b286929 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceAutoDriveConstants.java @@ -0,0 +1,24 @@ +package frc.trigon.robot.commands.commandclasses.gamepieceautodrive; + +import frc.trigon.robot.constants.FieldConstants; + +public class GamePieceAutoDriveConstants { + static final double CLUSTER_RADIUS_METERS = 1.0; + static final double MAX_COLLECTION_X_METERS = FieldConstants.FIELD_LENGTH_METERS / 2.0; + static final double SCORE_WEIGHT_COUNT = 2.5; + static final double SCORE_PENALTY_DISTANCE = 1.0; + static final double LINEARITY_THRESHOLD = 6.0; + static final double BLEND_START_DISTANCE_METERS = 1.5; + static final double BLEND_END_DISTANCE_METERS = 0.5; + + // Intake slowdown + public static final double INTAKE_SLOWDOWN_DISTANCE_METERS = 0.8; // start ramping speed here + public static final double INTAKE_DRIVE_SPEED_SCALE = 0.35; // minimum speed multiplier at the piece + + // Rotation rate limit while intaking + public static final double MAX_INTAKE_ROTATION_RATE_DEG_PER_SEC = 60.0; // tune on-field + + public static final double ROBOT_HALF_WIDTH = 0.37; // half-side of the robot square + public static final double INTAKE_REACH = 0.4; // how far the intake extends past the body + public static final double ALLIANCE_WALL_X_METERS = 0.0; // field X coordinate of the alliance wall +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceCluster.java b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceCluster.java new file mode 100644 index 00000000..cfd8341e --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/gamepieceautodrive/GamePieceCluster.java @@ -0,0 +1,115 @@ +package frc.trigon.robot.commands.commandclasses.gamepieceautodrive; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.util.List; + +public class GamePieceCluster { + private final Translation2d centroid; + private final Rotation2d approachHeading; + private final double distanceToRobot; + + public GamePieceCluster(List gamePieces, Pose2d robotPose) { + this.centroid = calculateCentroid(gamePieces); + this.distanceToRobot = centroid.getDistance(robotPose.getTranslation()); + this.approachHeading = calculateApproachHeading(gamePieces, robotPose); + } + + public Translation2d getCentroid() { + return centroid; + } + + public Rotation2d getApproachHeading() { + return approachHeading; + } + + public double getDistanceToRobot() { + return distanceToRobot; + } + + public int getSize() { + return 1; + } + + private Translation2d calculateCentroid(List pieces) { + double sumX = 0; + double sumY = 0; + for (Translation2d piece : pieces) { + sumX += piece.getX(); + sumY += piece.getY(); + } + return new Translation2d(sumX / pieces.size(), sumY / pieces.size()); + } + + private Rotation2d calculateApproachHeading(List pieces, Pose2d robotPose) { + Rotation2d angleToCentroid = new Rotation2d( + Math.atan2(centroid.getY() - robotPose.getY(), centroid.getX() - robotPose.getX()) + ); + + // If too few points, we cannot determine a line, so face the center + if (pieces.size() <= 2) + return angleToCentroid; + + if (isLinear(pieces)) { + Rotation2d axisAngle = calculatePrincipalAxis(pieces); + Rotation2d alignedAxis = resolveAmbiguity(axisAngle, angleToCentroid); + return blendHeadings(angleToCentroid, alignedAxis); + } else + return angleToCentroid; + } + + private boolean isLinear(List pieces) { + double varianceX = 0, varianceY = 0, covariance = 0; + + for (Translation2d p : pieces) { + double dx = p.getX() - centroid.getX(); + double dy = p.getY() - centroid.getY(); + varianceX += dx * dx; + varianceY += dy * dy; + covariance += dx * dy; + } + + double trace = varianceX + varianceY; + double det = (varianceX * varianceY) - (covariance * covariance); + double gap = Math.sqrt(Math.max(0, trace * trace - 4 * det)); + + double majorEigenvalue = (trace + gap) / 2.0; + double minorEigenvalue = (trace - gap) / 2.0; + + double aspectRatio = (minorEigenvalue == 0) ? Double.MAX_VALUE : (majorEigenvalue / minorEigenvalue); + return aspectRatio > GamePieceAutoDriveConstants.LINEARITY_THRESHOLD; + } + + private Rotation2d calculatePrincipalAxis(List pieces) { + double varianceX = 0, varianceY = 0, covariance = 0; + + for (Translation2d p : pieces) { + double dx = p.getX() - centroid.getX(); + double dy = p.getY() - centroid.getY(); + varianceX += dx * dx; + varianceY += dy * dy; + covariance += dx * dy; + } + + double angleRad = 0.5 * Math.atan2(2 * covariance, varianceX - varianceY); + return new Rotation2d(angleRad); + } + + private Rotation2d resolveAmbiguity(Rotation2d axisAngle, Rotation2d angleToCentroid) { + if (Math.abs(axisAngle.minus(angleToCentroid).getDegrees()) > 90.0) { + return axisAngle.rotateBy(Rotation2d.fromDegrees(180)); + } + return axisAngle; + } + + private Rotation2d blendHeadings(Rotation2d centerHeading, Rotation2d axisHeading) { + double blendFactor = (GamePieceAutoDriveConstants.BLEND_START_DISTANCE_METERS - distanceToRobot) + / (GamePieceAutoDriveConstants.BLEND_START_DISTANCE_METERS - GamePieceAutoDriveConstants.BLEND_END_DISTANCE_METERS); + + blendFactor = Math.max(0.0, Math.min(1.0, blendFactor)); // Clamp [0, 1] + + return centerHeading.interpolate(axisHeading, blendFactor); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java deleted file mode 100644 index cbf6158a..00000000 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ /dev/null @@ -1,148 +0,0 @@ -package frc.trigon.robot.commands.commandfactories; - -import com.pathplanner.lib.commands.PathPlannerAuto; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.*; -import frc.trigon.lib.utilities.flippable.FlippablePose2d; -import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.commands.commandclasses.GamePieceAutoDriveCommand; -import frc.trigon.robot.constants.AutonomousConstants; -import frc.trigon.robot.subsystems.swerve.SwerveCommands; -import org.json.simple.parser.ParseException; - -import java.io.IOException; -import java.util.function.Supplier; - -/** - * A class that contains command factories for preparation commands and commands used during the 15-second autonomous period at the start of each match. - */ -public class AutonomousCommands { - private static FlippablePose2d TARGET_SCORING_POSE = null; - - /** - * Creates a dynamic autonomous command intended for the 15-second autonomous period at the beginning of a match. - * Dynamic means that the command isn't pre-programmed and instead autonomously decides what game pieces to collect and where to score. - * - * @param intakeLocations the locations at which to collect game pieces - * @param scoringLocations the locations at which to score - * @return the command - */ - public static Command getDynamicAutonmousCommand(FlippablePose2d[] intakeLocations, FlippablePose2d... scoringLocations) { - return new SequentialCommandGroup( - getDriveAndScoreCommand(scoringLocations), - getCollectCommand(intakeLocations) - ).repeatedly().withName(generateDynamicAutonomousRoutineName(intakeLocations, scoringLocations)); - } - - private static Command getCollectCommand(FlippablePose2d[] intakeLocations) { - return new ParallelCommandGroup( - getIntakeSequenceCommand(), - getDriveToGamePieceCommand(intakeLocations) - ).until(AutonomousCommands::hasGamePiece); - } - - private static boolean hasGamePiece() { - //TODO: implement - return false; - } - - private static Command getDriveToGamePieceCommand(FlippablePose2d[] intakeLocations) { - return new ConditionalCommand( - new GamePieceAutoDriveCommand().onlyWhile(() -> RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null), - getFindGamePieceCommand(intakeLocations), - AutonomousCommands::shouldCollectGamePiece - ); - } - - private static boolean shouldCollectGamePiece() { - return RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null; - } - - private static Command getFindGamePieceCommand(FlippablePose2d[] intakeLocations) { - //TODO: implement - return null; - } - - private static Command getIntakeSequenceCommand() { - //TODO: implement - return null; - } - - private static Command getDriveAndScoreCommand(FlippablePose2d[] scoringLocations) { - return new ParallelCommandGroup( - getDriveToScoringLocationCommand(scoringLocations), - getScoringSequenceCommand() - ); - } - - private static Command getScoringSequenceCommand() { - return new SequentialCommandGroup( - new WaitUntilCommand(() -> TARGET_SCORING_POSE != null), - getScoreCommand() - ); - } - - private static Command getScoreCommand() { - //TODO: implement - return null; - } - - private static Command getDriveToScoringLocationCommand(FlippablePose2d[] scoringLocations) { - return new SequentialCommandGroup( - new InstantCommand(() -> TARGET_SCORING_POSE = calculateBestOpenScoringPose(scoringLocations)), - new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), - SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_SCORING_LOCATION_CONSTRAINTS).repeatedly() - ); - } - - private static FlippablePose2d calculateBestOpenScoringPose(FlippablePose2d[] scoringLocations) { - //TODO: implement - return null; - } - - /** - * Generates a name for the dynamic autonomous routine based on the intake and scoring locations. - * - * @param intakeLocations the intake locations - * @param scoringLocations the scoring locations - * @return the name of the dynamic autonomous routine - */ - private static String generateDynamicAutonomousRoutineName(FlippablePose2d[] intakeLocations, FlippablePose2d[] scoringLocations) { - //TODO: implement - return ""; - } - - /** - * Creates a command that resets the pose estimator's pose to the starting pose of the given autonomous as long as the robot is not enabled. - * - * @param autoName the name of the autonomous - * @return a command that resets the robot's pose estimator pose to the start position of the given autonomous - */ - public static Command getResetPoseToAutoPoseCommand(Supplier autoName) { - return new InstantCommand( - () -> { - if (DriverStation.isEnabled()) - return; - RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(getAutoStartPose(autoName.get())); - } - ).ignoringDisable(true); - } - - /** - * Gets the starting position of the target PathPlanner autonomous. - * - * @param autoName the name of the autonomous group - * @return the staring pose of the autonomous - */ - public static Pose2d getAutoStartPose(String autoName) { - try { - final Pose2d nonFlippedAutoStartPose = PathPlannerAuto.getPathGroupFromAutoFile(autoName).get(0).getStartingHolonomicPose().get(); - final FlippablePose2d flippedAutoStartPose = new FlippablePose2d(nonFlippedAutoStartPose, true); - return flippedAutoStartPose.get(); - } catch (IOException | ParseException e) { - e.printStackTrace(); - return new Pose2d(); - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java index beac4875..30ce2611 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java @@ -13,10 +13,10 @@ public class FuelIntakeCommands { SHOULD_KEEP_INTAKE_OPEN = new LoggedNetworkBoolean("/SmartDashboard/ShouldKeepIntakeOpen", true), SHOULD_ASSIST_INTAKE = new LoggedNetworkBoolean("/SmartDashboard/ShouldAssistIntake", true); - public static final double - X_ASSIST_POWER = 0.0, - Y_ASSIST_POWER = 0.5, - THETA_ASSIST_POWER = 0.0; + public static final boolean + SHOULD_ASSIST_X = false, + SHOULD_ASSIST_Y = true, + SHOULD_ASSIST_THETA = false; public static Command getIntakeCommand() { return new ParallelCommandGroup( @@ -45,9 +45,9 @@ public static Command getDisableIntakeAssistCommand() { private static Command getIntakeAssistCommand() { return new IntakeAssistCommand( - X_ASSIST_POWER, - Y_ASSIST_POWER, - THETA_ASSIST_POWER + SHOULD_ASSIST_X, + SHOULD_ASSIST_Y, + SHOULD_ASSIST_THETA ); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ShootingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ShootingCommands.java index 1df38877..0ca50317 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ShootingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ShootingCommands.java @@ -96,7 +96,7 @@ private static Command getLoadFuelWhenReadyCommand(boolean isAutoShootingAtHub) return new SequentialCommandGroup( new WaitUntilCommand(() -> canShoot(isAutoShootingAtHub)), getLoadFuelCommand() - .until(ShootingCommands::shouldStopShooting) + .until(() -> ShootingCommands.shouldStopShooting(isAutoShootingAtHub)) ).repeatedly(); } @@ -108,8 +108,11 @@ private static Command getLoadFuelCommand() { } private static boolean canShoot(boolean isShootingAtHub) { - return (!isShootingAtHub || canShootAtHub()) && - RobotContainer.SHOOTER.atTargetVelocity() + if (isShootingAtHub) { + return canShootAtHub() && RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.HOOD.atTargetAngle() && + RobotContainer.TURRET.atTargetShootingCalculationsAngle(false); + } + return RobotContainer.SHOOTER.atTargetVelocity() && RobotContainer.HOOD.atTargetAngle() && RobotContainer.TURRET.atTargetAngle(false); } @@ -118,8 +121,8 @@ private static boolean canShootAtHub() { return RobotContainer.SHOOTER.isAimingAtHub(); } - private static boolean shouldStopShooting() { - return !RobotContainer.TURRET.atTargetAngle(true); + private static boolean shouldStopShooting(boolean isShootingAtHub) { + return isShootingAtHub ? !RobotContainer.TURRET.atTargetShootingCalculationsAngle(true) : !RobotContainer.TURRET.atTargetAngle(true); } private static void updateShootingCalculations() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java new file mode 100644 index 00000000..46d7957c --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java @@ -0,0 +1,146 @@ +package frc.trigon.robot.commands.commandfactories.autonomous; + +import com.pathplanner.lib.commands.PathPlannerAuto; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.lib.utilities.flippable.FlippablePose2d; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandclasses.gamepieceautodrive.GamePieceAutoDriveCommand; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.commands.commandfactories.ShootingCommands; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.subsystems.intake.IntakeCommands; +import frc.trigon.robot.subsystems.intake.IntakeConstants; +import org.json.simple.parser.ParseException; + +import java.io.IOException; +import java.util.function.Supplier; + +/** + * A class that contains command factories for preparation commands and commands used during the 15-second autonomous period at the start of each match. + */ +public class GeneralAutonomousCommands { + public static Command getAutonomousCommand() { + return new SequentialCommandGroup( + AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.FIRST_AUTONOMOUS_CHOOSER.get().get(), + AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.SECOND_AUTONOMOUS_CHOOSER.get().get(), + AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get() == null ? new InstantCommand() : AutonomousConstants.THIRD_AUTONOMOUS_CHOOSER.get().get(), + getClimbCommand(AutonomousConstants.CLIMB_POSITION_CHOOSER.get()).onlyIf(() -> AutonomousConstants.CLIMB_POSITION_CHOOSER.get() != null) + ); + } + + public static Command getDeliveryCommand(boolean shootPreload, double collectionTimeout) { + return new ParallelDeadlineGroup( + getDriveToFuelInNeutralZoneCommand(shootPreload, collectionTimeout), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE), + GeneralCommands.getContinuousConditionalCommand( + ShootingCommands.getShootAtHubCommand(), + ShootingCommands.getDeliveryCommand(), + SafeAutonomousDriveCommands::isInAllianceZone + ) + ); + } + + public static Command getCollectFromNeutralZoneCommand(boolean shootPreload, double collectionTimeout) { + return new ParallelDeadlineGroup( + getDriveToFuelInNeutralZoneCommand(shootPreload, collectionTimeout), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE), + ShootingCommands.getShootAtHubCommand().onlyWhile(SafeAutonomousDriveCommands::isInAllianceZone) + ); + } + + public static Command getScoreCommand(double timeout) { + return new ParallelCommandGroup( + SafeAutonomousDriveCommands.getSafeDriveToPoseCommand( + () -> SafeAutonomousDriveCommands.isRight() ? FieldConstants.RIGHT_IDEAL_SHOOTING_POSITION : FieldConstants.LEFT_IDEAL_SHOOTING_POSITION, + AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, + 0, + AutonomousConstants.DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS, + 1000 + ), + ShootingCommands.getShootAtHubCommand().onlyWhile(SafeAutonomousDriveCommands::isInAllianceZone).repeatedly() + ).withTimeout(timeout); + } + + public static Command getCollectFromDepotCommand(boolean shootWhileDriving, double collectionTimeout) { + return new SequentialCommandGroup( + new ParallelCommandGroup( + SafeAutonomousDriveCommands.getSafeDriveToPoseCommand( + () -> FieldConstants.DEPOT_POSITION, + AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, + 0, + AutonomousConstants.DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS, + shootWhileDriving ? 1000 : 0 + ), + ShootingCommands.getShootAtHubCommand().onlyWhile(SafeAutonomousDriveCommands::isInAllianceZone).repeatedly() + ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.DEPOT_POSITION)), + new GamePieceAutoDriveCommand(true).alongWith(ShootingCommands.getShootAtHubCommand()).withTimeout(collectionTimeout) + ).alongWith(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.INTAKE)); + } + + public static Command getClimbCommand(FlippablePose2d climbPosition) { + return new SequentialCommandGroup( + SafeAutonomousDriveCommands.getSafeDriveToPoseCommand( + () -> climbPosition, + AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, + 0, + AutonomousConstants.DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS, + 1000 + ), + new InstantCommand() // TODO: Add climb command + ); + } + + private static Command getDriveToFuelInNeutralZoneCommand(boolean shootPreload, double timeout) { + return new SequentialCommandGroup( + SafeAutonomousDriveCommands.getSafeDriveToPoseCommand( + () -> SafeAutonomousDriveCommands.isRight() ? FieldConstants.RIGHT_INTAKE_POSITION : FieldConstants.LEFT_INTAKE_POSITION, + AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, + 3, + AutonomousConstants.SHOOT_PRELOAD_BEFORE_NEUTRAL_ZONE_DRIVE_CONSTRAINTS, + shootPreload ? AutonomousConstants.SHOOT_PRELOAD_BEFORE_NEUTRAL_ZONE_DRIVE_TIME : 0 + ), + new GamePieceAutoDriveCommand(false).withTimeout(timeout) + ); + } + + private static boolean shouldCollectGamePiecesFromNeutralZone() { + final Pose2d currentRobotPose = new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true).get(); + return currentRobotPose.getX() > FieldConstants.DELIVERY_ZONE_START_BLUE_X && RobotContainer.OBJECT_POSE_ESTIMATOR.hasObjects(); + } + + /** + * Creates a command that resets the pose estimator's pose to the starting pose of the given autonomous as long as the robot is not enabled. + * + * @param autoName the name of the autonomous + * @return a command that resets the robot's pose estimator pose to the start position of the given autonomous + */ + public static Command getResetPoseToAutoPoseCommand(Supplier autoName) { + return new InstantCommand( + () -> { + if (DriverStation.isEnabled()) + return; + RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(getAutoStartPose(autoName.get())); + } + ).ignoringDisable(true); + } + + /** + * Gets the starting position of the target PathPlanner autonomous. + * + * @param autoName the name of the autonomous group + * @return the staring pose of the autonomous + */ + public static Pose2d getAutoStartPose(String autoName) { + try { + final Pose2d nonFlippedAutoStartPose = PathPlannerAuto.getPathGroupFromAutoFile(autoName).get(0).getStartingHolonomicPose().get(); + final FlippablePose2d flippedAutoStartPose = new FlippablePose2d(nonFlippedAutoStartPose, true); + return flippedAutoStartPose.get(); + } catch (IOException | ParseException e) { + e.printStackTrace(); + return new Pose2d(); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/SafeAutonomousDriveCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/SafeAutonomousDriveCommands.java new file mode 100644 index 00000000..bbf92c2c --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/SafeAutonomousDriveCommands.java @@ -0,0 +1,107 @@ +package frc.trigon.robot.commands.commandfactories.autonomous; + +import com.pathplanner.lib.path.PathConstraints; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.trigon.lib.utilities.flippable.FlippablePose2d; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; + +import java.util.function.Supplier; + +public class SafeAutonomousDriveCommands { + public static Command getSafeDriveToPoseCommand(Supplier targetPose, PathConstraints normalPathConstrains) { + return getSafeDriveToPoseCommand(targetPose, normalPathConstrains, 0); + } + + public static Command getSafeDriveToPoseCommand(Supplier targetPose, PathConstraints normalPathConstrains, double endVelocity) { + return getSafeDriveToPoseCommand(targetPose, normalPathConstrains, endVelocity, null, 0); + } + + public static Command getSafeDriveToPoseCommand( + Supplier targetPose, PathConstraints normalPathConstrains, + double endVelocity, PathConstraints driveSlowlyInAllianceZoneConstraints, double driveSlowlyInAllianceZoneTime + ) { + return new ConditionalCommand( + getDriveThroughTrenchCommand(targetPose, normalPathConstrains, endVelocity, driveSlowlyInAllianceZoneConstraints, driveSlowlyInAllianceZoneTime), + getDriveSlowlyInAllianceZoneCommand(targetPose, AutonomousConstants.DRIVE_IN_AUTONOMOUS_CONSTRAINTS, endVelocity, driveSlowlyInAllianceZoneConstraints, driveSlowlyInAllianceZoneTime), + () -> shouldDriveThroughTrench(targetPose.get()) + ); + } + + private static Command getDriveThroughTrenchCommand( + Supplier targetPose, PathConstraints normalPathConstrains, + double endVelocity, PathConstraints driveSlowlyInAllianceZoneConstraints, double driveSlowlyInAllianceZoneTime + ) { + return new SequentialCommandGroup( + getDriveSlowlyInAllianceZoneCommand(SafeAutonomousDriveCommands::getTrenchEntryPose, normalPathConstrains, 4, driveSlowlyInAllianceZoneConstraints, driveSlowlyInAllianceZoneTime), + getDriveSlowlyInAllianceZoneCommand(() -> SafeAutonomousDriveCommands.getTrenchExitPose(targetPose.get()), normalPathConstrains, 4, driveSlowlyInAllianceZoneConstraints, driveSlowlyInAllianceZoneTime), + getDriveSlowlyInAllianceZoneCommand(targetPose, normalPathConstrains, endVelocity, driveSlowlyInAllianceZoneConstraints, driveSlowlyInAllianceZoneTime) + ); + } + + private static Command getDriveSlowlyInAllianceZoneCommand( + Supplier targetPose, PathConstraints normalPathConstrains, + double endVelocity, PathConstraints driveSlowlyInAllianceZoneConstraints, double driveSlowlyInAllianceZoneTime + ) { + if (driveSlowlyInAllianceZoneTime == 0) + return SwerveCommands.getDriveToPoseCommand(targetPose, normalPathConstrains, endVelocity); + + return new SequentialCommandGroup( + SwerveCommands.getDriveToPoseCommand(targetPose, normalPathConstrains, endVelocity).until(SafeAutonomousDriveCommands::isInAllianceZone), + SwerveCommands.getDriveToPoseCommand(targetPose, driveSlowlyInAllianceZoneConstraints, endVelocity).withTimeout(driveSlowlyInAllianceZoneTime).onlyWhile(SafeAutonomousDriveCommands::isInAllianceZone), + SwerveCommands.getDriveToPoseCommand(targetPose, normalPathConstrains, endVelocity) + ); + } + + private static boolean shouldDriveThroughTrench(FlippablePose2d targetPose) { + return (!isInAllianceZone() && isPoseInAllianceZone(targetPose)) || (isInAllianceZone() && !isPoseInAllianceZone(targetPose)); + } + + private static FlippablePose2d getTrenchExitPose(FlippablePose2d targetPose) { + final FlippablePose2d targetTrenchExitPose = isRight() ? + getClosestPoseToPose(targetPose, FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE, FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE) : + getClosestPoseToPose(targetPose, FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE, FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE); + if (RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() > 90 || RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() < -90) + return new FlippablePose2d(targetTrenchExitPose.getBlueObject().getTranslation(), Math.PI, true); + return targetTrenchExitPose; + } + + private static FlippablePose2d getTrenchEntryPose() { + final FlippablePose2d targetTrenchEntryPose = isRight() ? + isInAllianceZone() ? FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE : FieldConstants.RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE : + isInAllianceZone() ? FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE : FieldConstants.LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE; + if (RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getDegrees() > 90) + return new FlippablePose2d(targetTrenchEntryPose.getBlueObject().getTranslation(), Math.PI, true); + return targetTrenchEntryPose; + } + + private static FlippablePose2d getClosestPoseToPose(FlippablePose2d pose, FlippablePose2d... poses) { + FlippablePose2d closestPose = null; + double closestDistance = Double.MAX_VALUE; + for (FlippablePose2d candidatePose : poses) { + final double distance = pose.get().getTranslation().getDistance(candidatePose.get().getTranslation()); + if (distance < closestDistance) { + closestDistance = distance; + closestPose = candidatePose; + } + } + return closestPose; + } + + public static boolean isRight() { + return RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation().getY() < FieldConstants.FIELD_WIDTH_METERS / 2; + } + + public static boolean isInAllianceZone() { + return isPoseInAllianceZone(new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true)); + } + + private static boolean isPoseInAllianceZone(FlippablePose2d pose) { + return pose.get().getX() < FieldConstants.ALLIANCE_ZONE_LENGTH; + } + +} diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 7842e1da..b98cb629 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -8,17 +8,21 @@ import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.pathfinding.Pathfinding; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.trigon.lib.hardware.RobotHardwareStats; import frc.trigon.lib.utilities.LocalADStarAK; import frc.trigon.lib.utilities.flippable.Flippable; +import frc.trigon.lib.utilities.flippable.FlippablePose2d; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.autonomous.GeneralAutonomousCommands; import org.json.simple.parser.ParseException; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import java.io.IOException; +import java.util.function.Supplier; /** * A class that contains the constants and configurations for everything related to the 15-second autonomous period at the start of the match. @@ -26,25 +30,43 @@ public class AutonomousConstants { public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); - public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate - public static final PathConstraints DRIVE_TO_SCORING_LOCATION_CONSTRAINTS = new PathConstraints(2.5, 4.5, Units.degreesToRadians(450), Units.degreesToRadians(900)); + public static final double FEEDFORWARD_SCALAR = 0.7;//TODO: Calibrate + public static final PathConstraints + DRIVE_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(4, 7, Units.degreesToRadians(100), Units.degreesToRadians(100)), + SHOOT_PRELOAD_BEFORE_NEUTRAL_ZONE_DRIVE_CONSTRAINTS = new PathConstraints(0.2, 0.5, Units.degreesToRadians(100), Units.degreesToRadians(100)), + SHOOT_PRELOAD_BEFORE_COLLECTING_FROM_DEPOT_CONSTRAINTS = new PathConstraints(1.5, 3.0, Units.degreesToRadians(100), Units.degreesToRadians(100)), + DRIVE_SLOWLY_IN_AUTONOMOUS_CONSTRAINTS = new PathConstraints(2.5, 2, Units.degreesToRadians(100), Units.degreesToRadians(100)); + public static final double + SHOOT_PRELOAD_BEFORE_NEUTRAL_ZONE_DRIVE_TIME = 1, + SHOOT_PRELOAD_BEFORE_COLLECTING_FROM_DEPOT_TIME = 2; + public static LoggedDashboardChooser> + FIRST_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("FirstAutonomousChooser", new SendableChooser<>()), + SECOND_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("SecondAutonomousChooser", new SendableChooser<>()), + THIRD_AUTONOMOUS_CHOOSER = new LoggedDashboardChooser<>("ThirdAutonomousChooser", new SendableChooser<>()); + public static LoggedDashboardChooser CLIMB_POSITION_CHOOSER = new LoggedDashboardChooser<>("ClimbChooser", new SendableChooser<>()); + + public static double + DEPOT_COLLECTION_TIMEOUT_SECONDS = 6, + NEUTRAL_ZONE_COLLECTION_TIMEOUT_SECONDS = 1.5, + DELIVERY_TIMEOUT_SECONDS = 8, + SCORING_TIMEOUT_SECONDS = 6; private static final PIDConstants AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? - new PIDConstants(0, 0, 0) : + new PIDConstants(9, 0, 0) : new PIDConstants(0, 0, 0), AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? - new PIDConstants(0, 0, 0) : + new PIDConstants(2, 0, 0) : new PIDConstants(0, 0, 0); public static final PIDController GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new PIDController(0.5, 0, 0) : new PIDController(0.3, 0, 0.03); - public static final ProfiledPIDController GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? - new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : - new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); - public static final double AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS = 2; + public static final PIDController GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new PIDController(0.5, 0, 0) : + new PIDController(1, 0, 0); + public static final double AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS = 0.01; private static final PPHolonomicDriveController AUTO_PATH_FOLLOWING_CONTROLLER = new PPHolonomicDriveController( AUTO_TRANSLATION_PID_CONSTANTS, @@ -58,7 +80,8 @@ public static void init() { Pathfinding.setPathfinder(new LocalADStarAK()); CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand()); configureAutoBuilder(); - registerCommands(); + initializeAutoChoosers(); + } private static void configureAutoBuilder() { @@ -82,7 +105,25 @@ private static RobotConfig getRobotConfig() { } } - private static void registerCommands() { - //TODO: Implement + private static void initializeAutoChoosers() { + configureClimbPositionChooser(); + configureAutonomousPositionChooser(FIRST_AUTONOMOUS_CHOOSER); + configureAutonomousPositionChooser(SECOND_AUTONOMOUS_CHOOSER); + configureAutonomousPositionChooser(THIRD_AUTONOMOUS_CHOOSER); + } + + private static void configureAutonomousPositionChooser(LoggedDashboardChooser> firstAutonomousChooser) { + firstAutonomousChooser.addOption("Depot", () -> GeneralAutonomousCommands.getCollectFromDepotCommand(true, 6)); + firstAutonomousChooser.addOption("Score", () -> GeneralAutonomousCommands.getScoreCommand(4)); + firstAutonomousChooser.addOption("CollectFromNeutralZone", () -> GeneralAutonomousCommands.getCollectFromNeutralZoneCommand(true, 1.5)); + firstAutonomousChooser.addOption("Delivery", () -> GeneralAutonomousCommands.getDeliveryCommand(true, 6)); + firstAutonomousChooser.addDefaultOption("Nothing", null); + } + + private static void configureClimbPositionChooser() { + CLIMB_POSITION_CHOOSER.addOption("LeftClimb", FieldConstants.LEFT_CLIMB_POSITION); + CLIMB_POSITION_CHOOSER.addOption("CenterClimb", FieldConstants.CENTER_CLIMB_POSITION); + CLIMB_POSITION_CHOOSER.addOption("RightClimb", FieldConstants.RIGHT_CLIMB_POSITION); + CLIMB_POSITION_CHOOSER.addDefaultOption("NoClimb", null); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index c3b76c0b..fcc51d4f 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -5,9 +5,11 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; 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 frc.trigon.lib.utilities.FilesHandler; +import frc.trigon.lib.utilities.flippable.FlippablePose2d; import frc.trigon.lib.utilities.flippable.FlippableTranslation2d; import java.io.IOException; @@ -27,16 +29,29 @@ public class FieldConstants { private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIDToPoseMap(); + public static final FlippablePose2d + LEFT_CLIMB_POSITION = new FlippablePose2d(1.45, 4.25, Rotation2d.fromDegrees(0), true), + RIGHT_CLIMB_POSITION = new FlippablePose2d(LEFT_CLIMB_POSITION.getBlueObject().getX(), 3.28, Rotation2d.fromDegrees(0), true), + CENTER_CLIMB_POSITION = new FlippablePose2d((LEFT_CLIMB_POSITION.getBlueObject().getX() + LEFT_CLIMB_POSITION.getBlueObject().getX()) / 2, LEFT_CLIMB_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(0), true), + DEPOT_POSITION = new FlippablePose2d(0.45, 7, Rotation2d.fromDegrees(-90), true), + LEFT_INTAKE_POSITION = new FlippablePose2d(7.4, 7.3, Rotation2d.fromDegrees(-75), true), + RIGHT_INTAKE_POSITION = new FlippablePose2d(LEFT_INTAKE_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_INTAKE_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(40), true), + LEFT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(2.7, 5.8, Rotation2d.fromDegrees(0), true), + RIGHT_IDEAL_SHOOTING_POSITION = new FlippablePose2d(LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_IDEAL_SHOOTING_POSITION.getBlueObject().getY(), Rotation2d.fromDegrees(0), true), + LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE = new FlippablePose2d(3.9, 7.4, Rotation2d.kZero, true), + RIGHT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE = new FlippablePose2d(LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE.getBlueObject().getY(), Rotation2d.kZero, true), + LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE = new FlippablePose2d(5.53, LEFT_TRENCH_ENTRY_POSITION_FROM_ALLIANCE_ZONE.getBlueObject().getY(), Rotation2d.kZero, true), + RIGHT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE = new FlippablePose2d(LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE.getBlueObject().getX(), FIELD_WIDTH_METERS - LEFT_TRENCH_ENTRY_POSITION_FROM_NEUTRAL_ZONE.getBlueObject().getY(), Rotation2d.kZero, true); private static final double BLUE_RELATIVE_DELIVERY_POSITION_X = 3, DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS = 2.2; public static final FlippableTranslation2d - HUB_POSITION = new FlippableTranslation2d(4.625594, FIELD_WIDTH_METERS / 2, true), + HUB_POSITION = new FlippableTranslation2d(4.7, FIELD_WIDTH_METERS / 2, true), RIGHT_DELIVERY_POSITION = new FlippableTranslation2d(BLUE_RELATIVE_DELIVERY_POSITION_X, (FIELD_WIDTH_METERS / 2) - DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS, true), LEFT_DELIVERY_POSITION = new FlippableTranslation2d(BLUE_RELATIVE_DELIVERY_POSITION_X, (FIELD_WIDTH_METERS / 2) + DELIVERY_POSITION_Y_OFFSET_FROM_CENTER_METERS, true); public static final double - ALLIANCE_ZONE_LENGTH = 4.625594, - DELIVERY_ZONE_START_BLUE_X = ALLIANCE_ZONE_LENGTH + 0.5; + ALLIANCE_ZONE_LENGTH = 4.5, + DELIVERY_ZONE_START_BLUE_X = ALLIANCE_ZONE_LENGTH + 1; private static AprilTagFieldLayout createAprilTagFieldLayout() { try { diff --git a/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java b/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java index aa84bca2..e8f9466a 100644 --- a/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java +++ b/src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java @@ -132,12 +132,12 @@ private Rotation2d calculateTargetRobotAngle(Translation2d robotFieldRelativeVel return robotAngleToHub.plus(targetSelfRelativeYaw); } - private Translation2d calculateVelocityRelativeToPoint(Translation2d fieldPoint, Translation2d currentPosition, Translation2d robotFieldRelativeVelocity) { + public static Translation2d calculateVelocityRelativeToPoint(Translation2d fieldPoint, Translation2d currentPosition, Translation2d robotFieldRelativeVelocity) { final Rotation2d angleToPoint = calculateAngleToPoint(fieldPoint, currentPosition); return robotFieldRelativeVelocity.rotateBy(angleToPoint.unaryMinus()); } - private Rotation2d calculateAngleToPoint(Translation2d fieldPoint, Translation2d currentPosition) { + public static Rotation2d calculateAngleToPoint(Translation2d fieldPoint, Translation2d currentPosition) { return fieldPoint.minus(currentPosition).getAngle(); } } diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java index db021ed0..277bed2e 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -45,6 +45,10 @@ public class SimulatedGamePieceConstants { STARTING_FUEL_Y_POSITION_METERS = 1.724406, STARTING_FUEL_SPACING_METERS = 0.16; + private static final int + DEPOT_FUEL_ROWS = 6, + DEPOT_FUEL_COLUMNS = 4; + public static final double EJECTION_FROM_HUB_MINIMUM_VELOCITY_METERS_PER_SECOND = 4, EJECTION_FROM_HUB_MAXIMUM_VELOCITY_METERS_PER_SECOND = 15; @@ -63,6 +67,27 @@ private static void initializeFuel() { ); } } + + for (int i = 0; i <8; i++) { + final SimulatedGamePiece currentHeldFuel = new SimulatedGamePiece(0, 0); + SimulationFieldHandler.addHeldFuel(currentHeldFuel); + } + + initializeDepotFuel(0.31, 5.96); + } + + private static void initializeDepotFuel(double depotCenterX, double depotCenterY) { + double startX = depotCenterX - (DEPOT_FUEL_COLUMNS - 1) * STARTING_FUEL_SPACING_METERS / 2.0; + double startY = depotCenterY - (DEPOT_FUEL_ROWS - 1) * STARTING_FUEL_SPACING_METERS / 2.0; + + for (int row = 0; row < DEPOT_FUEL_ROWS; row++) { + for (int col = 0; col < DEPOT_FUEL_COLUMNS; col++) { + new SimulatedGamePiece( + startX + col * STARTING_FUEL_SPACING_METERS, + startY + row * STARTING_FUEL_SPACING_METERS + ); + } + } } public enum GamePieceType { diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java index 3a82cb79..e84c825a 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -43,12 +43,16 @@ private static void updateCollection() { if (HELD_FUEL.size() >= SimulatedGamePieceConstants.MAXIMUM_HELD_FUEL) return; - HELD_FUEL.add(fuel); - fuel.resetIndexing(); + addHeldFuel(fuel); } } } + public static void addHeldFuel(SimulatedGamePiece fuel) { + HELD_FUEL.add(fuel); + fuel.resetIndexing(); + } + /** * Gets the fuel object that is being collected. * diff --git a/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java index 24083906..01cd2f0d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/hood/HoodConstants.java @@ -68,7 +68,7 @@ public class HoodConstants { null ); - static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.5); + static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(1); static final Rotation2d REST_ANGLE = Rotation2d.fromDegrees(87), DELIVERY_ANGLE = Rotation2d.fromDegrees(50), diff --git a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java index 40e5d08e..520923d4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/shooter/ShooterConstants.java @@ -49,7 +49,7 @@ public class ShooterConstants { ); static final double EJECTION_VELOCITY_METERS_PER_SECOND = 3; - static final double VELOCITY_TOLERANCE_METERS_PER_SECOND = 0.1; + static final double VELOCITY_TOLERANCE_METERS_PER_SECOND = 0.4; static final double WHEEL_SLIPPAGE_COMPENSATION_VELOCITY_MULTIPLIER = RobotHardwareStats.isSimulation() ? 1 : 1.05; static final double DELIVERY_VELOCITY_SLOPE = 0.9, diff --git a/src/main/java/frc/trigon/robot/subsystems/spindexer/SpindexerConstants.java b/src/main/java/frc/trigon/robot/subsystems/spindexer/SpindexerConstants.java index f7fa1f8c..a2adb04c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/spindexer/SpindexerConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/spindexer/SpindexerConstants.java @@ -84,7 +84,7 @@ public class SpindexerConstants { } public enum SpindexerState { - LOAD_TO_TURRET(1), + LOAD_TO_TURRET(2), STOP(0); public final double targetVelocityRotationsPerSecond; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index 68b6b02b..a00c4c9f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -323,6 +323,7 @@ private SwerveModulePosition[] getSwerveWheelPositions(int odometryUpdateIndex) } private boolean atTranslationPosition(double currentTranslationPosition, double targetTranslationPosition, double currentTranslationVelocity) { +// System.out.println("Current Pos: " + currentTranslationPosition + ", Target Pos: " + targetTranslationPosition + ", Current Vel: " + currentTranslationVelocity); return Math.abs(currentTranslationPosition - targetTranslationPosition) < SwerveConstants.TRANSLATION_TOLERANCE_METERS && Math.abs(currentTranslationVelocity) < SwerveConstants.TRANSLATION_VELOCITY_TOLERANCE; } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java index 243d5e3f..be98a67c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -3,10 +3,10 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.wpilibj2.command.*; -import frc.trigon.robot.RobotContainer; import frc.trigon.lib.commands.InitExecuteCommand; import frc.trigon.lib.utilities.flippable.FlippablePose2d; import frc.trigon.lib.utilities.flippable.FlippableRotation2d; +import frc.trigon.robot.RobotContainer; import java.util.Set; import java.util.function.DoubleSupplier; @@ -129,25 +129,18 @@ public static Command getOpenLoopSelfRelativeDriveCommand(DoubleSupplier xSuppli } public static Command getDriveToPoseCommand(Supplier targetPose, PathConstraints constraints) { - return new DeferredCommand(() -> getCurrentDriveToPoseCommand(targetPose.get(), constraints), Set.of(RobotContainer.SWERVE)); + return getDriveToPoseCommand(targetPose, constraints, 0.0); } public static Command getDriveToPoseCommand(Supplier targetPose, PathConstraints constraints, double endVelocity) { return new DeferredCommand(() -> getCurrentDriveToPoseCommand(targetPose.get(), constraints, endVelocity), Set.of(RobotContainer.SWERVE)); } - private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, PathConstraints constraints) { - return new SequentialCommandGroup( - new InstantCommand(() -> RobotContainer.SWERVE.initializeDrive(true)), - AutoBuilder.pathfindToPose(targetPose.get(), constraints), - getPIDToPoseCommand(targetPose) - ); - } - private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, PathConstraints constraints, double endVelocity) { return new SequentialCommandGroup( new InstantCommand(() -> RobotContainer.SWERVE.initializeDrive(true)), - AutoBuilder.pathfindToPose(targetPose.get(), constraints, endVelocity) + AutoBuilder.pathfindToPose(targetPose.get(), constraints, endVelocity), + getPIDToPoseCommand(targetPose).onlyIf(() -> endVelocity < 1e-3) ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java index 5a0b67c1..4aa05fad 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java @@ -15,7 +15,7 @@ public class SwerveModuleConstants { private static final double - DRIVE_MOTOR_GEAR_RATIO = 6.03,//R1: 7.03, R2: 6.03, R3: 5.27 + DRIVE_MOTOR_GEAR_RATIO = 7.03,//R1: 7.03, R2: 6.03, R3: 5.27 STEER_MOTOR_GEAR_RATIO = 287.0 / 11.0; static final boolean ENABLE_FOC = true; @@ -80,9 +80,9 @@ static TalonFXConfiguration generateDriveMotorConfiguration() { config.Slot0.kP = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016046 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.8774 : 0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 00.020691 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.0010955 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.74583 : 0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.01247 : 0; config.Feedback.VelocityFilterTimeConstant = 0; diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java index a8a09cd7..ba3b505c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/trigon/robot/subsystems/turret/Turret.java @@ -106,9 +106,24 @@ public Rotation2d getCurrentFieldRelativeAngle() { return getCurrentSelfRelativeAngle().plus(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation()); } + public boolean atTargetShootingCalculationsAngle(boolean useWideTolerance) { + return atFieldRelativeAngle(shootingCalculations.getTargetShootingState().targetFieldRelativeYaw(), useWideTolerance); + } + public boolean atTargetAngle(boolean useWideTolerance) { - return Math.abs(targetSelfRelativeAngle.minus(getCurrentSelfRelativeAngle()).getRadians()) - < (useWideTolerance ? TurretConstants.WIDE_TOLERANCE.getRadians() : TurretConstants.NORMAL_TOLERANCE.getRadians()); + return atSelfRelativeAngle(targetSelfRelativeAngle, useWideTolerance); + } + + public boolean atFieldRelativeAngle(Rotation2d fieldRelativeAngle, boolean useWideTolerance) { + final double differenceRadians = Math.abs(fieldRelativeAngle.minus(getCurrentFieldRelativeAngle()).getRadians()); + final double toleranceRadians = useWideTolerance ? TurretConstants.WIDE_TOLERANCE.getRadians() : TurretConstants.NORMAL_TOLERANCE.getRadians(); + return differenceRadians < toleranceRadians; + } + + public boolean atSelfRelativeAngle(Rotation2d selfRelativeAngle, boolean useWideTolerance) { + final double differenceRadians = Math.abs(selfRelativeAngle.minus(getCurrentSelfRelativeAngle()).getRadians()); + final double toleranceRadians = useWideTolerance ? TurretConstants.WIDE_TOLERANCE.getRadians() : TurretConstants.NORMAL_TOLERANCE.getRadians(); + return differenceRadians < toleranceRadians; } public Rotation2d getCurrentSelfRelativeAngle() { diff --git a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java index 3a9ae6fd..b1fde27d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/turret/TurretConstants.java @@ -35,10 +35,10 @@ public class TurretConstants { static final CANcoderEncoder ENCODER = new CANcoderEncoder(ENCODER_ID, ENCODER_NAME); static final boolean FOC_ENABLED = true; - private static final double GEAR_RATIO = 52; + private static final double GEAR_RATIO = 64.8; private static final double CURRENT_LIMIT_AMPS = 100; private static final MotorAlignmentValue FOLLOWER_ALIGNMENT_TO_MASTER = MotorAlignmentValue.Aligned; - static final double RESIST_SWERVE_ROTATION_FEEDFORWARD_GAIN = RobotHardwareStats.isSimulation() ? 0.5 : 0; + static final double RESIST_SWERVE_ROTATION_FEEDFORWARD_GAIN = RobotHardwareStats.isSimulation() ? 0 : 0; private static final int MOTOR_AMOUNT = 2; private static final DCMotor GEARBOX = DCMotor.getFalcon500Foc(MOTOR_AMOUNT); @@ -67,11 +67,11 @@ public class TurretConstants { ); static final Rotation2d - MAXIMUM_ANGLE = Rotation2d.fromDegrees(179.5), - MINIMUM_ANGLE = Rotation2d.fromDegrees(-179.5), + MAXIMUM_ANGLE = Rotation2d.fromDegrees(370), + MINIMUM_ANGLE = Rotation2d.fromDegrees(-10), TOTAL_ANGULAR_RANGE = MAXIMUM_ANGLE.minus(MINIMUM_ANGLE); static final Rotation2d - NORMAL_TOLERANCE = Rotation2d.fromDegrees(2), + NORMAL_TOLERANCE = Rotation2d.fromDegrees(3), WIDE_TOLERANCE = Rotation2d.fromDegrees(15); static final double ROBOT_VELOCITY_TO_FUTURE_ANGLE_SECONDS = 0.2; static final double RESIST_Y_MOVEMENT_FOR_DELIVERY_COEFFICIENT = 10; @@ -93,15 +93,15 @@ private static void configureMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.ClosedLoopGeneral.GainSchedKpBehavior = GainSchedKpBehaviorValue.Discontinuous; + config.ClosedLoopGeneral.GainSchedKpBehavior = GainSchedKpBehaviorValue.Continuous; config.ClosedLoopGeneral.GainSchedErrorThreshold = 0.007; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 75 : 0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 270 : 0; config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.3 : 0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.02 : 0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 6.2 : 0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.6 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.01 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 7.5 : 0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.0005 : 0; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; config.Slot0.GainSchedBehavior = GainSchedBehaviorValue.Inactive; @@ -113,7 +113,7 @@ private static void configureMasterMotor() { config.Slot1.kA = 0; config.Slot1.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign; - config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 1.5 : 5; + config.MotionMagic.MotionMagicCruiseVelocity = RobotHardwareStats.isSimulation() ? 1.4 : 5; config.MotionMagic.MotionMagicAcceleration = RobotHardwareStats.isSimulation() ? 90 : 5; config.MotionMagic.MotionMagicJerk = 0;