From a653dc8a8b4f267974a6cc505ddebccbcff0fc51 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Sat, 23 Nov 2024 13:19:48 +0800 Subject: [PATCH 1/2] fixed a bug in odometry steer encoder --- .../simulation/drivesims/SwerveModuleSimulation.java | 2 +- .../src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ .../java/frc/robot/subsystems/drive/ModuleIOSim.java | 8 +++++--- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/project/src/main/java/org/ironmaple/simulation/drivesims/SwerveModuleSimulation.java b/project/src/main/java/org/ironmaple/simulation/drivesims/SwerveModuleSimulation.java index a949e94..6036a85 100644 --- a/project/src/main/java/org/ironmaple/simulation/drivesims/SwerveModuleSimulation.java +++ b/project/src/main/java/org/ironmaple/simulation/drivesims/SwerveModuleSimulation.java @@ -348,7 +348,7 @@ public AngularVelocity getSteerAbsoluteEncoderSpeed() { * *

Obtains the Cached Readings of the Drive Encoder's Un-Geared Position.

* - *

The values of {@link #getCachedDriveEncoderUnGearedPositions()} are cached at each sub-tick and can be + *

The values of {@link #getDriveEncoderUnGearedPosition()} are cached at each sub-tick and can be * retrieved using this method. * * @return an array of cached drive encoder un-geared positions diff --git a/templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/BuildConstants.java b/templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/BuildConstants.java index 9e14c0c..a5bace8 100644 --- a/templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/BuildConstants.java +++ b/templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "AdvantageKit_AdvancedSwerveDriveProject"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 215; - public static final String GIT_SHA = "67c80aa2bec5cd272c29be1630052316e0253a17"; - public static final String GIT_DATE = "2024-11-18 03:24:17 EST"; - public static final String GIT_BRANCH = "motor-sim-control-loops"; - public static final String BUILD_DATE = "2024-11-18 04:10:28 EST"; - public static final long BUILD_UNIX_TIME = 1731921028525L; + public static final int GIT_REVISION = 239; + public static final String GIT_SHA = "3512a78ca1802ed68d917ea8bedf31ee7151fb2a"; + public static final String GIT_DATE = "2024-11-21 08:01:35 EST"; + public static final String GIT_BRANCH = "dev"; + public static final String BUILD_DATE = "2024-11-23 00:14:25 EST"; + public static final long BUILD_UNIX_TIME = 1732338865894L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index b627eaa..23c9417 100644 --- a/templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -54,11 +54,13 @@ public void updateInputs(ModuleIOInputs inputs) { }; inputs.odometryTimestamps = OdometryTimeStampsSim.getTimeStamps(); - inputs.odometryDrivePositionsRad = Arrays.stream(moduleSimulation.getCachedDriveEncoderUnGearedPositions()) - .mapToDouble(angle -> angle.in(Radians) / moduleSimulation.STEER_GEAR_RATIO) + inputs.odometryDrivePositionsRad = Arrays.stream(moduleSimulation.getCachedDriveWheelFinalPositions()) + .mapToDouble(wheelPosition -> wheelPosition.in(Radians)) .toArray(); + inputs.odometryTurnPositions = Arrays.stream(moduleSimulation.getCachedSteerRelativeEncoderPositions()) - .map(Rotation2d::new) + .map(relativeEncoderPosition -> + new Rotation2d(relativeEncoderPosition.divide(moduleSimulation.STEER_GEAR_RATIO))) .toArray(Rotation2d[]::new); } From c70051a80a8fc249711add49c169da4479550200 Mon Sep 17 00:00:00 2001 From: caTrixLiu Date: Sun, 24 Nov 2024 21:04:18 +0800 Subject: [PATCH 2/2] added docs for opponent robot simulation --- docs/6_SIMULATING_OPPONENT_ROBOTS.md | 279 +++++++++++++++++++++++++-- 1 file changed, 267 insertions(+), 12 deletions(-) diff --git a/docs/6_SIMULATING_OPPONENT_ROBOTS.md b/docs/6_SIMULATING_OPPONENT_ROBOTS.md index e49271f..2451a4b 100644 --- a/docs/6_SIMULATING_OPPONENT_ROBOTS.md +++ b/docs/6_SIMULATING_OPPONENT_ROBOTS.md @@ -1,24 +1,279 @@ -## Simulating Opponent Robots +# Simulating Opponent Robots -#### This document is based on the [AIRobotInSimulation class](https://github.com/Shenzhen-Robotics-Alliance/Maple-Swerve-Skeleton/blob/main/src/main/java/frc/robot/utils/AIRobotInSimulation.java) from Maple-Swerve-Skeleton. Check the source for a more detailed understanding. +#### This document is based on the [AIRobotInSimulation class](https://github.com/Shenzhen-Robotics-Alliance/Maple-Swerve-Skeleton/blob/main/src/main/java/frc/robot/utils/AIRobotInSimulation.java) from Maple-Swerve-Skeleton. Check the source for a more detailed understanding. > ⚠️ **Note** > > You are reading the documentation for a Beta version of maple-sim. API references are subject to change in future versions. -### 0. Overview -Opponent robots can be added to the field for realistic driver practicing. They can be programmed to do the following: +--- -- Automatically cycle acrross the field. This can help the driver practice offense skills when there are other robots on field. -- Automatically running feed-cycles to deliver feed-shot notes. This can help the driver practice front-field clean up and feeder strategy. -- Controlled by another joystick to play defense. This can help the drivers practice defense and counter-defense skills. +## 0. Overview -### 1. Creating and managing opponent robots +Opponent robots can be added to the field for realistic driver practice. They can be programmed to perform the following tasks: -Opponent robots are also simulated using the `SimplifiedSwerveDriveSimulation` class. We need to create a container class that manages the instances. +- Automatically cycle across the field, helping drivers practice offense skills with other robots present. +- Automatically run feed-cycles to deliver feed-shot notes, assisting with front-field cleanup and feeder strategies. +- Be controlled by a joystick to play defense, allowing drivers to practice defense and counter-defense skills. +--- +## 1. Creating Opponent Robots -

-

<< Prev: Simulating Projectiles

-
\ No newline at end of file +Opponent robots are simulated using the `SimplifiedSwerveDriveSimulation` class. A container class is required to manage the instances. + +When opponent robots are not actively on the field, they are positioned in "queening" areas outside the field to avoid unnecessary interactions. + +```java +public class AIRobotInSimulation extends SubsystemBase { + /* If an opponent robot is not on the field, it is placed in a queening position for performance. */ + public static final Pose2d[] ROBOT_QUEENING_POSITIONS = new Pose2d[] { + new Pose2d(-6, 0, new Rotation2d()), + new Pose2d(-5, 0, new Rotation2d()), + new Pose2d(-4, 0, new Rotation2d()), + new Pose2d(-3, 0, new Rotation2d()), + new Pose2d(-2, 0, new Rotation2d()) + }; + + private final SimplifiedSwerveDriveSimulation driveSimulation; + private final Pose2d queeningPose; + private final int id; + + public AIRobotInSimulation(int id) { + this.id = id; + this.queeningPose = ROBOT_QUEENING_POSITIONS[id]; + this.driveSimulation = new SimplifiedSwerveDriveSimulation(new SwerveDriveSimulation( + DRIVETRAIN_CONFIG, + queeningPose + )); + + SimulatedArena.getInstance().addDriveTrainSimulation( + driveSimulation.getDriveTrainSimulation() + ); + } +} +``` + +--- + +## 2. Controlling Opponent Robots to Auto-Cycle + +Use [PathPlanner](https://pathplanner.dev/home.html) to enable opponent robots to auto-cycle. + +### Configuring PathPlanner + +```java +public class AIRobotInSimulation extends SubsystemBase { + ... // previous code not shown + + // PathPlanner configuration + private static final RobotConfig PP_CONFIG = new RobotConfig( + 55, // Robot mass in kg + 8, // Robot MOI + new ModuleConfig( + Units.inchesToMeters(2), 3.5, 1.2, DCMotor.getFalcon500(1).withReduction(8.14), 60, 1), // Swerve module config + 0.6, 0.6 // Track length and width + ); + + // PathPlanner PID settings + private final PPHolonomicDriveController driveController = + new PPHolonomicDriveController(new PIDConstants(5.0, 0.02), new PIDConstants(7.0, 0.05)); + + /** Follow path command for opponent robots */ + private Command opponentRobotFollowPath(PathPlannerPath path) { + return new FollowPathCommand( + path, // Specify the path + // Provide actual robot pose in simulation, bypassing odometry error + driveSimulation::getActualPoseInSimulationWorld, + // Provide actual robot speed in simulation, bypassing encoder measurement error + driveSimulation::getActualSpeedsRobotRelative, + // Chassis speeds output + (speeds, feedforwards) -> + driveSimulation.runChassisSpeeds(speeds, new Translation2d(), false, false), + driveController, // Specify PID controller + PP_CONFIG, // Specify robot configuration + // Flip path based on alliance side + () -> DriverStation.getAlliance() + .orElse(DriverStation.Alliance.Blue) + .equals(DriverStation.Alliance.Red), + this // AIRobotInSimulation is a subsystem; this command should use it as a requirement + ); + } +} +``` + +--- + +## 3. Controlling Opponent Robots with a Joystick + +Write a joystick drive command to allow manual control of opponent robots for defense practice. + +```java +public static final Pose2d[] ROBOTS_STARTING_POSITIONS = new Pose2d[] { + new Pose2d(15, 6, Rotation2d.fromDegrees(180)), + new Pose2d(15, 4, Rotation2d.fromDegrees(180)), + new Pose2d(15, 2, Rotation2d.fromDegrees(180)), + new Pose2d(1.6, 6, new Rotation2d()), + new Pose2d(1.6, 4, new Rotation2d()) +}; + +/** Joystick drive command for opponent robots */ +private Command joystickDrive(XboxController joystick) { + // Obtain chassis speeds from joystick input + final Supplier joystickSpeeds = () -> new ChassisSpeeds( + -joystick.getLeftY() * driveSimulation.maxLinearVelocity().in(MetersPerSecond), + -joystick.getLeftX() * driveSimulation.maxLinearVelocity().in(MetersPerSecond), + -joystick.getRightX() * driveSimulation.maxAngularVelocity().in(RadiansPerSecond)); + + // Obtain driverstation facing for opponent driver station + final Supplier opponentDriverStationFacing = () -> + FieldMirroringUtils.getCurrentAllianceDriverStationFacing().plus(Rotation2d.fromDegrees(180)); + + return Commands.run(() -> { + // Calculate field-centric speed from driverstation-centric speed + final ChassisSpeeds fieldCentricSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( + joystickSpeeds.get(), + FieldMirroringUtils.getCurrentAllianceDriverStationFacing() + .plus(Rotation2d.fromDegrees(180))); + // Run the field-centric speed + driveSimulation.runChassisSpeeds(fieldCentricSpeeds, new Translation2d(), true, true); + }, this) + // Before the command starts, reset the robot to a position inside the field + .beforeStarting(() -> driveSimulation.setSimulationWorldPose( + FieldMirroringUtils.toCurrentAlliancePose(ROBOTS_STARTING_POSITIONS[id - 1]))); +} +``` + +--- + +## 4. Launching Gamepieces from Opponent Robots + +Using the [Projectile Simulation in maple-sim](https://shenzhen-robotics-alliance.github.io/maple-sim/5_SIMULATING_PROJECTILES.html), opponent robots can deliver feed shots or score by launching gamepieces. + +```java +private Command feedShot() { + 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(45)) + .enableBecomeNoteOnFieldAfterTouchGround())); +} +``` + +--- + +## 5. Managing Opponent Robots (Optional) + +Opponent robots can be managed using a "behavior chooser," which is displayed on the dashboard. This allows for easy selection of behaviors such as disabling robots, running auto-cycles, or joystick driving. + +```java +/** Build the behavior chooser of this opponent robot and send it to the dashboard */ +public void buildBehaviorChooser( + PathPlannerPath segment0, + Command toRunAtEndOfSegment0, + PathPlannerPath segment1, + Command toRunAtEndOfSegment1, + XboxController joystick) { + SendableChooser behaviorChooser = new SendableChooser<>(); + final Supplier disable = + () -> Commands.runOnce(() -> driveSimulation.setSimulationWorldPose(queeningPose), this) + .andThen(Commands.runOnce(() -> driveSimulation.runChassisSpeeds( + new ChassisSpeeds(), new Translation2d(), false, false))) + .ignoringDisable(true); + + // Option to disable the robot + behaviorChooser.setDefaultOption("Disable", disable.get()); + + // Option to auto-cycle the robot + behaviorChooser.addOption( + "Auto Cycle", getAutoCycleCommand(segment0, toRunAtEndOfSegment0, segment1, toRunAtEndOfSegment1)); + + // Option to manually control the robot with a joystick + behaviorChooser.addOption("Joystick Drive", joystickDrive(joystick)); + + // Schedule the command when another behavior is selected + behaviorChooser.onChange((Command::schedule)); + + // Schedule the selected command when teleop starts + RobotModeTriggers.teleop() + .onTrue(Commands.runOnce(() -> behaviorChooser.getSelected().schedule())); + + // Disable the robot when the user robot is disabled + RobotModeTriggers.disabled().onTrue(disable.get()); + + SmartDashboard.putData("AIRobotBehaviors/Opponent Robot " + id + " Behavior", behaviorChooser); +} + +/** Get the command to auto-cycle the robot relatively */ +private Command getAutoCycleCommand( + PathPlannerPath segment0, + Command toRunAtEndOfSegment0, + PathPlannerPath segment1, + Command toRunAtEndOfSegment1) { + final SequentialCommandGroup cycle = new SequentialCommandGroup(); + final Pose2d startingPose = new Pose2d( + segment0.getStartingDifferentialPose().getTranslation(), + segment0.getIdealStartingState().rotation()); + + cycle.addCommands( + opponentRobotFollowPath(segment0).andThen(toRunAtEndOfSegment0).withTimeout(10)); + cycle.addCommands( + opponentRobotFollowPath(segment1).andThen(toRunAtEndOfSegment1).withTimeout(10)); + + return cycle.repeatedly() + .beforeStarting(Commands.runOnce(() -> driveSimulation.setSimulationWorldPose( + FieldMirroringUtils.toCurrentAlliancePose(startingPose)))); +} +``` + +--- + +## 6. Initializing Opponent Robots in Simulation + +Keep opponent robot instances in a static variable and initialize them during simulation startup. + +```java +public static final AIRobotInSimulation[] instances = new AIRobotInSimulation[...]; // you can create as many opponent robots as you needs +public static void startOpponentRobotSimulations() { + try { + instances[0] = new AIRobotInSimulation(0); + instances[0].buildBehaviorChooser( + PathPlannerPath.fromPathFile("opponent robot cycle path 0"), + Commands.none(), + PathPlannerPath.fromPathFile("opponent robot cycle path 0 backwards"), + Commands.none(), + new XboxController(2)); + + instances[1] = ...; + instances[1].buildBehaviorChooser( + PathPlannerPath.fromPathFile("opponent robot cycle path 1"), + instances[1].shootAtSpeaker(), + PathPlannerPath.fromPathFile("opponent robot cycle path 1 backwards"), + Commands.none(), + new XboxController(3)); + + ... // create more opponent robots if you need + } catch (Exception e) { + DriverStation.reportError("Failed to load opponent robot simulation paths, error: " + e.getMessage(), false); + } +} +``` + +Call this initialization in the simulation lifecycle: + +```java +// Robot.java +@Override +public void simulationInit() { + AIRobotInSimulation.startOpponentRobotSimulations(); +} +``` \ No newline at end of file