diff --git a/src/main/java/frc/robot/commands/drive/JoystickDrive.java b/src/main/java/frc/robot/commands/drive/JoystickDrive.java
index f2ab04e..8f54e95 100644
--- a/src/main/java/frc/robot/commands/drive/JoystickDrive.java
+++ b/src/main/java/frc/robot/commands/drive/JoystickDrive.java
@@ -47,6 +47,7 @@ public JoystickDrive(
super.addRequirements(driveSubsystem);
resetSensitivity();
+ SwerveDrive.swerveHeadingController.setHeadingRequest(new ChassisHeadingController.NullRequest());
}
@Override
diff --git a/src/main/java/frc/robot/utils/AIRobotInSimulation.java b/src/main/java/frc/robot/utils/AIRobotInSimulation.java
index b0a77a9..64085b0 100644
--- a/src/main/java/frc/robot/utils/AIRobotInSimulation.java
+++ b/src/main/java/frc/robot/utils/AIRobotInSimulation.java
@@ -1,6 +1,6 @@
package frc.robot.utils;
-import static edu.wpi.first.units.Units.Kilograms;
+import static edu.wpi.first.units.Units.*;
import com.pathplanner.lib.commands.FollowPathCommand;
import com.pathplanner.lib.config.ModuleConfig;
@@ -18,10 +18,7 @@
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.*;
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import java.util.Arrays;
import java.util.function.Supplier;
@@ -32,8 +29,23 @@
import org.ironmaple.simulation.seasonspecific.crescendo2024.NoteOnFly;
import org.ironmaple.utils.FieldMirroringUtils;
-public class AIRobotInSimulation implements Subsystem {
- /* if an opponent robot is not requested to be on field, it queens outside the field for performance */
+/**
+ *
+ *
+ * Represents an AI robot during simulation.
+ *
+ * This class models an AI-controlled robot used during simulation for driver practice and skill development.
+ *
+ *
The AI robots are capable of performing various tasks to enhance training scenarios, including:
+ *
+ *
+ * - Automatically cycling around the field to help practice offensive skills.
+ *
- Delivering feed-shots to assist in practicing front-field clean-ups.
+ *
- Driving via joystick control to simulate defense and counter-defense scenarios.
+ *
+ */
+public class AIRobotInSimulation extends SubsystemBase {
+ /* If an opponent robot is not requested to be on the field, it is placed ("queens") outside the field at predefined positions. */
public static final Pose2d[] ROBOT_QUEENING_POSITIONS = new Pose2d[] {
new Pose2d(-6, 0, new Rotation2d()),
new Pose2d(-5, 0, new Rotation2d()),
@@ -41,6 +53,7 @@ public class AIRobotInSimulation implements Subsystem {
new Pose2d(-3, 0, new Rotation2d()),
new Pose2d(-2, 0, new Rotation2d())
};
+ /* The robots will be teleported to these positions when teleop begins. */
public static final Pose2d[] ROBOTS_STARTING_POSITIONS = new Pose2d[] {
new Pose2d(15, 6, Rotation2d.fromDegrees(180)),
new Pose2d(15, 4, Rotation2d.fromDegrees(180)),
@@ -48,12 +61,15 @@ public class AIRobotInSimulation implements Subsystem {
new Pose2d(1.6, 6, new Rotation2d()),
new Pose2d(1.6, 4, new Rotation2d())
};
+ /* Store instances of AI robots in a static array. */
public static final AIRobotInSimulation[] instances = new AIRobotInSimulation[5];
- private static final DriveTrainSimulationConfig AI_ROBOT_CONFIG =
+ /* The drivetrain configuration for the opponent robots in the maple-sim simulation. */
+ private static final DriveTrainSimulationConfig DRIVETRAIN_CONFIG =
DriveTrainSimulationConfig.Default().withRobotMass(Kilograms.of(45));
- private static final RobotConfig robotConfig = new RobotConfig(
+ /* The PathPlanner configuration for the opponent robots. */
+ private static final RobotConfig PP_CONFIG = new RobotConfig(
55,
8,
new ModuleConfig(
@@ -63,143 +79,127 @@ public class AIRobotInSimulation implements Subsystem {
private static final PPHolonomicDriveController driveController =
new PPHolonomicDriveController(new PIDConstants(5.0, 0.02), new PIDConstants(7.0, 0.05));
+ /**
+ *
+ *
+ * Activates the opponent robots.
+ *
+ * The opponent robots will not appear on the field immediately. They are initially placed at
+ * the {@link #ROBOT_QUEENING_POSITIONS}.
+ *
+ *
Instead of being active right away, a sendable chooser is sent to the dashboard, allowing the user to select
+ * the mode of these robots.
+ */
public static void startOpponentRobotSimulations() {
try {
- instances[0] = new AIRobotInSimulation(
+ // Creates an instance of the first AI robot
+ instances[0] = new AIRobotInSimulation(0);
+ // Builds the behavior chooser for the first AI robot
+ instances[0].buildBehaviorChooser(
PathPlannerPath.fromPathFile("opponent robot cycle path 0"),
Commands.none(),
PathPlannerPath.fromPathFile("opponent robot cycle path 0 backwards"),
Commands.none(),
- ROBOT_QUEENING_POSITIONS[0],
- 1);
- instances[1] = new AIRobotInSimulation(
+ new XboxController(2));
+
+ // Same of the following:
+
+ instances[1] = new AIRobotInSimulation(1);
+ instances[1].buildBehaviorChooser(
PathPlannerPath.fromPathFile("opponent robot cycle path 1"),
- shootAtSpeaker(1),
+ instances[1].shootAtSpeaker(),
PathPlannerPath.fromPathFile("opponent robot cycle path 1 backwards"),
Commands.none(),
- ROBOT_QUEENING_POSITIONS[1],
- 2);
- instances[2] = new AIRobotInSimulation(
+ new XboxController(3));
+
+ instances[2] = new AIRobotInSimulation(2);
+ instances[2].buildBehaviorChooser(
PathPlannerPath.fromPathFile("opponent robot cycle path 2"),
- shootAtSpeaker(2),
+ instances[2].shootAtSpeaker(),
PathPlannerPath.fromPathFile("opponent robot cycle path 2 backwards"),
Commands.none(),
- ROBOT_QUEENING_POSITIONS[2],
- 3);
- instances[3] = new AIRobotInSimulation(
+ new XboxController(4));
+
+ instances[3] = new AIRobotInSimulation(3);
+ instances[3].buildBehaviorChooser(
PathPlannerPath.fromPathFile("opponent robot cycle path 3"),
- feedShotLow(),
+ instances[3].feedShotLow(),
PathPlannerPath.fromPathFile("opponent robot cycle path 3 backwards"),
Commands.none(),
- ROBOT_QUEENING_POSITIONS[3],
- 4);
- instances[4] = new AIRobotInSimulation(
+ new XboxController(5));
+
+ instances[4] = new AIRobotInSimulation(4);
+ instances[4].buildBehaviorChooser(
PathPlannerPath.fromPathFile("opponent robot cycle path 4"),
- feedShotHigh(),
+ instances[4].feedShotHigh(),
PathPlannerPath.fromPathFile("opponent robot cycle path 4 backwards"),
Commands.none(),
- ROBOT_QUEENING_POSITIONS[4],
- 5);
+ new XboxController(6));
} catch (Exception e) {
DriverStation.reportError("failed to load opponent robot simulation path, error:" + e.getMessage(), false);
}
}
- private static Command shootAtSpeaker(int index) {
- return Commands.runOnce(() -> SimulatedArena.getInstance()
- .addGamePieceProjectile(new NoteOnFly(
- instances[index]
- .driveSimulation
- .getActualPoseInSimulationWorld()
- .getTranslation(),
- new Translation2d(0.3, 0),
- instances[index].driveSimulation.getActualSpeedsFieldRelative(),
- instances[index]
- .driveSimulation
- .getActualPoseInSimulationWorld()
- .getRotation(),
- 0.5,
- 10,
- Math.toRadians(60))
- .asSpeakerShotNote(() -> {})));
- }
+ private final SimplifiedSwerveDriveSimulation driveSimulation;
+ private final Pose2d queeningPose;
+ private final int id;
- private static Command feedShotLow() {
- final int index = 3;
- return Commands.runOnce(() -> SimulatedArena.getInstance()
- .addGamePieceProjectile(new NoteOnFly(
- instances[index]
- .driveSimulation
- .getActualPoseInSimulationWorld()
- .getTranslation(),
- new Translation2d(0.3, 0),
- instances[index].driveSimulation.getActualSpeedsFieldRelative(),
- instances[index]
- .driveSimulation
- .getActualPoseInSimulationWorld()
- .getRotation(),
- 0.5,
- 10,
- Math.toRadians(20))
- .enableBecomeNoteOnFieldAfterTouchGround()));
- }
+ public AIRobotInSimulation(int id) {
+ this.id = id;
+ this.queeningPose = ROBOT_QUEENING_POSITIONS[id];
+ this.driveSimulation =
+ new SimplifiedSwerveDriveSimulation(new SwerveDriveSimulation(DRIVETRAIN_CONFIG, queeningPose));
- private static Command feedShotHigh() {
- final int index = 4;
- return Commands.runOnce(() -> SimulatedArena.getInstance()
- .addGamePieceProjectile(new NoteOnFly(
- instances[index]
- .driveSimulation
- .getActualPoseInSimulationWorld()
- .getTranslation(),
- new Translation2d(0.3, 0),
- instances[index].driveSimulation.getActualSpeedsFieldRelative(),
- instances[index]
- .driveSimulation
- .getActualPoseInSimulationWorld()
- .getRotation(),
- 0.5,
- 10,
- Math.toRadians(55))
- .enableBecomeNoteOnFieldAfterTouchGround()));
+ SimulatedArena.getInstance().addDriveTrainSimulation(driveSimulation.getDriveTrainSimulation());
}
- private final SimplifiedSwerveDriveSimulation driveSimulation;
- private final int id;
-
- public AIRobotInSimulation(
+ /**
+ * Builds the behavior chooser for this opponent robot and sends it to the dashboard. This allows the user to select
+ * the robot's behavior mode during simulation.
+ */
+ public void buildBehaviorChooser(
PathPlannerPath segment0,
Command toRunAtEndOfSegment0,
PathPlannerPath segment1,
Command toRunAtEndOfSegment1,
- Pose2d queeningPose,
- int id) {
- this.id = id;
- this.driveSimulation =
- new SimplifiedSwerveDriveSimulation(new SwerveDriveSimulation(AI_ROBOT_CONFIG, queeningPose));
+ XboxController joystick) {
SendableChooser behaviorChooser = new SendableChooser<>();
- behaviorChooser.setDefaultOption(
- "None", Commands.run(() -> driveSimulation.setSimulationWorldPose(queeningPose), this));
+
+ // Supplier to disable the robot: sets the robot's pose to the queening position and stops its movement
+ final Supplier disable =
+ () -> Commands.runOnce(() -> driveSimulation.setSimulationWorldPose(queeningPose), this)
+ .andThen(Commands.runOnce(() -> driveSimulation.runChassisSpeeds(
+ new ChassisSpeeds(), new Translation2d(), false, false)))
+ .ignoringDisable(true);
+
+ // The option to disable the robot
+ behaviorChooser.setDefaultOption("Disable", disable.get());
+
+ // The option for the robot to automatically cycle around the field
behaviorChooser.addOption(
"Auto Cycle", getAutoCycleCommand(segment0, toRunAtEndOfSegment0, segment1, toRunAtEndOfSegment1));
- behaviorChooser.addOption("Joystick Drive", getJoystickDriveCommand());
+
+ // The option for manual joystick control of the robot
+ behaviorChooser.addOption("Joystick Drive", joystickDrive(joystick));
+
+ // Schedule the selected command when another behavior is chosen
behaviorChooser.onChange((Command::schedule));
+
+ // Schedule the command when teleop mode is enabled
RobotModeTriggers.teleop()
.onTrue(Commands.runOnce(() -> behaviorChooser.getSelected().schedule()));
- RobotModeTriggers.disabled()
- .onTrue(Commands.runOnce(
- () -> {
- driveSimulation.setSimulationWorldPose(queeningPose);
- driveSimulation.runChassisSpeeds(
- new ChassisSpeeds(), new Translation2d(), false, false);
- },
- this)
- .ignoringDisable(true));
+ // Disable the robot when the user robot is disabled
+ RobotModeTriggers.disabled().onTrue(disable.get());
+
+ // Display the behavior chooser on the dashboard for the user to select the desired robot behavior
SmartDashboard.putData("AIRobotBehaviors/Opponent Robot " + id + " Behavior", behaviorChooser);
- SimulatedArena.getInstance().addDriveTrainSimulation(driveSimulation.getDriveTrainSimulation());
}
+ /**
+ * Retrieves the command that drives the robot to automatically cycle around the field. The robot's movement is
+ * based on a relative path for auto-cycling.
+ */
private Command getAutoCycleCommand(
PathPlannerPath segment0,
Command toRunAtEndOfSegment0,
@@ -219,35 +219,58 @@ private Command getAutoCycleCommand(
FieldMirroringUtils.toCurrentAlliancePose(startingPose))));
}
+ /**
+ * Retrieves the command that drives the robot to automatically cycle around the field. The robot's movement is
+ * based on a relative path for auto-cycling.
+ */
private Command opponentRobotFollowPath(PathPlannerPath path) {
return new FollowPathCommand(
- path,
+ path, // Specify the path the opponent robot should follow
+ // Provide PathPlanner with the actual robot pose in the simulation, ignoring odometry error
driveSimulation::getActualPoseInSimulationWorld,
+ // Provide PathPlanner with the actual robot speed in the simulation, ignoring encoder measurement error
driveSimulation::getActualSpeedsRobotRelative,
- (speeds, feedForwards) -> driveSimulation.runChassisSpeeds(speeds, new Translation2d(), false, false),
- driveController,
- robotConfig,
- FieldMirroringUtils::isSidePresentedAsRed,
- this);
+ // Output chassis speeds to control the robot's movement
+ (speeds, feedforwards) -> driveSimulation.runChassisSpeeds(speeds, new Translation2d(), false, false),
+ driveController, // Specify the PID controller for path following
+ PP_CONFIG, // Provide the robot's configuration for accurate simulation
+ // Flip path based on alliance side (Red vs. Blue)
+ () -> DriverStation.getAlliance()
+ .orElse(DriverStation.Alliance.Blue)
+ .equals(DriverStation.Alliance.Red),
+ this // AIRobotInSimulation is a subsystem, so the command uses it as a requirement
+ );
}
- private Command getJoystickDriveCommand() {
- final XboxController joystick = new XboxController(id);
+ /**
+ * Joystick drive command for controlling the opponent robots. This command allows the robot to be driven using an
+ * Xbox controller.
+ */
+ private Command joystickDrive(XboxController joystick) {
+ // Obtain chassis speeds from the joystick inputs
final Supplier joystickSpeeds = () -> new ChassisSpeeds(
- -joystick.getLeftY() * 3.5, -joystick.getLeftX() * 3.5, -joystick.getRightX() * Math.toRadians(360));
+ -joystick.getLeftY() * driveSimulation.maxLinearVelocity().in(MetersPerSecond), // Forward/Backward
+ -joystick.getLeftX() * driveSimulation.maxLinearVelocity().in(MetersPerSecond), // Left/Right
+ -joystick.getRightX() * driveSimulation.maxAngularVelocity().in(RadiansPerSecond) // Rotation
+ );
+
+ // Obtain the driver station facing for the opponent alliance
+ // Used in defense practice, where two tabs of AScope show the driver stations of both alliances
final Supplier opponentDriverStationFacing = () ->
FieldMirroringUtils.getCurrentAllianceDriverStationFacing().plus(Rotation2d.fromDegrees(180));
+
return Commands.run(
() -> {
+ // Calculate field-centric speed from the driver station-centric speed
final ChassisSpeeds fieldCentricSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(
joystickSpeeds.get(),
FieldMirroringUtils.getCurrentAllianceDriverStationFacing()
.plus(Rotation2d.fromDegrees(180)));
- driveSimulation.runChassisSpeeds(joystickSpeeds.get(), new Translation2d(), true, true);
- System.out.println("joystick speeds: " + joystick.getLeftY());
- System.out.println("id: " + id);
+ // Run the field-centric speed to control the robot's movement
+ driveSimulation.runChassisSpeeds(fieldCentricSpeeds, new Translation2d(), true, true);
},
this)
+ // Before the command starts, reset the robot to its starting position on the field
.beforeStarting(() -> driveSimulation.setSimulationWorldPose(
FieldMirroringUtils.toCurrentAlliancePose(ROBOTS_STARTING_POSITIONS[id - 1])));
}
@@ -265,4 +288,58 @@ private static Pose2d[] getRobotPoses(AIRobotInSimulation[] instances) {
.map(instance -> instance.driveSimulation.getActualPoseInSimulationWorld())
.toArray(Pose2d[]::new);
}
+
+ /** shoots a speaker-shot note from the opponent robot */
+ private Command shootAtSpeaker() {
+ return Commands.runOnce(() -> SimulatedArena.getInstance()
+ .addGamePieceProjectile(new NoteOnFly(
+ this.driveSimulation
+ .getActualPoseInSimulationWorld()
+ .getTranslation(),
+ new Translation2d(0.3, 0),
+ this.driveSimulation.getActualSpeedsFieldRelative(),
+ this.driveSimulation
+ .getActualPoseInSimulationWorld()
+ .getRotation(),
+ 0.5,
+ 10,
+ Math.toRadians(60))
+ .asSpeakerShotNote(() -> {})));
+ }
+
+ /** shoots a low-shot note from the opponent robot for feed-shots */
+ private Command feedShotLow() {
+ return Commands.runOnce(() -> SimulatedArena.getInstance()
+ .addGamePieceProjectile(new NoteOnFly(
+ this.driveSimulation
+ .getActualPoseInSimulationWorld()
+ .getTranslation(),
+ new Translation2d(0.3, 0),
+ this.driveSimulation.getActualSpeedsFieldRelative(),
+ this.driveSimulation
+ .getActualPoseInSimulationWorld()
+ .getRotation(),
+ 0.5,
+ 10,
+ Math.toRadians(20))
+ .enableBecomeNoteOnFieldAfterTouchGround()));
+ }
+
+ /** shoots a high-shot note from the opponent robot for feed-shots */
+ private Command feedShotHigh() {
+ return Commands.runOnce(() -> SimulatedArena.getInstance()
+ .addGamePieceProjectile(new NoteOnFly(
+ this.driveSimulation
+ .getActualPoseInSimulationWorld()
+ .getTranslation(),
+ new Translation2d(0.3, 0),
+ this.driveSimulation.getActualSpeedsFieldRelative(),
+ this.driveSimulation
+ .getActualPoseInSimulationWorld()
+ .getRotation(),
+ 0.5,
+ 10,
+ Math.toRadians(55))
+ .enableBecomeNoteOnFieldAfterTouchGround()));
+ }
}