Skip to content
This repository has been archived by the owner on Feb 3, 2025. It is now read-only.

Commit

Permalink
feat(SwerveSubsystem): Expose SwerveDriveSimulation object
Browse files Browse the repository at this point in the history
  • Loading branch information
@HENRYMARTIN5
HENRYMARTIN5 committed Feb 1, 2025
1 parent d166189 commit 61f0ebe
Show file tree
Hide file tree
Showing 2 changed files with 57 additions and 48 deletions.
59 changes: 20 additions & 39 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,6 @@

package frc.robot;

import org.ironmaple.simulation.SimulatedArena;

import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StructArrayPublisher;
Expand All @@ -14,31 +12,26 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import org.ironmaple.simulation.SimulatedArena;

/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the
* name of this class or
* the package after creating this project, you must also update the
* build.gradle file in the
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
* the package after creating this project, you must also update the build.gradle file in the
* project.
*/
public class Robot extends TimedRobot {
private Command m_autonomousCommand;

private RobotContainer m_robotContainer;

StructArrayPublisher<Pose3d> coralPoses = NetworkTableInstance.getDefault()
.getStructArrayTopic("Coral", Pose3d.struct)
.publish();
StructArrayPublisher<Pose3d> algaePoses = NetworkTableInstance.getDefault()
.getStructArrayTopic("Algae", Pose3d.struct)
.publish();
StructArrayPublisher<Pose3d> coralPoses =
NetworkTableInstance.getDefault().getStructArrayTopic("Coral", Pose3d.struct).publish();
StructArrayPublisher<Pose3d> algaePoses =
NetworkTableInstance.getDefault().getStructArrayTopic("Algae", Pose3d.struct).publish();

/**
* This function is run when the robot is first started up and should be used
* for any
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
@Override
Expand All @@ -56,13 +49,10 @@ public void robotInit() {
}

/**
* This function is called every 20 ms, no matter the mode. Use this for items
* like diagnostics
* This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
* that you want ran during disabled, autonomous, teleoperated and test.
*
* <p>
* This runs after the mode specific periodic functions, but before LiveWindow
* and
* <p>This runs after the mode specific periodic functions, but before LiveWindow and
* SmartDashboard integrated updating.
*/
@Override
Expand All @@ -80,17 +70,12 @@ public void robotPeriodic() {

/** This function is called once each time the robot enters Disabled mode. */
@Override
public void disabledInit() {
}
public void disabledInit() {}

@Override
public void disabledPeriodic() {
}
public void disabledPeriodic() {}

/**
* This autonomous runs the autonomous command selected by your
* {@link RobotContainer} class.
*/
/** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
@Override
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
Expand Down Expand Up @@ -128,8 +113,7 @@ public void teleopInit() {

/** This function is called periodically during operator control. */
@Override
public void teleopPeriodic() {
}
public void teleopPeriodic() {}

@Override
public void testInit() {
Expand All @@ -139,16 +123,13 @@ public void testInit() {

/** This function is called periodically during test mode. */
@Override
public void testPeriodic() {
}
public void testPeriodic() {}

@Override
public void simulationPeriodic() {
coralPoses.accept(SimulatedArena.getInstance()
.getGamePiecesByType("Coral")
.toArray(Pose3d[]::new));
algaePoses.accept(SimulatedArena.getInstance()
.getGamePiecesByType("Algae")
.toArray(Pose3d[]::new));
coralPoses.accept(
SimulatedArena.getInstance().getGamePiecesByType("Coral").toArray(Pose3d[]::new));
algaePoses.accept(
SimulatedArena.getInstance().getGamePiecesByType("Algae").toArray(Pose3d[]::new));
}
}
46 changes: 37 additions & 9 deletions src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
import frc.robot.Constants;
import frc.robot.Robot;
import frc.robot.subsystems.swervedrive.Vision.Cameras;
import java.io.File;
import java.io.IOException;
Expand All @@ -40,6 +41,8 @@
import java.util.concurrent.atomic.AtomicReference;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;

import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
import org.json.simple.parser.ParseException;
import org.photonvision.targeting.PhotonPipelineResult;
import swervelib.SwerveController;
Expand All @@ -57,6 +60,8 @@ public class SwerveSubsystem extends SubsystemBase {
/** Swerve drive object. */
private final SwerveDrive swerveDrive;

public final Optional<SwerveDriveSimulation> simDrive;

/** AprilTag field layout. */
// private final AprilTagFieldLayout aprilTagFieldLayout =
// AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
Expand All @@ -72,7 +77,8 @@ public class SwerveSubsystem extends SubsystemBase {
* @param directory Directory of swerve drive config files.
*/
public SwerveSubsystem(File directory) {
// Configure the Telemetry before creating the SwerveDrive to avoid unnecessary objects being
// Configure the Telemetry before creating the SwerveDrive to avoid unnecessary
// objects being
// created.
SwerveDriveTelemetry.verbosity = TelemetryVerbosity.HIGH;
try {
Expand All @@ -82,12 +88,17 @@ public SwerveSubsystem(File directory) {
Constants.DriveConstants.kMaxSpeedMetersPerSecond,
new Pose2d(
new Translation2d(Meter.of(1), Meter.of(4)), Rotation2d.fromDegrees(0)));
// Alternative method if you don't want to supply the conversion factor via JSON files.
// Alternative method if you don't want to supply the conversion factor via JSON
// files.
// swerveDrive = new SwerveParser(directory).createSwerveDrive(maximumSpeed,
// angleConversionFactor, driveConversionFactor);
} catch (Exception e) {
throw new RuntimeException(e);
}

// Simulation
simDrive = swerveDrive.getMapleSimDrive();

swerveDrive.setHeadingCorrection(
false); // Heading correction should only be used while controlling the robot via angle.
swerveDrive.setCosineCompensator(
Expand All @@ -100,14 +111,17 @@ public SwerveSubsystem(File directory) {
swerveDrive.setModuleEncoderAutoSynchronize(
false, 1); // Enable if you want to resynchronize your absolute encoders and motor encoders
// periodically when they are not moving.
// swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the
// swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used
// over the
// internal encoder and push the offsets onto it. Throws warning if not possible
if (visionDriveTest) {
setupPhotonVision();
// Stop the odometry thread if we are using vision that way we can synchronize updates better.
// Stop the odometry thread if we are using vision that way we can synchronize
// updates better.
swerveDrive.stopOdometryThread();
}
setupPathPlanner();
setupChassisSim();
}

/**
Expand All @@ -124,6 +138,15 @@ public SwerveSubsystem(
controllerCfg,
Constants.DriveConstants.kMaxSpeedMetersPerSecond,
new Pose2d(new Translation2d(Meter.of(2), Meter.of(0)), Rotation2d.fromDegrees(0)));
simDrive = swerveDrive.getMapleSimDrive();
setupChassisSim();
}

/** Setup the chassis simulation with the correct physical dimensions to match CAD. */
public void setupChassisSim() {
if (simDrive.isPresent()) {
simDrive.get()
}
}

/** Setup the photon vision class. */
Expand Down Expand Up @@ -170,10 +193,12 @@ public void setupPathPlanner() {
swerveDrive.setChassisSpeeds(speedsRobotRelative);
}
},
// Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally
// Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also
// optionally
// outputs individual module feedforwards
new PPHolonomicDriveController(
// PPHolonomicController is the built in path following controller for holonomic drive
// PPHolonomicController is the built in path following controller for holonomic
// drive
// trains
new PIDConstants(5.0, 0.0, 0.0),
// Translation PID constants
Expand All @@ -183,7 +208,8 @@ public void setupPathPlanner() {
config,
// The robot configuration
() -> {
// Boolean supplier that controls when the path will be mirrored for the red alliance
// Boolean supplier that controls when the path will be mirrored for the red
// alliance
// This will flip the path being followed to the red side of the field.
// THE ORIGIN WILL REMAIN ON THE BLUE SIDE

Expand Down Expand Up @@ -241,7 +267,8 @@ public Command aimAtTarget(Cameras camera) {
* @return {@link AutoBuilder#followPath(PathPlannerPath)} path command.
*/
public Command getAutonomousCommand(String pathName) {
// Create a path following command using AutoBuilder. This will also trigger event markers.
// Create a path following command using AutoBuilder. This will also trigger
// event markers.
return new PathPlannerAuto(pathName);
}

Expand Down Expand Up @@ -424,7 +451,8 @@ public Command driveCommand(
DoubleSupplier translationY,
DoubleSupplier headingX,
DoubleSupplier headingY) {
// swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for
// swerveDrive.setHeadingCorrection(true); // Normally you would want heading
// correction for
// this kind of control.
return run(
() -> {
Expand Down

0 comments on commit 61f0ebe

Please sign in to comment.