diff --git a/src/main/deploy/swerve/maxSwerve/modules/backleft.json b/src/main/deploy/swerve/maxSwerve/modules/backleft.json index 571d1667..05501b72 100644 --- a/src/main/deploy/swerve/maxSwerve/modules/backleft.json +++ b/src/main/deploy/swerve/maxSwerve/modules/backleft.json @@ -21,7 +21,7 @@ "absoluteEncoderInverted": true, "absoluteEncoderOffset": 301.3301969, "location": { - "front": -12, - "left": 12 + "front": -16, + "left": 14 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/maxSwerve/modules/backright.json b/src/main/deploy/swerve/maxSwerve/modules/backright.json index 90e2a566..ea3edaa0 100644 --- a/src/main/deploy/swerve/maxSwerve/modules/backright.json +++ b/src/main/deploy/swerve/maxSwerve/modules/backright.json @@ -21,7 +21,7 @@ "absoluteEncoderInverted": true, "absoluteEncoderOffset": 7.6941204, "location": { - "front": -12, - "left": -12 + "front": -16, + "left": -14 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/maxSwerve/modules/frontleft.json b/src/main/deploy/swerve/maxSwerve/modules/frontleft.json index c1ceeda3..09274f3b 100644 --- a/src/main/deploy/swerve/maxSwerve/modules/frontleft.json +++ b/src/main/deploy/swerve/maxSwerve/modules/frontleft.json @@ -21,7 +21,7 @@ "absoluteEncoderInverted": true, "absoluteEncoderOffset": 90, "location": { - "front": 12, - "left": 12 + "front": 16, + "left": 14 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/maxSwerve/modules/frontright.json b/src/main/deploy/swerve/maxSwerve/modules/frontright.json index 6541c7c8..aa3e4bb4 100644 --- a/src/main/deploy/swerve/maxSwerve/modules/frontright.json +++ b/src/main/deploy/swerve/maxSwerve/modules/frontright.json @@ -21,7 +21,7 @@ "absoluteEncoderInverted": true, "absoluteEncoderOffset": -90, "location": { - "front": 12, - "left": -12 + "front": 16, + "left": -14 } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1be267a4..3f85e1dc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,11 +4,15 @@ package frc.robot; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; 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 @@ -21,6 +25,13 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; + StructArrayPublisher coralPoses = + NetworkTableInstance.getDefault().getStructArrayTopic("Coral", Pose3d.struct).publish(); + StructArrayPublisher algaePoses = + NetworkTableInstance.getDefault().getStructArrayTopic("Algae", Pose3d.struct).publish(); + + private static Robot m_instance; + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -31,13 +42,19 @@ public void robotInit() { // and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + m_instance = this; if (isSimulation()) { DriverStation.refreshData(); DriverStation.silenceJoystickConnectionWarning(true); + SimulatedArena.getInstance().resetFieldForAuto(); } } + public static Robot getInstance() { + return m_instance; + } + /** * 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. @@ -87,7 +104,7 @@ public void autonomousInit() { @Override public void autonomousPeriodic() { // SmartDashboard.putString( - // "Current Pose Auto", RobotContainer.m_driveSubsystem.getPose().toString()); + // "Current Pose Auto", RobotContainer.m_driveSubsystem.getPose().toString()); } @Override @@ -114,4 +131,12 @@ public void testInit() { /** This function is called periodically during test mode. */ @Override 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)); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d86715e9..2298ed64 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.subsystems.CoralHandlerSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; import swervelib.SwerveInputStream; @@ -20,23 +21,25 @@ * (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - private final SwerveSubsystem m_drive = - new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/maxSwerve")); + private final SwerveSubsystem m_drive = new SwerveSubsystem( + new File(Filesystem.getDeployDirectory(), "swerve/maxSwerve")); + private final CoralHandlerSubsystem m_coral = new CoralHandlerSubsystem(m_drive.getSimDrive()); private final CommandXboxController m_driverController = new CommandXboxController(0); // Configure drive input stream - SwerveInputStream driveInput = - SwerveInputStream.of( - m_drive.getSwerveDrive(), - () -> m_driverController.getLeftY(), - () -> m_driverController.getLeftX()) - .withControllerRotationAxis(() -> -m_driverController.getRightX()) - .deadband(0.1) - .scaleTranslation(0.8) - .allianceRelativeControl(true); + SwerveInputStream driveInput = SwerveInputStream.of( + m_drive.getSwerveDrive(), + () -> m_driverController.getLeftY(), + () -> m_driverController.getLeftX()) + .withControllerRotationAxis(() -> -m_driverController.getRightX()) + .deadband(0.1) + .scaleTranslation(0.8) + .allianceRelativeControl(true); - /** The container for the robot. Contains subsystems, OI devices, and commands. */ + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ public RobotContainer() { configureButtonBindings(); @@ -45,9 +48,12 @@ public RobotContainer() { } /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * Use this method to define your button->command mappings. Buttons can be + * created by + * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its + * subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling + * passing it to a * {@link JoystickButton}. */ private void configureButtonBindings() { @@ -55,9 +61,12 @@ private void configureButtonBindings() { m_driverController.a().onTrue(Commands.runOnce(m_drive::zeroGyro)); // Lock wheels with left bumper + m_driverController + .rightBumper() + .onTrue(Commands.runOnce(m_coral::intake)); m_driverController .leftBumper() - .whileTrue(Commands.runOnce(m_drive::lock, m_drive).repeatedly()); + .onTrue(Commands.runOnce(m_coral::outtake)); } /** diff --git a/src/main/java/frc/robot/subsystems/CoralHandlerSubsystem.java b/src/main/java/frc/robot/subsystems/CoralHandlerSubsystem.java index 4af8cfd0..89536db8 100644 --- a/src/main/java/frc/robot/subsystems/CoralHandlerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/CoralHandlerSubsystem.java @@ -1,5 +1,101 @@ package frc.robot.subsystems; +import com.revrobotics.Rev2mDistanceSensor; +import com.revrobotics.Rev2mDistanceSensor.Unit; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Robot; +import org.dyn4j.geometry.Polygon; +import org.dyn4j.geometry.Rectangle; +import org.dyn4j.geometry.Vector2; +import org.ironmaple.simulation.IntakeSimulation; +import org.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation; + +public class CoralHandlerSubsystem extends SubsystemBase { + // two neo 550s, 9:1 gearing on both, going in opposite directions. + // looking on from the top, the right one going clockwise is intake, and + // counterclockwise is + // outtake. + // vice versa for the other + // TODO: move all of the hardcoded values to Constants + + public final SparkMax m_left; + public final SparkMax m_right; + private final Rev2mDistanceSensor m_sensor; + + public final IntakeSimulation m_intakeSim; + + public enum Mode { + kInactive, + kIntake, + kOuttake, + } + + private Mode m_mode = Mode.kInactive; + private boolean m_hasCoral = false; + + public CoralHandlerSubsystem(AbstractDriveTrainSimulation driveSim) { + m_left = new SparkMax(9, MotorType.kBrushless); + m_right = new SparkMax(10, MotorType.kBrushless); + if (Robot.getInstance().isReal()) { + m_sensor = new Rev2mDistanceSensor(Rev2mDistanceSensor.Port.kOnboard); + } else { + m_sensor = null; + } + m_intakeSim = new IntakeSimulation( + "Coral", + driveSim, + new Rectangle(.762, 1.007), + 1); + } + + public Mode getMode() { + return m_mode; + } + + public boolean hasCoral() { + return m_hasCoral; + } + + public void setMode(Mode mode) { + m_mode = mode; + switch (mode) { + case kInactive: + m_left.set(0); + m_right.set(0); + break; + case kIntake: + m_left.set(1); + m_right.set(-1); + break; + case kOuttake: + m_left.set(-1); + m_right.set(1); + break; + } + } + + @Override + public void periodic() { + if (Robot.getInstance().isReal()) { + m_hasCoral = m_sensor.getRange(Unit.kInches) < 5; + } else { + if (m_mode == Mode.kIntake) { + m_intakeSim.startIntake(); + } else { + m_intakeSim.stopIntake(); + } + m_hasCoral = m_intakeSim.getGamePiecesAmount() != 0; + } + } + + public void intake() { + setMode(Mode.kIntake); + } -public class CoralHandlerSubsystem extends SubsystemBase {} + public void outtake() { + setMode(Mode.kOuttake); + } +} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index cd76ed0e..338659a6 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -40,6 +40,7 @@ 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; @@ -57,6 +58,8 @@ public class SwerveSubsystem extends SubsystemBase { /** Swerve drive object. */ private final SwerveDrive swerveDrive; + public final Optional simDrive; + /** AprilTag field layout. */ // private final AprilTagFieldLayout aprilTagFieldLayout = // AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); @@ -72,7 +75,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 { @@ -82,12 +86,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( @@ -100,14 +109,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(); } /** @@ -124,6 +136,19 @@ 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()) { + // TODO: figure out setting dimensions + } + } + + public SwerveDriveSimulation getSimDrive() { + return simDrive.orElse(null); } /** Setup the photon vision class. */ @@ -170,10 +195,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 @@ -183,7 +210,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 @@ -241,7 +269,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); } @@ -424,7 +453,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( () -> { diff --git a/vendordeps/REV2mDistanceSensor.json b/vendordeps/REV2mDistanceSensor.json new file mode 100644 index 00000000..1c7afa14 --- /dev/null +++ b/vendordeps/REV2mDistanceSensor.json @@ -0,0 +1,56 @@ +{ + "fileName": "REV2mDistanceSensor.json", + "name": "REV2mDistanceSensor", + "version": "2025.0.0", + "frcYear": "2025", + "uuid": "9e352acd-4eec-40f7-8490-3357b5ed65ae", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://github.com/REVrobotics/2m-Distance-Sensor/blob/master/Source/vendordeps/REV2mDistanceSensor.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-java", + "version": "2025.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "2025.0.0", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "linuxathena" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-cpp", + "version": "2025.0.0", + "libName": "libDistanceSensor", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "DistanceSensor-driver", + "version": "2025.0.0", + "libName": "libDistanceSensorDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + } + ] +} \ No newline at end of file