diff --git a/.vscode/launch.json b/.vscode/launch.json index c9c9713..9ce8380 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,6 +4,8 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ + + { "type": "wpilib", diff --git a/.vscode/settings.json b/.vscode/settings.json index 612cdd0..5e6ede8 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -56,5 +56,6 @@ "edu.wpi.first.math.proto.*", "edu.wpi.first.math.**.proto.*", "edu.wpi.first.math.**.struct.*", - ] + ], + "java.dependency.enableDependencyCheckup": false } diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 0fe20e8..c4bad9a 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": false, "currentLanguage": "java", - "projectYear": "2025", + "projectYear": "2026", "teamNumber": 1334 } \ No newline at end of file diff --git a/WPILib-License.md b/WPILib-License.md index 645e542..eb3061b 100644 --- a/WPILib-License.md +++ b/WPILib-License.md @@ -1,4 +1,4 @@ -Copyright (c) 2009-2024 FIRST and other WPILib contributors +Copyright (c) 2009-2026 FIRST and other WPILib contributors All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/build.gradle b/build.gradle index 44f1061..ab61ab1 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.2" + id "edu.wpi.first.GradleRIO" version "2026.2.1" } java { @@ -8,6 +8,14 @@ java { targetCompatibility = JavaVersion.VERSION_17 } +// Force UTF-8 encoding for all Java source files +compileJava { + options.encoding = 'UTF-8' +} +compileTestJava { + options.encoding = 'UTF-8' +} + def ROBOT_MAIN_CLASS = "frc.robot.Main" // Define my targets (RoboRIO) and artifacts (deployable files) @@ -43,11 +51,12 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcJava -// Set to true to use debug for JNI. +// Set to true to use debug for all targets including JNI, which will drastically impact +// performance. wpi.java.debugJni = false // Set this to true to enable desktop support. -def includeDesktopSupport = false +def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. @@ -88,7 +97,9 @@ wpi.sim.addDriverstation() // knows where to look for our Robot Class. jar { from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } - from sourceSets.main.allSource + from('src') { into 'backup/src' } + from('vendordeps') { into 'backup/vendordeps' } + from('build.gradle') { into 'backup' } manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE } diff --git a/settings.gradle b/settings.gradle index c493958..25f6f6e 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2025' + String frcYear = '2026' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..2c50070 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,100 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 81, + 70 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + }, + {}, + { + "guid": "Keyboard2" + } + ] +} diff --git a/src/main/deploy/SWERVE/controllerproperties.json b/src/main/deploy/SWERVE/controllerproperties.json new file mode 100644 index 0000000..6b8b234 --- /dev/null +++ b/src/main/deploy/SWERVE/controllerproperties.json @@ -0,0 +1,8 @@ +{ + "angleJoystickRadiusDeadband": 0.05, + "heading": { + "p": 0.0, + "i": 0.0, + "d": 0.0 + } +} diff --git a/src/main/deploy/SWERVE/modules/Backleft.json b/src/main/deploy/SWERVE/modules/Backleft.json new file mode 100644 index 0000000..2091243 --- /dev/null +++ b/src/main/deploy/SWERVE/modules/Backleft.json @@ -0,0 +1 @@ +{"location":{"front":-11.125,"left":11.125},"absoluteEncoderOffset":0,"drive":{"type":"talonfx","id":43,"canbus":null},"angle":{"type":"talonfx","id":42,"canbus":null},"encoder":{"type":"cancoder","id":41,"canbus":null},"inverted":{"drive":false,"angle":false}} diff --git a/src/main/deploy/SWERVE/modules/Backright.json b/src/main/deploy/SWERVE/modules/Backright.json new file mode 100644 index 0000000..13907a0 --- /dev/null +++ b/src/main/deploy/SWERVE/modules/Backright.json @@ -0,0 +1 @@ +{"location":{"front":-11.125,"left":-11.125},"absoluteEncoderOffset":0,"drive":{"type":"talonfx","id":33,"canbus":null},"angle":{"type":"talonfx","id":32,"canbus":null},"encoder":{"type":"cancoder","id":31,"canbus":null},"inverted":{"drive":false,"angle":false}} diff --git a/src/main/deploy/SWERVE/modules/Frontleft.json b/src/main/deploy/SWERVE/modules/Frontleft.json new file mode 100644 index 0000000..d3f2db9 --- /dev/null +++ b/src/main/deploy/SWERVE/modules/Frontleft.json @@ -0,0 +1 @@ +{"location":{"front":11.125,"left":11.125},"absoluteEncoderOffset":0,"drive":{"type":"talonfx","id":13,"canbus":null},"angle":{"type":"talonfx","id":12,"canbus":null},"encoder":{"type":"cancoder","id":11,"canbus":null},"inverted":{"drive":false,"angle":false}} diff --git a/src/main/deploy/SWERVE/modules/Frontright.json b/src/main/deploy/SWERVE/modules/Frontright.json new file mode 100644 index 0000000..605b896 --- /dev/null +++ b/src/main/deploy/SWERVE/modules/Frontright.json @@ -0,0 +1 @@ +{"location":{"front":11.125,"left":-11.125},"absoluteEncoderOffset":0,"drive":{"type":"talonfx","id":23,"canbus":null},"angle":{"type":"talonfx","id":22,"canbus":null},"encoder":{"type":"cancoder","id":21,"canbus":null},"inverted":{"drive":false,"angle":false}} diff --git a/src/main/deploy/SWERVE/modules/physicalproperties.json b/src/main/deploy/SWERVE/modules/physicalproperties.json new file mode 100644 index 0000000..41df2be --- /dev/null +++ b/src/main/deploy/SWERVE/modules/physicalproperties.json @@ -0,0 +1,24 @@ +{ + "optimalVoltage": 12, + "robotMass": 110.2311, + "wheelGripCoefficientOfFriction": 1.19, + "currentLimit": { + "drive": 40, + "angle": 20 + }, + "conversionFactors": { + "angle": { + "gearRatio": 21.428571429, + "factor": 0 + }, + "drive": { + "diameter": 4, + "gearRatio": 6.75, + "factor": 0 + } + }, + "rampRate": { + "drive": 0.25, + "angle": 0.0 + } +} \ No newline at end of file diff --git a/src/main/deploy/SWERVE/modules/pidfproperties.json b/src/main/deploy/SWERVE/modules/pidfproperties.json new file mode 100644 index 0000000..0622922 --- /dev/null +++ b/src/main/deploy/SWERVE/modules/pidfproperties.json @@ -0,0 +1,16 @@ +{ + "drive": { + "p": 0.0020645, + "i": 0, + "d": 0, + "f": 0, + "iz": 0 + }, + "angle": { + "p": 0.080645, + "i": 0, + "d": 0.001, + "f": 0, + "iz": 0 + } +} diff --git a/src/main/deploy/SWERVE/swervedrive.json b/src/main/deploy/SWERVE/swervedrive.json new file mode 100644 index 0000000..1ed6650 --- /dev/null +++ b/src/main/deploy/SWERVE/swervedrive.json @@ -0,0 +1,15 @@ +{ + "imu": { + "type": "pigeon2", + "id": 50, + "canbus": null + }, + "invertedIMU": false, + "modules": [ + "Frontleft.json", + "Frontright.json", + "Backleft.json", + "Backright.json" + ], + "note": "Check IMU inversion." +} diff --git a/src/main/deploy/pathplanner/2026-rebuilt.json b/src/main/deploy/pathplanner/2026-rebuilt.json new file mode 100644 index 0000000..fa81b88 --- /dev/null +++ b/src/main/deploy/pathplanner/2026-rebuilt.json @@ -0,0 +1,19 @@ +{ + "game": "Rebuilt", + "field-image": "image.png", + "field-corners": { + "top-left": [ + 245, + 118 + ], + "bottom-right": [ + 3942, + 1914 + ] + }, + "field-size": [ + 54.269, + 26.474 + ], + "field-unit": "foot" +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/image.png b/src/main/deploy/pathplanner/image.png new file mode 100644 index 0000000..e1c3bf8 Binary files /dev/null and b/src/main/deploy/pathplanner/image.png differ diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c50ba05..f210a69 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,16 +4,80 @@ package frc.robot; -/** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

It is advised to statically import this class (or one of its inner classes) wherever the - * constants are needed, to reduce verbosity. - */ +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + public final class Constants { - public static class OperatorConstants { - public static final int kDriverControllerPort = 0; - } + + public int getId() { return 0; } + + public static final class DriveConstants { + public static final double MAX_SPEED_MPS = 4.5; + public static final double MAX_ANGULAR_VELOCITY = Math.PI; + public static final double DEADBAND = 0.05; + public static final double ROTATION_SCALE = 0.5; + public static final double FULL_SPEED_ROTATION_SCALE = 1.0; + public static final double PRECISION_MULTIPLIER = 0.3; + public static final double PRECISION_ROTATION_SCALE = 0.3; + + // Speed multiplier bounds for bumper-based speed control + public static final double MIN_SPEED_MULTIPLIER = 0.2; + public static final double MAX_SPEED_MULTIPLIER = 1.0; + public static final double SPEED_MULTIPLIER_STEP = 0.1; + public static final double DEFAULT_SPEED_MULTIPLIER = 0.7; + + // Auto constants + public static final double AUTO_MAX_SPEED_MPS = 4.0; + public static final double AUTO_POSITION_TOLERANCE_M = 0.20; + public static final double AUTO_ANGLE_TOLERANCE_DEG = 15.0; + } + + public static final class FieldConstants { + // Field dimensions (Rebuilt 2025) - converted from feet to meters + // 54.269 ft = 16.54 m, 26.474 ft = 8.07 m + public static final double FIELD_LENGTH = 16.54; + public static final double FIELD_WIDTH = 8.07; + // starting positions + public static final Pose2d BLUE_ALLIANCE_START = new Pose2d(1.0, 1.0, new Rotation2d(0)); + public static final Pose2d RED_ALLIANCE_START = new Pose2d(15.54, 1.0, new Rotation2d(Math.PI)); + public static final Pose2d CENTER_START = new Pose2d(8.27, 4.035, new Rotation2d(0)); // Updated to actual field center + + // Reef structure (center of field) + public static final double REEF_CENTER_X = 8.27; + public static final double REEF_CENTER_Y = 4.035; + public static final double REEF_RADIUS = 1.5; + public static final double REEF_SAFE_DISTANCE = 2.5; + + // Safe zones (away from obstacles) + public static final Pose2d BLUE_SAFE_ZONE = new Pose2d(2.0, 1.5, Rotation2d.kZero); + public static final Pose2d RED_SAFE_ZONE = new Pose2d(FIELD_LENGTH - 2.0, FIELD_WIDTH - 1.5, Rotation2d.fromDegrees(180)); + + // Coral stations (at field edges) + public static final Pose2d BLUE_CORAL_STATION = new Pose2d(1.2, 7.0, Rotation2d.kZero); + public static final Pose2d RED_CORAL_STATION = new Pose2d(FIELD_LENGTH - 1.2, 1.2, Rotation2d.fromDegrees(180)); + + // Reef branches (scoring positions) - adjusted for correct field center + public static final Pose2d REEF_FRONT = new Pose2d(8.27, 2.5, Rotation2d.fromDegrees(0)); + public static final Pose2d REEF_LEFT = new Pose2d(7.2, 5.0, Rotation2d.fromDegrees(120)); + public static final Pose2d REEF_RIGHT = new Pose2d(9.3, 5.0, Rotation2d.fromDegrees(60)); + + // Processor station + public static final Pose2d PROCESSOR = new Pose2d(13.5, 4.035, Rotation2d.fromDegrees(90)); + + // Convenient teleop rest position near the hub + public static final Pose2d HUB_REST_POSITION = new Pose2d(8.27, 4.5, Rotation2d.kZero); + + // Field boundaries (with safety margin) + public static final double FIELD_MARGIN = 0.5; // meters from edge + public static final double MIN_X = FIELD_MARGIN; + public static final double MAX_X = FIELD_LENGTH - FIELD_MARGIN; + public static final double MIN_Y = FIELD_MARGIN; + public static final double MAX_Y = FIELD_WIDTH - FIELD_MARGIN; + } + + public static final class ControllerConstants { + public static final int DRIVER_CONTROLLER_PORT = 0; + public static final int OPERATOR_CONTROLLER_PORT = 1; + public static final double TRIGGER_THRESHOLD = 0.5; + } } diff --git a/src/main/java/frc/robot/Input.java b/src/main/java/frc/robot/Input.java new file mode 100644 index 0000000..5cb15df --- /dev/null +++ b/src/main/java/frc/robot/Input.java @@ -0,0 +1,73 @@ +package frc.robot; + +import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; +import edu.wpi.first.wpilibj.GenericHID; + +public class Input { + // Keyboard0 on robot port 0: WASD keys + E/R rotation + // axis 0 = A (dec) / D (inc) — strafe left/right + // axis 1 = W (dec) / S (inc) — forward/back + // axis 2 = E (dec) / R (inc) — rotation (slow ramp, keyRate 0.01) + // Keyboard2 on robot port 2: Arrow keys + Q/F buttons + // axis 0 = Left (dec) / Right (inc) — strafe left/right + // axis 1 = Up (dec) / Down (inc) — forward/back + // button 5 = Q (rotate left), button 6 = F (rotate right) + private static final GenericHID kb0 = new GenericHID(0); + private static final GenericHID kb2 = new GenericHID(2); + + /** + * Translation X (strafe left/right). + * Returns the larger-magnitude value between WASD (A/D) and arrow keys. + */ + public static double getTranslationX() { + double wasd = kb0.getRawAxis(0); + double arrows = kb2.getRawAxis(0); + return (Math.abs(arrows) > Math.abs(wasd)) ? arrows : wasd; + } + + /** + * Translation Y (forward/back). + * Returns the larger-magnitude value between WASD (W/S) and arrow keys. + */ + public static double getTranslationY() { + double wasd = kb0.getRawAxis(1); + double arrows = kb2.getRawAxis(1); + return (Math.abs(arrows) > Math.abs(wasd)) ? arrows : wasd; + } + + /** + * Rotation. + * Sources (picks whichever has the largest magnitude): + * - Keyboard0 axis 2: E (dec = left) / R (inc = right) — smooth ramp via keyRate + * - Keyboard2 button 5 (Q) = rotate left (-1.0), button 6 (F) = rotate right (+1.0) + */ + public static double getRotation() { + // E/R keys on Keyboard0 (axis 2) — smooth ramp rotation + double erRot = kb0.getRawAxis(2); + + // Q/F keys on Keyboard2 (buttons) — instant full rotation + double qfRot = 0; + if (kb2.getRawButton(5)) qfRot -= 1.0; // Q = rotate left (CCW) + if (kb2.getRawButton(6)) qfRot += 1.0; // F = rotate right (CW) + + return (Math.abs(qfRot) > Math.abs(erRot)) ? qfRot : erRot; + } + + public static void testInput() { + System.out.printf("X: %.2f Y: %.2f Rot: %.2f (kb0: %.2f/%.2f/%.2f kb2: %.2f/%.2f Q:%b F:%b)%n", + getTranslationX(), getTranslationY(), getRotation(), + kb0.getRawAxis(0), kb0.getRawAxis(1), kb0.getRawAxis(2), + kb2.getRawAxis(0), kb2.getRawAxis(1), + kb2.getRawButton(5), kb2.getRawButton(6)); + } + + /** Get the Keyboard2 HID for button binding */ + public static GenericHID getKeyboard() { + return kb2; + } + + /** Create a CommandGenericHID wrapper for Keyboard2 */ + public static CommandGenericHID getCommandKeyboard() { + return new CommandGenericHID(2); + } +} diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 8776e5d..3f8fc66 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -19,7 +19,7 @@ private Main() {} * *

If you change your main robot class, change the parameter type. */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); + public static void main(String[] args) { + RobotBase.startRobot(() -> new Robot()); } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 47d3d5a..3afb9b0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,9 +4,14 @@ package frc.robot; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.math.geometry.Pose2d; /** * The methods in this class are called automatically corresponding to each mode, as described in @@ -15,17 +20,30 @@ */ public class Robot extends TimedRobot { private Command m_autonomousCommand; + private RobotContainer m_robotContainer; + private boolean robotInitialized = false; - private final RobotContainer m_robotContainer; - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - public Robot() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); + @Override + public void robotInit() { + long startTime = System.currentTimeMillis(); + try { + m_robotContainer = new RobotContainer(); + robotInitialized = true; + SmartDashboard.putBoolean("Robot/Initialized", true); + SmartDashboard.putString("Robot/Mode", "Initialized"); + SmartDashboard.putNumber("Robot/Init Time (ms)", System.currentTimeMillis() - startTime); + + // Simulation info + if (RobotBase.isSimulation()) { + SmartDashboard.putBoolean("Simulation/Running", true); + SmartDashboard.putString("Simulation/Info", "Use Field2d widget to view robot"); + } + } catch (Exception e) { + DriverStation.reportError("Robot initialization failed: " + e.getMessage(), e.getStackTrace()); + SmartDashboard.putBoolean("Robot/Initialized", false); + SmartDashboard.putString("Robot/Error", e.getMessage()); + robotInitialized = false; + } } /** @@ -37,16 +55,27 @@ public Robot() { */ @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. - CommandScheduler.getInstance().run(); + try { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + + // Update match time on dashboard + SmartDashboard.putNumber("Robot/Match Time", Timer.getMatchTime()); + SmartDashboard.putNumber("Robot/FPGA Time", Timer.getFPGATimestamp()); + } catch (Exception e) { + DriverStation.reportError("robotPeriodic error: " + e.getMessage(), false); + } } /** This function is called once each time the robot enters Disabled mode. */ @Override - public void disabledInit() {} + public void disabledInit() { + SmartDashboard.putString("Robot/Mode", "DISABLED"); + SmartDashboard.putBoolean("Robot/Enabled", false); + } @Override public void disabledPeriodic() {} @@ -54,26 +83,96 @@ public void disabledPeriodic() {} /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + SmartDashboard.putString("Robot/Mode", "AUTONOMOUS"); + SmartDashboard.putBoolean("Robot/Enabled", true); - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); + if (!robotInitialized || m_robotContainer == null) { + DriverStation.reportError("Cannot run autonomous - robot not initialized", false); + return; + } + + try { + // Log position BEFORE getting autonomous command (only if DriveSubsystem available) + if (m_robotContainer.m_DriveSubsystem != null) { + Pose2d prePose = m_robotContainer.m_DriveSubsystem.getPose(); + System.out.println("[Robot] Position BEFORE getAutonomousCommand: (" + + String.format("%.2f, %.2f", prePose.getX(), prePose.getY()) + ")"); + } + + // Get a fresh autonomous command each time + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // Log position AFTER getting autonomous command (but before scheduling) + if (m_robotContainer.m_DriveSubsystem != null) { + Pose2d postPose = m_robotContainer.m_DriveSubsystem.getPose(); + System.out.println("[Robot] Position AFTER getAutonomousCommand: (" + + String.format("%.2f, %.2f", postPose.getX(), postPose.getY()) + ")"); + + // Verify robot is on field before starting autonomous + if (!isValidFieldPosition(postPose)) { + DriverStation.reportWarning("WARNING: Robot starting position outside field bounds! Pos: (" + + String.format("%.2f, %.2f", postPose.getX(), postPose.getY()) + ")", false); + } + } + + // Schedule the autonomous command + if (m_autonomousCommand != null) { + CommandScheduler.getInstance().schedule(m_autonomousCommand); + SmartDashboard.putString("Status/Auto Running", m_autonomousCommand.getName()); + System.out.println("[Robot] Autonomous scheduled: " + m_autonomousCommand.getName()); + } else { + SmartDashboard.putString("Status/Auto Running", "None (Error: null command)"); + } + } catch (Exception e) { + DriverStation.reportError("autonomousInit error: " + e.getMessage(), e.getStackTrace()); + SmartDashboard.putString("Status/Auto Running", "Error: " + e.getMessage()); + System.out.println("[Robot] Exception in autonomousInit: " + e.getMessage()); + e.printStackTrace(); } } + /** + * Verify robot position is within valid FRC field bounds. + * FRC field: 16.46m × 8.23m with 0.5m buffer for bumpers + */ + private boolean isValidFieldPosition(Pose2d pose) { + final double FIELD_LENGTH = 16.46; + final double FIELD_WIDTH = 8.23; + final double BUFFER = 0.5; + + boolean xValid = pose.getX() >= -BUFFER && pose.getX() <= FIELD_LENGTH + BUFFER; + boolean yValid = pose.getY() >= -BUFFER && pose.getY() <= FIELD_WIDTH + BUFFER; + + System.out.println("[Robot] Field validation - X: " + String.format("%.2f", pose.getX()) + + " (valid: " + xValid + "), Y: " + String.format("%.2f", pose.getY()) + + " (valid: " + yValid + ")"); + + return xValid && yValid; + } + /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() {} @Override public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. + SmartDashboard.putString("Robot/Mode", "TELEOP"); + SmartDashboard.putBoolean("Robot/Enabled", true); + SmartDashboard.putString("Status/Auto Running", "Cancelled"); + + // Cancel autonomous command when teleop starts if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); + m_autonomousCommand = null; + } + + // Initialize teleop + if (robotInitialized && m_robotContainer != null) { + try { + m_robotContainer.teleopInit(); + } catch (Exception e) { + DriverStation.reportError("teleopInit error: " + e.getMessage(), false); + } } } @@ -83,8 +182,16 @@ public void teleopPeriodic() {} @Override public void testInit() { + SmartDashboard.putString("Robot/Mode", "TEST"); + SmartDashboard.putBoolean("Robot/Enabled", true); // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + + // Auto-start the forward drive diagnostic + if (robotInitialized && m_robotContainer != null && m_robotContainer.m_DriveSubsystem != null) { + CommandScheduler.getInstance().schedule(m_robotContainer.m_DriveSubsystem.testDriveForwardCommand()); + System.out.println("[TEST] Auto-started testDriveForwardCommand"); + } } /** This function is called periodically during test mode. */ @@ -93,7 +200,10 @@ public void testPeriodic() {} /** This function is called once when the robot is first started up. */ @Override - public void simulationInit() {} + public void simulationInit() { + SmartDashboard.putBoolean("Robot/Simulation", true); + SmartDashboard.putString("Robot/Mode", "Simulation Init"); + } /** This function is called periodically whilst in simulation. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a33249e..0b690c6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,63 +1,392 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot; -import frc.robot.Constants.OperatorConstants; -import frc.robot.commands.Autos; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; +import java.io.File; +import java.util.Set; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +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.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -/** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - * subsystems, commands, and trigger mappings) should be declared here. - */ + +import frc.robot.Constants.ControllerConstants; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.FieldConstants; +import frc.robot.commands.Autos; +import frc.robot.commands.auto.AutoAlgaeAuto; +import frc.robot.commands.auto.AutoPathes; +import frc.robot.commands.auto.AutoAvoidCollision; +import frc.robot.commands.auto.AutoBalance; +import frc.robot.commands.auto.AutoComplexPath; +import frc.robot.commands.auto.AutoFigure8; +import frc.robot.commands.auto.AutoPickupAuto; +import frc.robot.commands.auto.AutoProcessorAuto; +import frc.robot.commands.auto.AutoSafeScore; +import frc.robot.commands.auto.AutoTripleScore; +import frc.robot.commands.auto.SequentialAuto; +import frc.robot.commands.auto.SimpleAuto; +import frc.robot.subsystems.DriveSubsystem; +import frc.robot.subsystems.LimelightVision; +import com.pathplanner.lib.auto.AutoBuilder; +@SuppressWarnings("unused") // Many commands and constants are defined but not used yet as we build out functionality incrementally public class RobotContainer { - // The robot's subsystems and commands are defined here... - private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); - // Replace with CommandPS4Controller or CommandJoystick if needed - private final CommandXboxController m_driverController = - new CommandXboxController(OperatorConstants.kDriverControllerPort); + public DriveSubsystem m_DriveSubsystem; // Not final - may fail to initialize + private final CommandXboxController driverXbox; + private final CommandXboxController m_operatorController; + private LimelightVision limelightVision; + private SendableChooser autoChooser; - /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { - // Configure the trigger bindings + // Initialize controller first + driverXbox = new CommandXboxController(ControllerConstants.DRIVER_CONTROLLER_PORT); + m_operatorController = new CommandXboxController(1); + + // Initialize drive subsystem (wrapped in try-catch to allow robot to start even if it fails) + try { + m_DriveSubsystem = new DriveSubsystem(new File(Filesystem.getDeployDirectory(), "SWERVE")); + System.out.println("[RobotContainer] DriveSubsystem initialized successfully"); + + // Set initial robot position based on alliance reported by Driver Station + Pose2d startPose = getAllianceStartPose(); + m_DriveSubsystem.resetOdometry(startPose); + System.out.println("[RobotContainer] Robot initialized at " + + (isRedAlliance() ? "Red" : "Blue") + " Alliance start: (" + + String.format("%.2f, %.2f", startPose.getX(), startPose.getY()) + ")"); + } catch (Exception e) { + DriverStation.reportError("DriveSubsystem initialization failed: " + e.getMessage(), e.getStackTrace()); + System.err.println("[RobotContainer] CRITICAL: DriveSubsystem failed to initialize - robot will run in limited mode"); + e.printStackTrace(); + // m_DriveSubsystem will be null, checks will be needed in methods that use it + } + + // Initialize vision (optional - won't crash if it fails) + try { + limelightVision = new LimelightVision(); + if (m_DriveSubsystem != null) { + limelightVision.setDriveSubsystem(m_DriveSubsystem); + } + } catch (Exception e) { + DriverStation.reportWarning("LimelightVision initialization failed: " + e.getMessage(), false); + limelightVision = null; + } + + // Configure commands and bindings + configureDefaultCommand(); configureBindings(); + + // Setup dashboard + setupSmartDashboard(); + setupAutonomousChooser(); + + SmartDashboard.putBoolean("Robot/Container Initialized", true); + SmartDashboard.putStringArray("Auto/Choices", new String[]{"Simple", "Sequential", "Balance"}); + SmartDashboard.putString("Auto/Choice", "Simple"); } + private void setupSmartDashboard() { + SmartDashboard.putData("Commands", CommandScheduler.getInstance()); + + // Alliance color selector + SmartDashboard.putString("Alliance/Color", "Blue"); + + // Control mode indicator + SmartDashboard.putString("Controls/Mode", "Keyboard: Arrows+QE | Xbox: Sticks (both work)"); + + // Field constants for debugging + SmartDashboard.putNumber("Field/Length (m)", 17.54); + SmartDashboard.putNumber("Field/Width (m)", 8.21); + SmartDashboard.putNumber("Field/Blue Start X", FieldConstants.BLUE_ALLIANCE_START.getX()); + SmartDashboard.putNumber("Field/Blue Start Y", FieldConstants.BLUE_ALLIANCE_START.getY()); + SmartDashboard.putNumber("Field/Red Start X", FieldConstants.RED_ALLIANCE_START.getX()); + SmartDashboard.putNumber("Field/Red Start Y", FieldConstants.RED_ALLIANCE_START.getY()); + SmartDashboard.putNumber("Field/Hub Rest X", FieldConstants.HUB_REST_POSITION.getX()); + SmartDashboard.putNumber("Field/Hub Rest Y", FieldConstants.HUB_REST_POSITION.getY()); + + // Control instructions - Xbox Controller + SmartDashboard.putString("Xbox/Left Stick", "Move Robot"); + SmartDashboard.putString("Xbox/Right Stick X", "Rotate Robot"); + SmartDashboard.putString("Xbox/Right Bumper (R1)", "Increase Speed (+10%)"); + SmartDashboard.putString("Xbox/Left Bumper (L1)", "Decrease Speed (-10%)"); + SmartDashboard.putString("Xbox/Right Trigger", "Full Speed Mode"); + SmartDashboard.putString("Xbox/Left Trigger", "Precision Mode"); + SmartDashboard.putString("Xbox/A Button", "Test Auto"); + SmartDashboard.putString("Xbox/B Button", "Reset to Blue Start"); + SmartDashboard.putString("Xbox/X Button", "Reset to Red Start"); + SmartDashboard.putString("Xbox/Y Button", "Reset to Center"); + SmartDashboard.putString("Xbox/POV Down", "Reset Heading"); + SmartDashboard.putString("Xbox/POV Up", "Zero Gyro & Sync Modules"); + + // Keyboard instructions + SmartDashboard.putString("Keyboard/W", "Forward"); + SmartDashboard.putString("Keyboard/S", "Backward"); + SmartDashboard.putString("Keyboard/A", "Strafe Left"); + SmartDashboard.putString("Keyboard/D", "Strafe Right"); + SmartDashboard.putString("Keyboard/Q", "Rotate Left (instant)"); + SmartDashboard.putString("Keyboard/F", "Rotate Right (instant)"); + SmartDashboard.putString("Keyboard/E", "Rotate Left (smooth)"); + SmartDashboard.putString("Keyboard/R", "Rotate Right (smooth)"); + SmartDashboard.putString("Keyboard/Arrows", "Alternative WASD"); + + // Computer key (dashboard) to lock and rest near hub (only if DriveSubsystem available) + if (m_DriveSubsystem != null) { + SmartDashboard.putData("Drive/Hub Rest", Commands.runOnce(() -> { + m_DriveSubsystem.resetOdometry(FieldConstants.HUB_REST_POSITION); + m_DriveSubsystem.lock(); + SmartDashboard.putString("Status/Last Action", "Hub Rest (Dashboard)"); + }, m_DriveSubsystem).withName("HubRest")); + } + } + + private void setupAutonomousChooser() { + // Use PathPlanner's AutoBuilder if configured, otherwise fall back to manual chooser + if (m_DriveSubsystem != null && AutoBuilder.isConfigured()) { + // PathPlanner auto chooser - reads autos from src/main/deploy/pathplanner/autos/ + autoChooser = AutoBuilder.buildAutoChooser(); + System.out.println("[RobotContainer] PathPlanner AutoBuilder chooser configured"); + } else { + // Fallback: manual auto chooser using deferred commands (so they can be re-run) + autoChooser = new SendableChooser<>(); + autoChooser.setDefaultOption("Do Nothing", Autos.doNothingAuto()); + + if (m_DriveSubsystem != null) { + Set driveReqs = Set.of(m_DriveSubsystem); + autoChooser.addOption("Drive Forward", + Commands.defer(() -> Autos.driveForwardAuto(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Simple (Reef)", + Commands.defer(() -> new SimpleAuto(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Safe Score (1 piece)", + Commands.defer(() -> new AutoSafeScore(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Sequential (2 pieces)", + Commands.defer(() -> new SequentialAuto(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Triple Score (3 pieces)", + Commands.defer(() -> new AutoTripleScore(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Avoid Collision", + Commands.defer(() -> new AutoAvoidCollision(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Algae Removal", + Commands.defer(() -> new AutoAlgaeAuto(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Processor Scoring", + Commands.defer(() -> new AutoProcessorAuto(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Complex Path", + Commands.defer(() -> new AutoComplexPath(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Balance", + Commands.defer(() -> new AutoBalance(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Pickup Cycles", + Commands.defer(() -> new AutoPickupAuto(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Figure 8 Pattern", + Commands.defer(() -> new AutoFigure8(m_DriveSubsystem), driveReqs)); + autoChooser.addOption("Auto Pathes (PathPlanner)", + Commands.defer(() -> new AutoPathes(m_DriveSubsystem), driveReqs)); + } else { + DriverStation.reportWarning("DriveSubsystem not initialized - limited autonomous options available", false); + } + } + + SmartDashboard.putData("Auto Chooser", autoChooser); + } + + private void configureDefaultCommand() { + if (m_DriveSubsystem != null) { + m_DriveSubsystem.setDefaultCommand( + m_DriveSubsystem.driveCommand( + // Forward/back: W/S keys (sim) or Xbox left stick Y (real robot) — pick larger magnitude + () -> { + double kb = -Input.getTranslationY(); + double xbox = -driverXbox.getLeftY(); + return (Math.abs(kb) > Math.abs(xbox)) ? kb : xbox; + }, + // Strafe: A/D keys (sim) or Xbox left stick X (real robot) + () -> { + double kb = -Input.getTranslationX(); + double xbox = -driverXbox.getLeftX(); + return (Math.abs(kb) > Math.abs(xbox)) ? kb : xbox; + }, + // Rotation: E/R axis + Q/F buttons (sim) or Xbox right stick X (real robot) + // Keyboard gives 0 or ±1 (buttons) or slow ramp (axis), so don't pre-scale here. + // DriveSubsystem.driveCommand applies cube scaling internally, which is enough. + // Only apply ROTATION_SCALE to Xbox stick (which is analog and needs dampening). + () -> { + double kb = -Input.getRotation(); + double xbox = -driverXbox.getRightX() * DriveConstants.ROTATION_SCALE; + return (Math.abs(kb) > Math.abs(xbox)) ? kb : xbox; + } + ).withName("DefaultDrive") + ); + } + } + + private void configureBindings() { + // Skip binding drive commands if DriveSubsystem failed to initialize + if (m_DriveSubsystem == null) { + DriverStation.reportWarning("DriveSubsystem not available - skipping drive button bindings", false); + return; + } + + // Back button - EMERGENCY STOP: instantly stop all motors and lock wheels + driverXbox.back().onTrue(Commands.runOnce(() -> { + CommandScheduler.getInstance().cancelAll(); + m_DriveSubsystem.stop(); + m_DriveSubsystem.lock(); + SmartDashboard.putString("Status/Last Action", "EMERGENCY STOP"); + System.out.println("[RobotContainer] EMERGENCY STOP triggered"); + }, m_DriveSubsystem)); + + // Speed control with bumpers — persistently adjusts drive speed + driverXbox.rightBumper().onTrue(Commands.runOnce(() -> { + m_DriveSubsystem.increaseSpeed(); + SmartDashboard.putString("Status/Last Action", + "Speed: " + String.format("%.0f%%", m_DriveSubsystem.getSpeedMultiplier() * 100)); + })); + driverXbox.leftBumper().onTrue(Commands.runOnce(() -> { + m_DriveSubsystem.decreaseSpeed(); + SmartDashboard.putString("Status/Last Action", + "Speed: " + String.format("%.0f%%", m_DriveSubsystem.getSpeedMultiplier() * 100)); + })); + + // Heading reset (POV Down) + driverXbox.povDown().onTrue(Commands.runOnce(() -> { + m_DriveSubsystem.seedForwards(); + SmartDashboard.putString("Status/Last Action", "Heading Reset"); + })); + + // Zero gyro and sync modules (POV Up) + driverXbox.povUp().onTrue(Commands.runOnce(() -> { + m_DriveSubsystem.zeroGyro(); + m_DriveSubsystem.synchronizeModuleEncoders(); + SmartDashboard.putString("Status/Last Action", "Gyro & Modules Zeroed"); + })); + + // Print encoder offsets for calibration (POV Left) + driverXbox.povLeft().onTrue(Commands.runOnce(() -> { + m_DriveSubsystem.printEncoderOffsets(); + SmartDashboard.putString("Status/Last Action", "Printed Encoder Offsets - Check Console"); + })); + + // POV Right - Lock wheels + driverXbox.povRight().onTrue(Commands.runOnce(() -> { + m_DriveSubsystem.lock(); + SmartDashboard.putString("Status/Last Action", "Wheels Locked"); + })); + + // Position resets + driverXbox.b().onTrue(Commands.runOnce(() -> { + m_DriveSubsystem.resetOdometry(FieldConstants.BLUE_ALLIANCE_START); + SmartDashboard.putString("Status/Last Action", "Reset to Blue Start"); + })); + + driverXbox.x().onTrue(Commands.runOnce(() -> { + m_DriveSubsystem.resetOdometry(FieldConstants.RED_ALLIANCE_START); + SmartDashboard.putString("Status/Last Action", "Reset to Red Start"); + })); + + driverXbox.y().onTrue(Commands.runOnce(() -> { + m_DriveSubsystem.resetOdometry(FieldConstants.CENTER_START); + SmartDashboard.putString("Status/Last Action", "Reset to Center"); + })); + + // Controller key to lock at hub rest position (press START) + driverXbox.start().onTrue(Commands.runOnce(() -> { + m_DriveSubsystem.resetOdometry(FieldConstants.HUB_REST_POSITION); + m_DriveSubsystem.lock(); + SmartDashboard.putString("Status/Last Action", "Hub Rest (Start)"); + }, m_DriveSubsystem)); + + // Full speed mode (right trigger) - Remove duplicate deadband + driverXbox.rightTrigger(ControllerConstants.TRIGGER_THRESHOLD) + .onTrue(Commands.runOnce(() -> SmartDashboard.putString("Status/Speed Mode", "FULL SPEED"))) + .onFalse(Commands.runOnce(() -> SmartDashboard.putString("Status/Speed Mode", "Normal"))) + .whileTrue( + m_DriveSubsystem.driveCommand( + () -> -driverXbox.getLeftY(), + () -> -driverXbox.getLeftX(), + () -> -driverXbox.getRightX() * DriveConstants.FULL_SPEED_ROTATION_SCALE + ).withName("FullSpeedDrive") + ); + + // Precision mode (left trigger) - Remove duplicate deadband + driverXbox.leftTrigger(ControllerConstants.TRIGGER_THRESHOLD) + .onTrue(Commands.runOnce(() -> SmartDashboard.putString("Status/Speed Mode", "PRECISION"))) + .onFalse(Commands.runOnce(() -> SmartDashboard.putString("Status/Speed Mode", "Normal"))) + .whileTrue( + m_DriveSubsystem.driveCommand( + () -> -driverXbox.getLeftY() * DriveConstants.PRECISION_MULTIPLIER, + () -> -driverXbox.getLeftX() * DriveConstants.PRECISION_MULTIPLIER, + () -> -driverXbox.getRightX() * DriveConstants.PRECISION_ROTATION_SCALE + ).withName("PrecisionDrive") + ); + + // Test: A button = pure forward drive (1 m/s, no joystick, no field-relative) + // If robot spins during this, the issue is module config. If straight, issue is joystick/field-relative. + driverXbox.a() + .onTrue(Commands.runOnce(() -> SmartDashboard.putString("Status/Last Action", "TEST FORWARD DRIVE"))) + .whileTrue(m_DriveSubsystem.testDriveForwardCommand()); + + // Initialize status + SmartDashboard.putString("Status/Speed Mode", "Normal"); + SmartDashboard.putString("Status/Last Action", "Ready"); + + // Operator diagnostics (only if DriveSubsystem available) + if (m_DriveSubsystem != null) { + m_operatorController.a().onTrue(Commands.runOnce(m_DriveSubsystem::printEncoderOffsets)); + m_operatorController.b().onTrue(Commands.runOnce(m_DriveSubsystem::diagnoseGyro)); + m_operatorController.x().onTrue(Commands.runOnce(m_DriveSubsystem::diagnoseMotorInversions)); + } + } + /** - * Use this method to define your trigger->command mappings. Triggers can be created via the - * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary - * predicate, or via the named factories in {@link - * edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for {@link - * CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller - * PS4} controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight - * joysticks}. + * Get the autonomous command to run. + * Uses PathPlanner auto chooser if configured, otherwise falls back to manual chooser. */ - private void configureBindings() { - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - new Trigger(m_exampleSubsystem::exampleCondition) - .onTrue(new ExampleCommand(m_exampleSubsystem)); + public Command getAutonomousCommand() { + // If DriveSubsystem failed to initialize, return do-nothing command + if (m_DriveSubsystem == null) { + DriverStation.reportWarning("Cannot run autonomous - DriveSubsystem not initialized", false); + return Autos.doNothingAuto(); + } + + // Get command from auto chooser (PathPlanner or manual) + Command selectedAuto = autoChooser.getSelected(); + + if (selectedAuto != null) { + System.out.println("[RobotContainer] Selected autonomous: " + selectedAuto.getName()); + return selectedAuto; + } + + return Autos.doNothingAuto(); + } - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. - m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); + /** + * Returns true if the Driver Station reports Red alliance (or defaults to false/Blue if unknown). + */ + private boolean isRedAlliance() { + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; } /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous + * Returns the correct starting pose based on the current alliance color. */ - public Command getAutonomousCommand() { - // An example command will be run in autonomous - return Autos.exampleAuto(m_exampleSubsystem); + private Pose2d getAllianceStartPose() { + return isRedAlliance() ? FieldConstants.RED_ALLIANCE_START : FieldConstants.BLUE_ALLIANCE_START; } + + /** + * Called when teleop starts. + */ + public void teleopInit() { + if (m_DriveSubsystem == null) { + DriverStation.reportWarning("DriveSubsystem not available for teleop", false); + return; + } + + // Reset odometry to the correct alliance start position + Pose2d startPose = getAllianceStartPose(); + m_DriveSubsystem.resetOdometry(startPose); + System.out.println("Teleop initialized - odometry reset to " + + (isRedAlliance() ? "Red" : "Blue") + " Alliance start"); + } } diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 107aad7..222a006 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -1,20 +1,64 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.commands; -import frc.robot.subsystems.ExampleSubsystem; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.DriveSubsystem; +/** + * Utility class containing autonomous routines for FIRST Rebuilt 2025. + * + * Game: FIRST Reefscape Rebuilt + * + * Scoring Opportunities: + * - Reef Scoring: 3 points per coral (3 branches: Front, Left, Right) + * - Processor Scoring: 4 points per coral (higher value) + * - Algae Removal: Bonus points (variable) + * + * Field Layout: + * - Field Size: 16.54m × 8.07m (54.27' × 26.47') + * - Reef Center: (8.27, 4.035) - Center obstacle + * - Coral Stations: Alliance walls for pickup + * - Processor Stations: Side stations for bonus scoring + * + * Available Auto Routines: + * - Simple: 1 coral (3 pts) - Safe and reliable + * - Triple Score: 3 corals (9 pts) - Multi-branch scoring + * - Complex Path: 3 corals (9 pts) - With pickup cycles + * - Pickup Cycles: 3 corals (9 pts) - Rapid cycling practice + * - Sequential: 1 coral (3 pts) - Multi-waypoint demo + * - Processor: 1 coral (4 pts) - Processor bonus scoring + * - Algae Removal: Variable points - Field control + * - Figure 8: Navigation practice only + * - Balance/Center: Positioning practice only + * - Collision Avoid: Safe pathing practice + */ public final class Autos { - /** Example static factory for an autonomous command. */ - public static Command exampleAuto(ExampleSubsystem subsystem) { - return Commands.sequence(subsystem.exampleMethodCommand(), new ExampleCommand(subsystem)); + + /** + * Do nothing autonomous - robot stays still. + */ + public static Command doNothingAuto() { + return Commands.none().withName("Do Nothing"); + } + + /** + * Drive forward autonomous routine. + * Robot drives forward at a constant speed for a set duration. + */ + public static Command driveForwardAuto(DriveSubsystem driveSubsystem) { + return Commands.sequence( + // Drive forward at 50% speed for 3 seconds + Commands.run(() -> driveSubsystem.driveFieldOriented( + new ChassisSpeeds(2.25, 0, 0)), driveSubsystem) + .withTimeout(3), + // Stop the robot + Commands.runOnce(() -> driveSubsystem.stop(), driveSubsystem) + ).withName("Drive Forward"); } + // Private constructor to prevent instantiation private Autos() { - throw new UnsupportedOperationException("This is a utility class!"); + throw new UnsupportedOperationException("Utility class - do not instantiate"); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java index 7481d3c..e57403c 100644 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ b/src/main/java/frc/robot/commands/ExampleCommand.java @@ -9,7 +9,7 @@ /** An example command that uses an example subsystem. */ public class ExampleCommand extends Command { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField","unused"}) private final ExampleSubsystem m_subsystem; /** diff --git a/src/main/java/frc/robot/commands/auto/AutoAlgaeAuto.java b/src/main/java/frc/robot/commands/auto/AutoAlgaeAuto.java new file mode 100644 index 0000000..565389d --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoAlgaeAuto.java @@ -0,0 +1,56 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Algae Removal Auto - FIRST Rebuilt 2025 Strategy + * + * Game: FIRST Reefscape Rebuilt + * Objective: Clear algae from designated field zones + * + * Strategy: + * 1. Start at Blue Alliance + * 2. Navigate to left algae zone (X=4.0m) + * 3. Clear algae (wait for mechanism) + * 4. Navigate to right algae zone (X=13.0m) + * 5. Clear algae (wait for mechanism) + * 6. Hold position + * + * Points: Variable (depends on algae cleared) + * Time: ~18 seconds + * Note: Algae removal provides bonus points and field control + */ +public class AutoAlgaeAuto extends SequentialCommandGroup { + + public AutoAlgaeAuto(DriveSubsystem drive) { + addCommands( + Commands.runOnce(() -> { + System.out.println("\n========== ALGAE REMOVAL - FIRST REBUILT 2025 =========="); + System.out.println("Strategy: Clear algae from field zones"); + System.out.println("Target: 2 algae zones"); + System.out.println("Expected Points: Variable (bonus points)"); + System.out.println("======================================================\n"); + }), + Commands.runOnce(() -> drive.stop()), + Commands.waitSeconds(0.3), + Commands.runOnce(() -> System.out.println("[Algae] Patrolling algae zones")), + new AutoDriveCommand(drive, new Pose2d(4.0, 4.0, Rotation2d.kZero), 2.0, 6.0), + Commands.waitSeconds(0.5), + new AutoDriveCommand(drive, new Pose2d(13.0, 4.0, Rotation2d.kZero), 2.0, 6.0), + Commands.runOnce(() -> { + drive.stop(); + drive.lock(); + System.out.println("[Algae] Complete\n"); + }), + + // Hold position until autonomous ends - prevents default command from taking over + Commands.run(() -> { + drive.stop(); + }, drive).withName("HoldPosition") + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/AutoAvoidCollision.java b/src/main/java/frc/robot/commands/auto/AutoAvoidCollision.java new file mode 100644 index 0000000..93558fa --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoAvoidCollision.java @@ -0,0 +1,65 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Collision Avoidance Auto - FIRST Rebuilt 2025 Strategy + * + * Game: FIRST Reefscape Rebuilt + * Objective: Navigate around field obstacles (reef and game pieces) + * + * Strategy: + * 1. Start at Blue Alliance + * 2. Move along perimeter to avoid center reef + * 3. Navigate to safe approach position north of reef + * 4. Approach Reef Left scoring position carefully + * 5. Retreat via perimeter route back to safe zone + * + * Points: 0 (navigation practice - can be modified for scoring) + * Time: ~25 seconds + * Purpose: Demonstrate collision avoidance and safe pathing + * Best for: Testing obstacle detection and path planning + */ +public class AutoAvoidCollision extends SequentialCommandGroup { + + public AutoAvoidCollision(DriveSubsystem drive) { + addCommands( + Commands.runOnce(() -> { + System.out.println("\n========== COLLISION AVOID - FIRST REBUILT 2025 =========="); + System.out.println("Strategy: Navigate around center field obstacles"); + System.out.println("Objective: Safe pathing demonstration"); + System.out.println("Expected Points: 0 (navigation practice)"); + System.out.println("=========================================================\n"); + }), + Commands.runOnce(() -> drive.stop()), + Commands.waitSeconds(0.3), + Commands.runOnce(() -> System.out.println("[AvoidCollision] Moving along perimeter")), + new AutoDriveCommand(drive, new Pose2d(1.5, 6.5, Rotation2d.kZero), 2.0, 6.0), + Commands.waitSeconds(0.3), + Commands.runOnce(() -> System.out.println("[AvoidCollision] Moving to safe approach")), + new AutoDriveCommand(drive, new Pose2d(7.0, 6.0, Rotation2d.fromDegrees(120.0)), 2.0, 8.0), + Commands.waitSeconds(0.3), + Commands.runOnce(() -> System.out.println("[AvoidCollision] Final approach")), + new AutoDriveCommand(drive, FieldConstants.REEF_LEFT, 1.5, 6.0), + Commands.waitSeconds(1.5), + Commands.runOnce(() -> System.out.println("[AvoidCollision] Retreating")), + new AutoDriveCommand(drive, new Pose2d(3.0, 6.0, Rotation2d.kZero), 2.0, 8.0), + new AutoDriveCommand(drive, FieldConstants.BLUE_SAFE_ZONE, 2.0, 8.0), + Commands.runOnce(() -> { + System.out.println("[AvoidCollision] Complete\n"); + drive.stop(); + drive.lock(); + }), + + // Hold position until autonomous ends - prevents default command from taking over + Commands.run(() -> { + drive.stop(); + }, drive).withName("HoldPosition") + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/AutoBalance.java b/src/main/java/frc/robot/commands/auto/AutoBalance.java new file mode 100644 index 0000000..26d9df7 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoBalance.java @@ -0,0 +1,65 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Balance/Center Auto - FIRST Rebuilt 2025 Strategy + * + * Game: FIRST Reefscape Rebuilt + * Objective: Navigate to field center and maintain position + * + * Strategy: + * 1. Start at Blue Alliance + * 2. Navigate to field center (8.27, 4.035) + * 3. Hold position for balance/stability test + * 4. Lock wheels to maintain position + * + * Points: 0 (positioning practice) + * Time: ~15 seconds + * Purpose: Test center field navigation and position holding + * Note: Can be modified for endgame balance/park scoring + */ +public class AutoBalance extends SequentialCommandGroup { + + private static final double BALANCE_POSITION_X = 8.27; + private static final Pose2d BALANCE_POSITION = new Pose2d(BALANCE_POSITION_X, 4.035, Rotation2d.kZero); + + public AutoBalance(DriveSubsystem drive) { + addCommands( + Commands.runOnce(() -> { + System.out.println("\n========== BALANCE/CENTER - FIRST REBUILT 2025 =========="); + System.out.println("Strategy: Navigate to center and hold position"); + System.out.println("Target: Field center (" + String.format("%.2f, %.2f", + BALANCE_POSITION.getX(), BALANCE_POSITION.getY()) + ")"); + System.out.println("Expected Points: 0 (positioning practice)"); + System.out.println("========================================================\n"); + }), + + Commands.runOnce(() -> drive.stop()), + Commands.waitSeconds(0.3), + + Commands.runOnce(() -> System.out.println("[Balance] Moving to balance position")), + new AutoDriveCommand(drive, BALANCE_POSITION, 2.0, 8.0), + + Commands.waitSeconds(0.5), + + Commands.runOnce(() -> System.out.println("[Balance] Maintaining position")), + Commands.waitSeconds(2.0), + + Commands.runOnce(() -> { + drive.stop(); + drive.lock(); + System.out.println("[Balance] Complete - holding position\n"); + }), + + // Hold position until autonomous ends - prevents default command from taking over + Commands.run(() -> { + drive.stop(); + }, drive).withName("HoldPosition") + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/AutoComplexPath.java b/src/main/java/frc/robot/commands/auto/AutoComplexPath.java new file mode 100644 index 0000000..9286f26 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoComplexPath.java @@ -0,0 +1,115 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Complex Path Auto - FIRST Rebuilt 2025 Strategy + * + * Game: FIRST Reefscape Rebuilt + * Objective: Maximum coral scoring with multiple pickup cycles + * + * Strategy (3-piece auto): + * 1. Start with preloaded coral → Score on Reef Front (Level 1) + * 2. Pickup from Coral Station → Score on Reef Left (Level 1) + * 3. Pickup from Coral Station → Score on Reef Right (Level 1) + * 4. Return to safe zone for endgame positioning + * + * Points: 9 points (3 corals × 3 points each) + * Time: ~25-30 seconds + * Complexity: High - requires precise navigation and coral handling + * Best for: Experienced drivers, reliable intake/outtake mechanisms + */ +public class AutoComplexPath extends SequentialCommandGroup { + + public AutoComplexPath(DriveSubsystem drive) { + addCommands( + // Startup diagnostics + Commands.runOnce(() -> { + System.out.println("\n========== COMPLEX PATH - FIRST REBUILT 2025 =========="); + System.out.println("Strategy: Multi-coral scoring (3-piece auto)"); + Pose2d startPose = drive.getPose(); + System.out.println("Starting position: (" + + String.format("%.2f, %.2f", startPose.getX(), startPose.getY()) + ")"); + System.out.println("\nScoring Sequence:"); + System.out.println(" 1. Reef Front (preload): (8.27, 2.5) @ 0° → 3 pts"); + System.out.println(" 2. Pickup: Coral Station (1.2, 7.0)"); + System.out.println(" 3. Reef Left: (7.2, 5.0) @ 120° → 3 pts"); + System.out.println(" 4. Pickup: Coral Station (1.2, 7.0)"); + System.out.println(" 5. Reef Right: (9.3, 5.0) @ 60° → 3 pts"); + System.out.println(" 6. Safe Zone: (2.0, 1.5) @ 0°"); + System.out.println("\nExpected Points: 9 (3 corals on reef)"); + System.out.println("Field: 16.54m × 8.07m (Rebuilt 2025)"); + System.out.println("=====================================================\n"); + }), + + // Initialize + Commands.runOnce(() -> { + System.out.println("[ComplexPath] Ready with preloaded coral"); + drive.stop(); + }), + Commands.waitSeconds(0.5), + + // Score 1: Reef Front (preload) + Commands.runOnce(() -> { + System.out.println("[ComplexPath] Score 1/3: Reef Front (preload)"); + }), + new AutoDriveCommand(drive, FieldConstants.REEF_FRONT, 2.5, 8.0), + Commands.waitSeconds(1.0), // Score time + + // Pickup 1: Coral Station + Commands.runOnce(() -> { + System.out.println("[ComplexPath] Pickup 1/2: Coral Station"); + }), + new AutoDriveCommand(drive, FieldConstants.BLUE_CORAL_STATION, 2.5, 8.0), + Commands.waitSeconds(1.0), // Pickup time + + // Score 2: Reef Left + Commands.runOnce(() -> { + System.out.println("[ComplexPath] Score 2/3: Reef Left (120°)"); + }), + new AutoDriveCommand(drive, FieldConstants.REEF_LEFT, 2.5, 8.0), + Commands.waitSeconds(1.0), // Score time + + // Pickup 2: Coral Station + Commands.runOnce(() -> { + System.out.println("[ComplexPath] Pickup 2/2: Coral Station"); + }), + new AutoDriveCommand(drive, FieldConstants.BLUE_CORAL_STATION, 2.5, 8.0), + Commands.waitSeconds(1.0), // Pickup time + + // Score 3: Reef Right + Commands.runOnce(() -> { + System.out.println("[ComplexPath] Score 3/3: Reef Right (60°)"); + }), + new AutoDriveCommand(drive, FieldConstants.REEF_RIGHT, 2.5, 8.0), + Commands.waitSeconds(1.0), // Score time + + // Return to safe zone + Commands.runOnce(() -> { + System.out.println("[ComplexPath] Returning to safe zone"); + }), + new AutoDriveCommand(drive, FieldConstants.BLUE_SAFE_ZONE, 2.5, 10.0), + + // Finish + Commands.runOnce(() -> { + Pose2d finalPose = drive.getPose(); + System.out.println("[ComplexPath] [OK] Complete at (" + + String.format("%.2f, %.2f", finalPose.getX(), finalPose.getY()) + ")"); + System.out.println("[ComplexPath] [OK] Total pieces scored: 3"); + System.out.println("[ComplexPath] [OK] Reef branches covered: Front, Left, Right"); + drive.stop(); + drive.lock(); + System.out.println(); + }), + + // Hold position until autonomous ends - prevents default command from taking over + Commands.run(() -> { + drive.stop(); + }, drive).withName("HoldPosition") + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/AutoDriveCommand.java b/src/main/java/frc/robot/commands/auto/AutoDriveCommand.java new file mode 100644 index 0000000..1ed1fd7 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoDriveCommand.java @@ -0,0 +1,138 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Drive to a target pose autonomously. + * Moves in straight lines between waypoints using field-oriented control. + */ +public class AutoDriveCommand extends Command { + private final DriveSubsystem drive; + private final Pose2d targetPose; + private final double maxSpeed; + private final double timeoutSeconds; + private final double positionTolerance; + private double startTime; + + private static final double DEFAULT_POSITION_TOLERANCE = 0.20; // meters + private static final double ANGLE_TOLERANCE = 15.0; // degrees + private static final double KP_LINEAR = 2.0; + private static final double KP_ANGULAR = 1.5; + private static final double MIN_SPEED = 0.3; // m/s + + public AutoDriveCommand(DriveSubsystem drive, Pose2d targetPose, double positionTolerance, double maxSpeed, double timeoutSeconds) { + this.drive = drive; + this.targetPose = targetPose; + this.positionTolerance = positionTolerance; + this.maxSpeed = maxSpeed; + this.timeoutSeconds = timeoutSeconds; + addRequirements(drive); + } + + // Convenience constructor with default tolerance + public AutoDriveCommand(DriveSubsystem drive, Pose2d targetPose, double maxSpeed, double timeoutSeconds) { + this(drive, targetPose, DEFAULT_POSITION_TOLERANCE, maxSpeed, timeoutSeconds); + } + + @Override + public void initialize() { + startTime = Timer.getFPGATimestamp(); + + Pose2d currentPose = drive.getPose(); + double distance = currentPose.getTranslation().getDistance(targetPose.getTranslation()); + + System.out.println("[AutoDriveCommand] Moving from (" + + String.format("%.2f, %.2f @ %.0f deg", currentPose.getX(), currentPose.getY(), currentPose.getRotation().getDegrees()) + + ") to (" + String.format("%.2f, %.2f @ %.0f deg", targetPose.getX(), targetPose.getY(), targetPose.getRotation().getDegrees()) + + ") | Distance: " + String.format("%.2fm", distance)); + } + + @Override + public void execute() { + Pose2d currentPose = drive.getPose(); + + // Calculate error to target in field frame + double dx = targetPose.getX() - currentPose.getX(); + double dy = targetPose.getY() - currentPose.getY(); + double distance = Math.hypot(dx, dy); + + // Calculate angle error + double angleError = normalizeAngle( + targetPose.getRotation().getDegrees() - currentPose.getRotation().getDegrees() + ); + + // If close enough to target position, focus on rotation + if (distance < positionTolerance) { + if (Math.abs(angleError) < ANGLE_TOLERANCE) { + drive.stop(); + return; + } + + double omega = KP_ANGULAR * Math.toRadians(angleError); + omega = clamp(omega, -Math.PI, Math.PI); + drive.driveFieldOriented(new ChassisSpeeds(0, 0, omega)); + return; + } + + // Calculate speed toward target with proportional control + double speed = Math.min(KP_LINEAR * distance, maxSpeed); + speed = Math.max(MIN_SPEED, speed); // Minimum speed to overcome friction + + // Normalize direction vector and apply speed + double vx = (dx / distance) * speed; + double vy = (dy / distance) * speed; + + // Also correct heading while moving (but limit rotation to avoid instability) + double omega = KP_ANGULAR * Math.toRadians(angleError); + omega = clamp(omega, -Math.PI / 2, Math.PI / 2); + + // Send velocities to drive (field-oriented) + drive.driveFieldOriented(new ChassisSpeeds(vx, vy, omega)); + } + + @Override + public boolean isFinished() { + Pose2d currentPose = drive.getPose(); + double distance = currentPose.getTranslation().getDistance(targetPose.getTranslation()); + + double angleError = Math.abs(normalizeAngle( + targetPose.getRotation().getDegrees() - currentPose.getRotation().getDegrees() + )); + + boolean atTarget = distance < positionTolerance && angleError < ANGLE_TOLERANCE; + boolean timedOut = (Timer.getFPGATimestamp() - startTime) > timeoutSeconds; + + if (atTarget) { + System.out.println("[AutoDriveCommand] [OK] Reached target"); + } + if (timedOut) { + System.out.println("[AutoDriveCommand] [WARN] Timed out - distance: " + + String.format("%.2f", distance) + "m, angle error: " + + String.format("%.1f", angleError) + " deg"); + } + + return atTarget || timedOut; + } + + @Override + public void end(boolean interrupted) { + drive.stop(); + if (interrupted) { + System.out.println("[AutoDriveCommand] [WARN] Interrupted"); + } + } + + private double normalizeAngle(double angle) { + while (angle > 180) angle -= 360; + while (angle < -180) angle += 360; + return angle; + } + + private double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } +} diff --git a/src/main/java/frc/robot/commands/auto/AutoFigure8.java b/src/main/java/frc/robot/commands/auto/AutoFigure8.java new file mode 100644 index 0000000..86a6291 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoFigure8.java @@ -0,0 +1,169 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Figure 8 Navigation Auto - FIRST Rebuilt 2025 Strategy + * + * Game: FIRST Reefscape Rebuilt + * Objective: Demonstrate advanced field navigation around reef obstacle + * + * Strategy: + * - Navigate in figure 8 pattern around entire Rebuilt field + * - Avoid center reef structure at all times + * - Left loop: Counter-clockwise around west side + * - Right loop: Clockwise around east side + * - Cross field safely above reef (Y=6.9m) + * + * Path Design: + * - Starts at Blue Alliance wall (X=1.5, Y=4.035) + * - Moves NORTH first to avoid reef collision + * - Left loop: X < 5.5m (stays west of reef) + * - Crosses at Y=6.9m (2.87m clearance above reef) + * - Right loop: X > 10.5m (stays east of reef) + * - Returns via safe northern route + * + * Safety Zones: + * - Reef danger zone: X=6.5-10.0m, Y=2.5-5.5m (avoided completely) + * - Crossing height: Y=6.9m (safe clearance) + * - Field boundaries: X=0.5-16.04m, Y=0.5-7.57m (0.5m margin) + * + * Points: 0 (navigation practice only) + * Time: ~35-40 seconds + * Purpose: Test autonomous navigation and obstacle avoidance + */ +public class AutoFigure8 extends SequentialCommandGroup { + + // Safety constants + private static final double REEF_CENTER_X = 8.27; + @SuppressWarnings("unused")// to suppress the error message because I don't like it. + private static final double REEF_CENTER_Y = 4.035; + private static final double SAFE_CROSSING_Y = 6.9; // 2.87m above reef center + private static final double LEFT_ZONE_MAX_X = 5.5; + private static final double RIGHT_ZONE_MIN_X = 10.5; + + // Initial safe movement - go NORTH first, then around perimeter + private static final Pose2d SAFE_NORTH = new Pose2d(1.5, SAFE_CROSSING_Y, Rotation2d.fromDegrees(0)); + + // LEFT LOOP waypoints (counter-clockwise around left side) + private static final Pose2d LEFT_TOP = new Pose2d(3.0, 7.4, Rotation2d.fromDegrees(0)); + private static final Pose2d LEFT_MID_EAST = new Pose2d(LEFT_ZONE_MAX_X, 6.3, Rotation2d.fromDegrees(-45)); + private static final Pose2d LEFT_BOTTOM = new Pose2d(3.5, 1.5, Rotation2d.fromDegrees(-90)); + private static final Pose2d LEFT_MID_RETURN = new Pose2d(5.0, 3.0, Rotation2d.fromDegrees(45)); + + // CROSSING waypoint - safely ABOVE the reef + private static final Pose2d CROSSING_NORTH = new Pose2d(REEF_CENTER_X, SAFE_CROSSING_Y, Rotation2d.fromDegrees(0)); + + // RIGHT LOOP waypoints (clockwise around right side) + private static final Pose2d RIGHT_TOP = new Pose2d(11.5, 7.4, Rotation2d.fromDegrees(0)); + private static final Pose2d RIGHT_EAST = new Pose2d(13.5, 5.0, Rotation2d.fromDegrees(-90)); + private static final Pose2d RIGHT_BOTTOM = new Pose2d(11.0, 1.5, Rotation2d.fromDegrees(-135)); + private static final Pose2d RIGHT_MID_RETURN = new Pose2d(RIGHT_ZONE_MIN_X, 3.5, Rotation2d.fromDegrees(135)); + + // Return crossing - back north of reef + private static final Pose2d CROSSING_RETURN = new Pose2d(REEF_CENTER_X, SAFE_CROSSING_Y, Rotation2d.fromDegrees(180)); + + public AutoFigure8(DriveSubsystem drive) { + addCommands( + // Print start message + Commands.runOnce(() -> { + System.out.println("\n========== FIGURE 8 NAV - FIRST REBUILT 2025 =========="); + System.out.println("Strategy: Advanced field navigation practice"); + System.out.println("Objective: Navigate figure 8 avoiding reef obstacle"); + System.out.println("Left Loop: Y=1.5-7.4m, X=1.5-5.5m (stays west of reef)"); + System.out.println("Right Loop: Y=1.5-7.4m, X=10.5-13.5m (stays east of reef)"); + System.out.println("Crossing: Y=6.9m (2.87m clearance above reef center)"); + System.out.println("Expected Points: 0 (navigation practice)"); + System.out.println("Field: 16.54m × 8.07m (Rebuilt 2025)"); + System.out.println("=====================================================\n"); + }), + + // Start from Blue Alliance position + Commands.runOnce(() -> { + drive.stop(); + System.out.println("[Figure8] Starting from Blue Alliance wall"); + System.out.println("[Figure8] Phase 1: Moving NORTH to safe crossing height..."); + }), + Commands.waitSeconds(0.5), + + // FIRST: Move NORTH along wall (no forward movement toward reef!) + new AutoDriveCommand(drive, SAFE_NORTH, 0.25, 2.5, 4.0), + Commands.waitSeconds(0.3), + + // === LEFT LOOP (Counter-clockwise) === + Commands.runOnce(() -> System.out.println("[Figure8] Phase 2: LEFT LOOP (counter-clockwise)")), + + new AutoDriveCommand(drive, LEFT_TOP, 0.25, 3.0, 4.0), + Commands.waitSeconds(0.2), + + Commands.runOnce(() -> System.out.println("[Figure8] Moving east (staying north of reef)")), + new AutoDriveCommand(drive, LEFT_MID_EAST, 0.30, 3.5, 4.5), + Commands.waitSeconds(0.2), + + Commands.runOnce(() -> System.out.println("[Figure8] Moving south along left safe zone")), + new AutoDriveCommand(drive, LEFT_BOTTOM, 0.35, 4.0, 5.0), + Commands.waitSeconds(0.2), + + Commands.runOnce(() -> System.out.println("[Figure8] Completing left loop - returning north")), + new AutoDriveCommand(drive, LEFT_MID_RETURN, 0.30, 3.5, 4.5), + Commands.waitSeconds(0.3), + + // === CROSS TO RIGHT SIDE (Above reef) === + Commands.runOnce(() -> System.out.println("[Figure8] Phase 3: CROSSING to right side (above reef)")), + new AutoDriveCommand(drive, CROSSING_NORTH, 0.35, 4.5, 5.0), + Commands.waitSeconds(0.4), + + // === RIGHT LOOP (Clockwise) === + Commands.runOnce(() -> System.out.println("[Figure8] Phase 4: RIGHT LOOP (clockwise)")), + + new AutoDriveCommand(drive, RIGHT_TOP, 0.30, 4.0, 4.5), + Commands.waitSeconds(0.2), + + Commands.runOnce(() -> System.out.println("[Figure8] Moving south along right safe zone")), + new AutoDriveCommand(drive, RIGHT_EAST, 0.35, 4.5, 5.0), + Commands.waitSeconds(0.2), + + new AutoDriveCommand(drive, RIGHT_BOTTOM, 0.35, 4.5, 5.0), + Commands.waitSeconds(0.2), + + Commands.runOnce(() -> System.out.println("[Figure8] Completing right loop - returning north")), + new AutoDriveCommand(drive, RIGHT_MID_RETURN, 0.30, 4.0, 4.5), + Commands.waitSeconds(0.3), + + // === CROSS BACK (Above reef) === + Commands.runOnce(() -> System.out.println("[Figure8] Phase 5: Crossing back (above reef)")), + new AutoDriveCommand(drive, CROSSING_RETURN, 0.35, 4.5, 5.0), + Commands.waitSeconds(0.3), + + // Return to safe zone + Commands.runOnce(() -> System.out.println("[Figure8] Phase 6: Returning to Blue safe zone")), + new AutoDriveCommand(drive, SAFE_NORTH, 0.25, 3.5, 4.0), + Commands.waitSeconds(0.2), + new AutoDriveCommand(drive, FieldConstants.BLUE_SAFE_ZONE, 0.20, 3.0, 4.0), + + // Finish + Commands.runOnce(() -> { + drive.stop(); + drive.lock(); + Pose2d finalPose = drive.getPose(); + System.out.println("\n[Figure8] [OK] Complete!"); + System.out.println("[Figure8] Figure 8 pattern completed with ZERO collisions"); + System.out.println("[Figure8] Final position: (" + + String.format("%.2f, %.2f @ %.0f deg", finalPose.getX(), finalPose.getY(), finalPose.getRotation().getDegrees()) + ")"); + System.out.println("[Figure8] Total distance: ~35 meters\n"); + }), + + // Hold position until autonomous ends - prevents default command from taking over + Commands.run(() -> { + drive.stop(); + }, drive).withName("HoldPosition") + ); + } +} + + diff --git a/src/main/java/frc/robot/commands/auto/AutoPathes.java b/src/main/java/frc/robot/commands/auto/AutoPathes.java new file mode 100644 index 0000000..875610a --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoPathes.java @@ -0,0 +1,110 @@ +package frc.robot.commands.auto; + +import java.util.List; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.Waypoint; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Autonomous command that follows a PathPlanner path. + * Tries to load 'ExamplePath' from deploy/pathplanner/paths/. + * Falls back to an on-the-fly path if the file doesn't exist. + */ +public class AutoPathes extends SequentialCommandGroup { + public AutoPathes(DriveSubsystem drive) { + Command followCommand = buildFollowCommand(drive); + + addCommands( + Commands.runOnce(() -> { + System.out.println("\n========== AUTO PATHES =========="); + System.out.println("Following PathPlanner path"); + System.out.println("=================================\n"); + }), + + // Small settle delay + Commands.waitSeconds(0.1), + + // Follow the path + followCommand, + + // Stop when done + Commands.runOnce(() -> { + drive.stop(); + drive.lock(); + System.out.println("[AutoPathes] Path complete. Robot stopped and locked."); + }) + ); + } + + /** + * Build the follow command. Tries to load from file first, falls back to on-the-fly path. + */ + private Command buildFollowCommand(DriveSubsystem drive) { + // Try loading from file first + try { + PathPlannerPath path = PathPlannerPath.fromPathFile("ExamplePath"); + System.out.println("[AutoPathes] Loaded 'ExamplePath' from file."); + + // Reset pose to path start before following + return Commands.sequence( + Commands.runOnce(() -> { + try { + path.getStartingHolonomicPose().ifPresent(pose -> { + drive.resetOdometry(pose); + System.out.println("[AutoPathes] Reset pose to path start: " + pose); + }); + } catch (Exception e) { + System.err.println("[AutoPathes] Could not reset pose: " + e.getMessage()); + } + }), + Commands.waitSeconds(0.1), + AutoBuilder.followPath(path) + ); + } catch (Exception e) { + System.out.println("[AutoPathes] 'ExamplePath' not found, using on-the-fly path."); + } + + // Fallback: create an on-the-fly path from current position forward + try { + Pose2d start = new Pose2d(1.0, 1.0, Rotation2d.kZero); + Pose2d end = new Pose2d(3.0, 3.0, Rotation2d.kZero); + + List waypoints = PathPlannerPath.waypointsFromPoses(start, end); + PathConstraints constraints = new PathConstraints( + 2.0, 2.0, // max velocity (m/s), max acceleration (m/s²) + Math.PI, Math.PI // max angular velocity, max angular acceleration + ); + PathPlannerPath onTheFlyPath = new PathPlannerPath( + waypoints, + constraints, + null, // ideal starting state (null = use current) + new GoalEndState(0.0, end.getRotation()) // stop at end + ); + + System.out.println("[AutoPathes] Created on-the-fly path: (1,1) -> (3,3)"); + + return Commands.sequence( + Commands.runOnce(() -> { + drive.resetOdometry(start); + System.out.println("[AutoPathes] Reset pose to on-the-fly start: " + start); + }), + Commands.waitSeconds(0.1), + AutoBuilder.followPath(onTheFlyPath) + ); + } catch (Exception e) { + System.err.println("[AutoPathes] ERROR: Could not create on-the-fly path: " + e.getMessage()); + e.printStackTrace(); + return Commands.none(); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/AutoPickupAuto.java b/src/main/java/frc/robot/commands/auto/AutoPickupAuto.java new file mode 100644 index 0000000..18bddc7 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoPickupAuto.java @@ -0,0 +1,80 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Pickup Cycles Auto - FIRST Rebuilt 2025 Strategy + * + * Game: FIRST Reefscape Rebuilt + * Objective: Practice rapid coral pickup and scoring cycles + * + * Strategy (3-cycle auto): + * 1. Score preloaded coral on Reef Front (3 pts) + * 2. Cycle 1: Pickup from Coral Station → Score on Reef Left (3 pts) + * 3. Cycle 2: Pickup from Coral Station → Score on Reef Right (3 pts) + * 4. Return to safe zone + * + * Points: 9 points (3 corals × 3 points each) + * Time: ~22-25 seconds + * Focus: Speed and efficiency in coral handling + */ +public class AutoPickupAuto extends SequentialCommandGroup { + + public AutoPickupAuto(DriveSubsystem drive) { + addCommands( + Commands.runOnce(() -> { + System.out.println("\n========== PICKUP CYCLES - FIRST REBUILT 2025 =========="); + System.out.println("Strategy: Rapid coral pickup and scoring cycles"); + System.out.println("Target: 3 corals (preload + 2 pickups)"); + System.out.println("Expected Points: 9 (3 × 3 pts)"); + System.out.println("======================================================\n"); + }), + + Commands.runOnce(() -> drive.stop()), + Commands.waitSeconds(0.3), + + // Cycle 1: Preload to Reef Front + Commands.runOnce(() -> System.out.println("[PickupCycles] Cycle 1: Scoring preload")), + new AutoDriveCommand(drive, FieldConstants.REEF_FRONT, 2.5, 6.0), + Commands.waitSeconds(0.8), + + // Pickup from Coral Station + Commands.runOnce(() -> System.out.println("[PickupCycles] Cycle 1: Picking up coral")), + new AutoDriveCommand(drive, FieldConstants.BLUE_CORAL_STATION, 2.5, 6.0), + Commands.waitSeconds(0.8), + + // Cycle 2: Score on Reef Left + Commands.runOnce(() -> System.out.println("[PickupCycles] Cycle 2: Scoring on Reef Left")), + new AutoDriveCommand(drive, FieldConstants.REEF_LEFT, 2.5, 6.0), + Commands.waitSeconds(0.8), + + // Pickup from Coral Station + Commands.runOnce(() -> System.out.println("[PickupCycles] Cycle 2: Picking up coral")), + new AutoDriveCommand(drive, FieldConstants.BLUE_CORAL_STATION, 2.5, 6.0), + Commands.waitSeconds(0.8), + + // Cycle 3: Score on Reef Right + Commands.runOnce(() -> System.out.println("[PickupCycles] Cycle 3: Scoring on Reef Right")), + new AutoDriveCommand(drive, FieldConstants.REEF_RIGHT, 2.5, 6.0), + Commands.waitSeconds(0.8), + + // Return home + Commands.runOnce(() -> System.out.println("[PickupCycles] Returning to safe zone")), + new AutoDriveCommand(drive, FieldConstants.BLUE_SAFE_ZONE, 2.0, 6.0), + + Commands.runOnce(() -> { + drive.stop(); + drive.lock(); + System.out.println("[PickupCycles] [OK] Complete - 3 pieces scored\n"); + }), + + // Hold position until autonomous ends - prevents default command from taking over + Commands.run(() -> { + drive.stop(); + }, drive).withName("HoldPosition") + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/AutoProcessorAuto.java b/src/main/java/frc/robot/commands/auto/AutoProcessorAuto.java new file mode 100644 index 0000000..c963bc5 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoProcessorAuto.java @@ -0,0 +1,53 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Processor Scoring Auto - FIRST Rebuilt 2025 Strategy + * + * Game: FIRST Reefscape Rebuilt + * Objective: Score coral in processor station + * + * Strategy: + * 1. Start with preloaded coral + * 2. Navigate to Processor station + * 3. Score coral in processor (4 points - higher value!) + * 4. Return to safe zone + * + * Points: 4 points (processor scoring bonus) + * Time: ~15 seconds + * Note: Processor scores are worth more than reef (4 pts vs 3 pts) + */ +public class AutoProcessorAuto extends SequentialCommandGroup { + + public AutoProcessorAuto(DriveSubsystem drive) { + addCommands( + Commands.runOnce(() -> { + System.out.println("\n========== PROCESSOR AUTO - FIRST REBUILT 2025 =========="); + System.out.println("Strategy: Score coral in processor station"); + System.out.println("Expected Points: 4 (processor bonus)"); + System.out.println("=======================================================\n"); + }), + Commands.runOnce(() -> drive.stop()), + Commands.waitSeconds(0.3), + Commands.runOnce(() -> System.out.println("[Processor] Driving to processor")), + new AutoDriveCommand(drive, FieldConstants.PROCESSOR, 2.5, 8.0), + Commands.waitSeconds(1.5), + Commands.runOnce(() -> System.out.println("[Processor] Returning")), + new AutoDriveCommand(drive, FieldConstants.BLUE_SAFE_ZONE, 2.5, 8.0), + Commands.runOnce(() -> { + drive.stop(); + drive.lock(); + System.out.println("[Processor] Complete\n"); + }), + + // Hold position until autonomous ends - prevents default command from taking over + Commands.run(() -> { + drive.stop(); + }, drive).withName("HoldPosition") + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/AutoSafeScore.java b/src/main/java/frc/robot/commands/auto/AutoSafeScore.java new file mode 100644 index 0000000..a1c61f9 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoSafeScore.java @@ -0,0 +1,53 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Safe Score Auto - FIRST Rebuilt 2025 Strategy + * + * Game: FIRST Reefscape Rebuilt + * Objective: Conservative, reliable single coral scoring + * + * Strategy: + * 1. Start with preloaded coral + * 2. Drive to Reef Front (nearest, safest approach) + * 3. Score coral on Level 1 (3 points) + * 4. Return to safe zone immediately + * + * Points: 3 points (guaranteed) + * Time: ~12-15 seconds + * Best for: Reliability, untested robots, practice matches + */ +public class AutoSafeScore extends SequentialCommandGroup { + + public AutoSafeScore(DriveSubsystem drive) { + addCommands( + Commands.runOnce(() -> { + System.out.println("\n========== SAFE SCORE - FIRST REBUILT 2025 =========="); + System.out.println("Strategy: Conservative single coral scoring"); + System.out.println("Expected Points: 3 (1 coral, guaranteed)"); + System.out.println("===================================================\n"); + }), + Commands.runOnce(() -> drive.stop()), + Commands.waitSeconds(0.3), + Commands.runOnce(() -> System.out.println("[SafeScore] Driving to Reef Front")), + new AutoDriveCommand(drive, FieldConstants.REEF_FRONT, 2.0, 8.0), + Commands.waitSeconds(1.5), + Commands.runOnce(() -> System.out.println("[SafeScore] Returning to safe zone")), + new AutoDriveCommand(drive, FieldConstants.BLUE_SAFE_ZONE, 2.0, 8.0), + Commands.runOnce(() -> { + drive.stop(); + drive.lock(); + System.out.println("[SafeScore] Complete\n"); + }), + + // Hold position until autonomous ends - prevents default command from taking over + Commands.run(() -> { + drive.stop(); + }, drive).withName("HoldPosition") + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/AutoTripleScore.java b/src/main/java/frc/robot/commands/auto/AutoTripleScore.java new file mode 100644 index 0000000..c3745df --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoTripleScore.java @@ -0,0 +1,60 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Triple Score Auto - FIRST Rebuilt 2025 Strategy + * + * Game: FIRST Reefscape Rebuilt + * Objective: Score on all three reef branches (no pickup cycles) + * + * Strategy: + * Assumes robot has 3 preloaded corals (or uses ground corals) + * 1. Score on Reef Front branch (0°) + * 2. Score on Reef Left branch (120°) + * 3. Score on Reef Right branch (60°) + * 4. Return to safe zone + * + * Points: 9 points (3 corals × 3 points each) + * Time: ~20 seconds + * Note: Requires multi-coral storage or ground pickup capability + */ +public class AutoTripleScore extends SequentialCommandGroup { + + public AutoTripleScore(DriveSubsystem drive) { + addCommands( + Commands.runOnce(() -> { + System.out.println("\n========== TRIPLE SCORE - FIRST REBUILT 2025 =========="); + System.out.println("Strategy: Score on all 3 reef branches"); + System.out.println("Expected Points: 9 (3 corals × 3 pts)"); + System.out.println("======================================================\n"); + }), + Commands.runOnce(() -> drive.stop()), + Commands.waitSeconds(0.3), + Commands.runOnce(() -> System.out.println("[TripleScore] Score 1: Reef Front")), + new AutoDriveCommand(drive, FieldConstants.REEF_FRONT, 2.5, 8.0), + Commands.waitSeconds(1.5), + Commands.runOnce(() -> System.out.println("[TripleScore] Score 2: Reef Left")), + new AutoDriveCommand(drive, FieldConstants.REEF_LEFT, 2.5, 8.0), + Commands.waitSeconds(1.5), + Commands.runOnce(() -> System.out.println("[TripleScore] Score 3: Reef Right")), + new AutoDriveCommand(drive, FieldConstants.REEF_RIGHT, 2.5, 8.0), + Commands.waitSeconds(1.5), + Commands.runOnce(() -> System.out.println("[TripleScore] Returning to safe zone")), + new AutoDriveCommand(drive, FieldConstants.BLUE_SAFE_ZONE, 2.5, 10.0), + Commands.runOnce(() -> { + drive.stop(); + drive.lock(); + System.out.println("[TripleScore] Complete - 3 pieces scored\n"); + }), + + // Hold position until autonomous ends - prevents default command from taking over + Commands.run(() -> { + drive.stop(); + }, drive).withName("HoldPosition") + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/SequentialAuto.java b/src/main/java/frc/robot/commands/auto/SequentialAuto.java new file mode 100644 index 0000000..fc8733b --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/SequentialAuto.java @@ -0,0 +1,100 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Sequential Auto - FIRST Rebuilt 2025 Strategy + * + * Game: FIRST Reefscape Rebuilt + * Objective: Multi-waypoint coral scoring demonstration + * + * Strategy: + * 1. Start at Blue Alliance + * 2. Navigate to Coral Station → Pickup coral + * 3. Navigate to Reef Left branch → Score coral (3 pts) + * 4. Return to safe zone for endgame + * + * Points: 3 points (1 coral from pickup) + * Time: ~20 seconds + * Purpose: Demonstrates path planning and waypoint navigation + */ +public class SequentialAuto extends SequentialCommandGroup { + + public SequentialAuto(DriveSubsystem drive) { + addCommands( + // Startup diagnostics + Commands.runOnce(() -> { + System.out.println("\n========== SEQUENTIAL AUTO - FIRST REBUILT 2025 =========="); + System.out.println("Strategy: Multi-waypoint coral scoring"); + Pose2d startPose = drive.getPose(); + System.out.println("Starting position: (" + + String.format("%.2f, %.2f", startPose.getX(), startPose.getY()) + ")"); + System.out.println("Waypoints:"); + System.out.println(" 1. Coral Station: (1.2, 7.0) - Pickup"); + System.out.println(" 2. Reef Left: (7.2, 5.0) - Score (3 pts)"); + System.out.println(" 3. Safe Zone: (2.0, 1.5) - Return"); + System.out.println("Expected Points: 3"); + System.out.println("Field: 16.54m × 8.07m (Rebuilt 2025)"); + System.out.println("========================================================\n"); + }), + + // Initialize: Verify starting position + Commands.runOnce(() -> { + System.out.println("[SequentialAuto] Verifying start position..."); + drive.stop(); + }), + Commands.waitSeconds(0.5), + + // Waypoint 1: Drive to Coral Station + Commands.runOnce(() -> { + System.out.println("[SequentialAuto] Waypoint 1: Driving to Coral Station (1.2, 7.0)"); + }), + new AutoDriveCommand( + drive, + FieldConstants.BLUE_CORAL_STATION, + 2.0, 8.0 + ), + Commands.waitSeconds(1.0), // Pickup time + + // Waypoint 2: Drive to Reef Left branch + Commands.runOnce(() -> { + System.out.println("[SequentialAuto] Waypoint 2: Driving to Reef Left (7.57, 5.105)"); + }), + new AutoDriveCommand( + drive, + FieldConstants.REEF_LEFT, + 2.5, 8.0 + ), + Commands.waitSeconds(1.5), // Score time + + // Waypoint 3: Return to safe zone + Commands.runOnce(() -> { + System.out.println("[SequentialAuto] Waypoint 3: Returning to safe zone"); + }), + new AutoDriveCommand( + drive, + FieldConstants.BLUE_SAFE_ZONE, + 2.5, 8.0 + ), + + // Finish + Commands.runOnce(() -> { + Pose2d finalPose = drive.getPose(); + System.out.println("[SequentialAuto] Complete at (" + + String.format("%.2f, %.2f", finalPose.getX(), finalPose.getY()) + ")"); + drive.stop(); + drive.lock(); + System.out.println(); + }), + + // Hold position until autonomous ends - prevents default command from taking over + Commands.run(() -> { + drive.stop(); + }, drive).withName("HoldPosition") + ); + } +} diff --git a/src/main/java/frc/robot/commands/auto/SimpleAuto.java b/src/main/java/frc/robot/commands/auto/SimpleAuto.java new file mode 100644 index 0000000..8da67f3 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/SimpleAuto.java @@ -0,0 +1,94 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.FieldConstants; +import frc.robot.subsystems.DriveSubsystem; + +/** + * Simple Auto - FIRST Rebuilt 2025 Strategy + * + * Game: FIRST Reefscape Rebuilt + * Objective: Score preloaded coral on reef center structure + * + * Strategy: + * 1. Start at Blue Alliance starting zone with preloaded coral + * 2. Drive to Reef Front scoring position (nearest branch) + * 3. Score coral on reef (Level 1 - 3 points) + * 4. Return to safe zone for endgame positioning + * + * Points: 3 points (1 coral on reef) + * Time: ~12 seconds + * Reliability: High - simple path, minimal risk + */ +public class SimpleAuto extends SequentialCommandGroup { + + public SimpleAuto(DriveSubsystem drive) { + addCommands( + // Startup diagnostics + Commands.runOnce(() -> { + System.out.println("\n========== SIMPLE AUTO - FIRST REBUILT 2025 =========="); + System.out.println("Strategy: Score preloaded coral on reef"); + Pose2d startPose = drive.getPose(); + System.out.println("Starting position: (" + + String.format("%.2f, %.2f", startPose.getX(), startPose.getY()) + ")"); + System.out.println("Target: Reef Front (8.27, 2.5)"); + System.out.println("Expected Points: 3 (Level 1 coral)"); + System.out.println("Field: 16.54m × 8.07m (Rebuilt 2025)"); + System.out.println("====================================================\n"); + }), + + // Step 1: Confirm starting position and stabilize + Commands.runOnce(() -> { + System.out.println("[SimpleAuto] Step 1: Confirming at valid field position"); + drive.stop(); + }), + Commands.waitSeconds(0.5), + + // Step 2: Drive to Reef Front approach + Commands.runOnce(() -> { + System.out.println("[SimpleAuto] Step 2: Driving to Reef front (8.77, 2.6)"); + }), + new AutoDriveCommand( + drive, + FieldConstants.REEF_FRONT, // Approaches from front + 2.0, // max speed (m/s) + 8.0 // timeout + ), + + // Step 3: Simulate scoring + Commands.runOnce(() -> { + System.out.println("[SimpleAuto] Step 3: Attempting to score..."); + drive.stop(); + }), + Commands.waitSeconds(1.5), // Time for intake/mechanism + + // Step 4: Return to safe zone + Commands.runOnce(() -> { + System.out.println("[SimpleAuto] Step 4: Returning to safe zone"); + }), + new AutoDriveCommand( + drive, + FieldConstants.BLUE_SAFE_ZONE, + 2.5, + 8.0 + ), + + // Finish + Commands.runOnce(() -> { + Pose2d finalPose = drive.getPose(); + System.out.println("[SimpleAuto] Complete at (" + + String.format("%.2f, %.2f", finalPose.getX(), finalPose.getY()) + ")"); + drive.stop(); + drive.lock(); + System.out.println(); + }), + + // Hold position until autonomous ends - prevents default command from taking over + Commands.run(() -> { + drive.stop(); + }, drive).withName("HoldPosition") + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/2026ChassisBot.code-workspace b/src/main/java/frc/robot/subsystems/2026ChassisBot.code-workspace new file mode 100644 index 0000000..856a5af --- /dev/null +++ b/src/main/java/frc/robot/subsystems/2026ChassisBot.code-workspace @@ -0,0 +1,7 @@ +{ + "folders": [ + { + "path": "../../../../../.." + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java new file mode 100644 index 0000000..a2459c0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -0,0 +1,686 @@ +package frc.robot.subsystems; + +import java.io.File; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.FieldConstants; +import swervelib.SwerveDrive; +import swervelib.parser.SwerveParser; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +@SuppressWarnings("unused") + + +public class DriveSubsystem extends SubsystemBase { + private SwerveDrive swerveDrive; + private final Field2d field2d; + private boolean initialized = false; + private boolean hasZeroed = false; + private long lastErrorTime = 0; + private static final long ERROR_THROTTLE_MS = 500; + + private double currentXSpeed = 0.0; + private double currentYSpeed = 0.0; + private double currentRotSpeed = 0.0; + + private double startupTime = 0; + private static final double STARTUP_DELAY_SECONDS = 0.5; + private static final double DEADZONE_THRESHOLD = 0.05; + private static final double ROTATION_DEADZONE = 0.05; + private static final double MAX_SPEED_LIMIT = 5.0; + + // Adjustable speed multiplier (changed via bumpers) + private double speedMultiplier = DriveConstants.DEFAULT_SPEED_MULTIPLIER; + + public DriveSubsystem(File directory) { + field2d = new Field2d(); + SmartDashboard.putData("Field", field2d); + + currentXSpeed = 0.0; + currentYSpeed = 0.0; + currentRotSpeed = 0.0; + + initializeSwerve(); + + startupTime = Timer.getFPGATimestamp(); + + stop(); + } + + private void initializeSwerve() { + try { + File swerveJsonDirectory = new File(Filesystem.getDeployDirectory(), "SWERVE"); + + if (!swerveJsonDirectory.exists()) { + logError("SWERVE directory not found: " + swerveJsonDirectory.getAbsolutePath()); + return; + } + + File[] files = swerveJsonDirectory.listFiles(); + if (files != null) { + logDebug("Files in SWERVE directory:"); + for (File f : files) { + logDebug(" - " + f.getName()); + } + } + + logDebug("Creating SwerveDrive with max speed: " + DriveConstants.MAX_SPEED_MPS + " m/s"); + logDebug("Running in " + (RobotBase.isSimulation() ? "SIMULATION" : "REAL") + " mode"); + + swerveDrive = new SwerveParser(swerveJsonDirectory).createSwerveDrive(DriveConstants.MAX_SPEED_MPS); + + // Match YAGSL official example configuration: + // 1. Heading correction should only be used while controlling via angle setpoint + swerveDrive.setHeadingCorrection(false); + // 2. Disable cosine compensation — causes discrepancies in simulation + swerveDrive.setCosineCompensator(false); + // 3. Correct for skew that gets worse as angular velocity increases + swerveDrive.setAngularVelocityCompensation(true, true, 0.1); + // 4. Disable auto-synchronize to prevent jitter + swerveDrive.setModuleEncoderAutoSynchronize(false, 1); + + verifyModules(); + initialized = true; + + try { + if (swerveDrive != null) { + var initialHeading = swerveDrive.getOdometryHeading(); + logDebug("Initial gyro heading: " + initialHeading.getDegrees() + "°"); + SmartDashboard.putNumber("Drive/Initial Heading", initialHeading.getDegrees()); + } + } catch (Exception e) { + logDebug("Could not read initial heading: " + e.getMessage()); + } + + zeroGyro(); + + SmartDashboard.putBoolean("Drive/Initialized", true); + logDebug("Swerve drive initialized successfully"); + + // Configure PathPlanner after successful swerve init + configurePathPlanner(); + + } catch (Exception e) { + logError("FATAL ERROR during swerve initialization: " + e.getMessage()); + e.printStackTrace(); + SmartDashboard.putString("Drive/Error", "Init failed: " + e.getMessage()); + SmartDashboard.putBoolean("Drive/Initialized", false); + } + + SmartDashboard.putNumber("Drive/Max Speed (m/s)", DriveConstants.MAX_SPEED_MPS); + SmartDashboard.putNumber("Drive/Max Angular Velocity (rad/s)", DriveConstants.MAX_ANGULAR_VELOCITY); + } + + /** + * Configure PathPlanner's AutoBuilder for autonomous path following. + * Must be called after YAGSL swerve drive is initialized. + */ + private void configurePathPlanner() { + try { + RobotConfig config = RobotConfig.fromGUISettings(); + + AutoBuilder.configure( + this::getPose, // Pose supplier + this::resetOdometry, // Pose reset consumer + this::getRobotRelativeSpeeds, // ChassisSpeeds supplier (robot-relative) + (speeds, feedforwards) -> driveRobotRelative(speeds), // Drive consumer (robot-relative!) + new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), // Translation PID + new PIDConstants(5.0, 0.0, 0.0) // Rotation PID + ), + config, + () -> { + // Flip path for red alliance + var alliance = DriverStation.getAlliance(); + return alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red; + }, + this // Subsystem requirement + ); + logDebug("PathPlanner AutoBuilder configured successfully"); + } catch (Exception e) { + DriverStation.reportError("PathPlanner config failed: " + e.getMessage(), e.getStackTrace()); + } + } + + /** + * Get the robot-relative chassis speeds (used by PathPlanner). + */ + public ChassisSpeeds getRobotRelativeSpeeds() { + if (isInitialized()) { + return swerveDrive.getRobotVelocity(); + } + return new ChassisSpeeds(); + } + + private void verifyModules() { + var modules = swerveDrive.getModules(); + logDebug("Loaded " + modules.length + " swerve modules"); + SmartDashboard.putNumber("Drive/Module Count", modules.length); + + int validModules = 0; + for (int i = 0; i < modules.length; i++) { + boolean isValid = modules[i] != null; + if (isValid) validModules++; + logDebug("Module " + i + ": " + (isValid ? "OK" : "NULL")); + SmartDashboard.putBoolean("Drive/Module " + i + " OK", isValid); + } + + if (validModules != 4) { + logError("Only " + validModules + "/4 modules loaded successfully"); + } + } + + private void logError(String message) { + long now = System.currentTimeMillis(); + if (now - lastErrorTime > ERROR_THROTTLE_MS) { + System.out.println("[DriveSubsystem ERROR] " + message); + SmartDashboard.putString("Drive/Error", message); + DriverStation.reportError(message, false); + lastErrorTime = now; + } + } + + private void logDebug(String message) { + System.out.println("[DriveSubsystem] " + message); + } + + public void zeroGyro() { + if (!isInitialized()) return; + + try { + swerveDrive.zeroGyro(); + SmartDashboard.putBoolean("Drive/Gyro Zeroed", true); + logDebug("Gyro zeroed"); + } catch (Exception e) { + logError("zeroGyro error: " + e.getMessage()); + } + } + + public void synchronizeModuleEncoders() { + if (!isInitialized()) return; + + try { + swerveDrive.synchronizeModuleEncoders(); + hasZeroed = true; + SmartDashboard.putBoolean("Drive/Modules Synced", true); + logDebug("Module encoders synchronized"); + } catch (Exception e) { + DriverStation.reportWarning("synchronizeModuleEncoders error: " + e.getMessage(), true); + } + } + + public boolean isInitialized() { + return initialized && swerveDrive != null; + } + + private boolean isReadyToDrive() { + return (Timer.getFPGATimestamp() - startupTime) >= STARTUP_DELAY_SECONDS; + } + + private double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } + + private double applyDeadzone(double value) { + double absValue = Math.abs(value); + if (absValue < DEADZONE_THRESHOLD) { + return 0.0; + } + double sign = Math.signum(value); + return sign * ((absValue - DEADZONE_THRESHOLD) / (1.0 - DEADZONE_THRESHOLD)); + } + + private double applyRotationDeadzone(double value) { + double absValue = Math.abs(value); + if (absValue < ROTATION_DEADZONE) { + return 0.0; + } + double sign = Math.signum(value); + return sign * ((absValue - ROTATION_DEADZONE) / (1.0 - ROTATION_DEADZONE)); + } + + /** + * Clamp a velocity component so the robot cannot drive outside the field. + * When the robot is near or beyond a boundary, velocity pushing further out is zeroed. + * The margin constant controls how close to the wall the brake engages. + */ + private double clampFieldVelocity(double velocity, double position, double minBound, double maxBound) { + final double MARGIN = FieldConstants.FIELD_MARGIN; // 0.5 m from wall + if (position <= minBound + MARGIN && velocity < 0) return 0.0; + if (position >= maxBound - MARGIN && velocity > 0) return 0.0; + return velocity; + } + + private ChassisSpeeds limitSpeeds(ChassisSpeeds speeds) { + double linearMagnitude = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + double angularMagnitude = Math.abs(speeds.omegaRadiansPerSecond); + + if (linearMagnitude == 0 && angularMagnitude == 0) { + return speeds; + } + + double linearLimit = Math.min(1.0, MAX_SPEED_LIMIT / Math.max(linearMagnitude, 0.01)); + double angularLimit = Math.min(1.0, DriveConstants.MAX_ANGULAR_VELOCITY / Math.max(angularMagnitude, 0.01)); + + double limitFactor = Math.min(linearLimit, angularLimit); + + return new ChassisSpeeds( + speeds.vxMetersPerSecond * limitFactor, + speeds.vyMetersPerSecond * limitFactor, + speeds.omegaRadiansPerSecond * limitFactor + ); + } + + public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, + DoubleSupplier rotationX) { + return run(() -> { + if (!isInitialized()) { + return; + } + + try { + double xInput = translationX.getAsDouble(); + double yInput = translationY.getAsDouble(); + double rotInput = rotationX.getAsDouble(); + + // Apply deadzone + xInput = applyDeadzone(xInput); + yInput = applyDeadzone(yInput); + rotInput = applyRotationDeadzone(rotInput); + + // Scale to velocity (m/s and rad/s), applying speed multiplier + currentXSpeed = xInput * swerveDrive.getMaximumChassisVelocity() * speedMultiplier; + currentYSpeed = yInput * swerveDrive.getMaximumChassisVelocity() * speedMultiplier; + currentRotSpeed = Math.pow(rotInput, 3) * swerveDrive.getMaximumChassisAngularVelocity() * speedMultiplier; + + // Field boundary protection — prevent driving off the field + // Velocities here are field-relative (X = downfield, Y = cross-field) + Pose2d pose = swerveDrive.getPose(); + currentXSpeed = clampFieldVelocity(currentXSpeed, pose.getX(), 0, FieldConstants.FIELD_LENGTH); + currentYSpeed = clampFieldVelocity(currentYSpeed, pose.getY(), 0, FieldConstants.FIELD_WIDTH); + + // Drive using YAGSL — field-relative, closed-loop + swerveDrive.drive( + new Translation2d(currentXSpeed, currentYSpeed), + currentRotSpeed, + true, + false); + + } catch (Exception e) { + logError("Drive command error: " + e.getMessage()); + stop(); + } + }).withName("DriveCommand"); + } + + // Removed deprecated driveFieldOrientedSafe - all drive paths now use YAGSL consistently + + private long diagPrintCounter = 0; + + /** + * Diagnostic command: drives the robot at a fixed 1 m/s forward using robot-relative ChassisSpeeds. + * Logs detailed module state info to identify spinning root cause. + */ + public Command testDriveForwardCommand() { + return run(() -> { + if (isInitialized()) { + ChassisSpeeds desired = new ChassisSpeeds(1.0, 0.0, 0.0); + + // Drive robot-relative, closed-loop + swerveDrive.drive(desired); + + // Log what kinematics WANTS vs what modules ACTUALLY report + var desiredStates = swerveDrive.kinematics.toSwerveModuleStates(desired); + var modules = swerveDrive.getModules(); + String[] names = {"FL", "FR", "BL", "BR"}; + boolean shouldPrint = (diagPrintCounter++ % 50 == 0); // Print every 1 second at 50Hz + + if (shouldPrint) { + System.out.println("=== DIAG: ChassisSpeeds(1,0,0) robot-relative ==="); + System.out.printf(" Heading: %.2f deg%n", swerveDrive.getOdometryHeading().getDegrees()); + } + + for (int i = 0; i < modules.length; i++) { + var actual = modules[i].getState(); + SmartDashboard.putNumber("Diag/" + names[i] + "/DesiredAngle", desiredStates[i].angle.getDegrees()); + SmartDashboard.putNumber("Diag/" + names[i] + "/DesiredSpeed", desiredStates[i].speedMetersPerSecond); + SmartDashboard.putNumber("Diag/" + names[i] + "/ActualAngle", actual.angle.getDegrees()); + SmartDashboard.putNumber("Diag/" + names[i] + "/ActualSpeed", actual.speedMetersPerSecond); + SmartDashboard.putNumber("Diag/" + names[i] + "/AngleError", + desiredStates[i].angle.getDegrees() - actual.angle.getDegrees()); + + // Also log maple-sim direct state if available (via YAGSL sim wrapper) + var simMod = modules[i].getSimModule(); + if (simMod != null) { + var simState = simMod.getState(); + SmartDashboard.putNumber("Diag/" + names[i] + "/SimAngle", simState.angle.getDegrees()); + SmartDashboard.putNumber("Diag/" + names[i] + "/SimSpeed", simState.speedMetersPerSecond); + } + + if (shouldPrint) { + System.out.printf(" %s: desired(%.1f°, %.2f m/s) actual(%.1f°, %.2f m/s) err=%.1f°%n", + names[i], + desiredStates[i].angle.getDegrees(), desiredStates[i].speedMetersPerSecond, + actual.angle.getDegrees(), actual.speedMetersPerSecond, + desiredStates[i].angle.getDegrees() - actual.angle.getDegrees()); + } + } + var velocity = swerveDrive.getRobotVelocity(); + SmartDashboard.putNumber("Diag/ActualVx", velocity.vxMetersPerSecond); + SmartDashboard.putNumber("Diag/ActualVy", velocity.vyMetersPerSecond); + SmartDashboard.putNumber("Diag/ActualOmega", velocity.omegaRadiansPerSecond); + SmartDashboard.putNumber("Diag/Heading", swerveDrive.getOdometryHeading().getDegrees()); + + if (shouldPrint) { + System.out.printf(" Velocity: vx=%.2f vy=%.2f omega=%.3f rad/s%n", + velocity.vxMetersPerSecond, velocity.vyMetersPerSecond, velocity.omegaRadiansPerSecond); + System.out.printf(" Pose: (%.2f, %.2f) @ %.1f°%n", + swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), + swerveDrive.getPose().getRotation().getDegrees()); + } + } + }).withName("TestDriveForward"); + } + + public void stop() { + currentXSpeed = 0.0; + currentYSpeed = 0.0; + currentRotSpeed = 0.0; + + // Always tell YAGSL to stop — it handles sim internally + if (isInitialized()) { + try { + swerveDrive.drive(new Translation2d(0, 0), 0.0, false, false); + } catch (Exception e) { + logError("Stop error: " + e.getMessage()); + } + } + SmartDashboard.putString("Drive/Status", "Stopped"); + } + + public void lock() { + if (isInitialized()) { + try { + swerveDrive.lockPose(); + SmartDashboard.putBoolean("Drive/Locked", true); + } catch (Exception e) { + logError("Lock error: " + e.getMessage()); + } + } + } + + public void seedForwards() { + try { + zeroGyro(); + Pose2d pose = getPose(); + resetOdometry(new Pose2d(pose.getX(), pose.getY(), Rotation2d.kZero)); + SmartDashboard.putBoolean("Drive/Heading Reset", true); + } catch (Exception e) { + logError("seedForwards error: " + e.getMessage()); + } + } + + public void driveFieldOriented(ChassisSpeeds velocity) { + if (velocity == null) { + velocity = new ChassisSpeeds(0, 0, 0); + } + + currentXSpeed = velocity.vxMetersPerSecond; + currentYSpeed = velocity.vyMetersPerSecond; + currentRotSpeed = velocity.omegaRadiansPerSecond; + + // Always use YAGSL — it handles sim internally via maple-sim + if (isInitialized()) { + try { + swerveDrive.driveFieldOriented(velocity); + } catch (Exception e) { + logError("driveFieldOriented error: " + e.getMessage()); + } + } + } + + /** + * Drive using robot-relative chassis speeds. + * Used by PathPlanner's AutoBuilder — PathPlanner outputs robot-relative speeds. + */ + public void driveRobotRelative(ChassisSpeeds velocity) { + if (velocity == null) { + velocity = new ChassisSpeeds(0, 0, 0); + } + + currentXSpeed = velocity.vxMetersPerSecond; + currentYSpeed = velocity.vyMetersPerSecond; + currentRotSpeed = velocity.omegaRadiansPerSecond; + + if (isInitialized()) { + try { + swerveDrive.drive(velocity); + } catch (Exception e) { + logError("driveRobotRelative error: " + e.getMessage()); + } + } + } + + public Pose2d getPose() { + // Always use YAGSL's pose — it tracks sim state internally + if (isInitialized()) { + try { + return swerveDrive.getPose(); + } catch (Exception e) { + logError("getPose error: " + e.getMessage()); + } + } + return new Pose2d(); + } + + public void resetOdometry(Pose2d pose) { + if (pose == null) { + pose = new Pose2d(); + } + + if (isInitialized()) { + try { + swerveDrive.resetOdometry(pose); + } catch (Exception e) { + logError("resetOdometry error: " + e.getMessage()); + } + } + + SmartDashboard.putString("Drive/Last Reset Pose", + String.format("(%.2f, %.2f) @ %.1f°", pose.getX(), pose.getY(), pose.getRotation().getDegrees())); + } + + public void addVisionMeasurement(Pose2d visionPose, double timestamp) { + if (visionPose == null || !isInitialized()) { + return; + } + + try { + swerveDrive.addVisionMeasurement(visionPose, timestamp); + } catch (Exception e) { + logError("addVisionMeasurement error: " + e.getMessage()); + } + } + + public Field2d getField2d() { + return field2d; + } + + public double getMaxSpeed() { + return DriveConstants.MAX_SPEED_MPS; + } + + /** Increase the teleop speed multiplier by one step. */ + public void increaseSpeed() { + speedMultiplier = Math.min(DriveConstants.MAX_SPEED_MULTIPLIER, + speedMultiplier + DriveConstants.SPEED_MULTIPLIER_STEP); + SmartDashboard.putNumber("Drive/Speed Multiplier", speedMultiplier); + logDebug("Speed multiplier: " + String.format("%.0f%%", speedMultiplier * 100)); + } + + /** Decrease the teleop speed multiplier by one step. */ + public void decreaseSpeed() { + speedMultiplier = Math.max(DriveConstants.MIN_SPEED_MULTIPLIER, + speedMultiplier - DriveConstants.SPEED_MULTIPLIER_STEP); + SmartDashboard.putNumber("Drive/Speed Multiplier", speedMultiplier); + logDebug("Speed multiplier: " + String.format("%.0f%%", speedMultiplier * 100)); + } + + /** Get the current speed multiplier (0.0–1.0). */ + public double getSpeedMultiplier() { + return speedMultiplier; + } + + public void printEncoderOffsets() { + if (!isInitialized()) { + System.out.println("Drive not initialized - cannot read encoders"); + return; + } + + String[] moduleNames = {"Front Left", "Front Right", "Back Left", "Back Right"}; + System.out.println("========== ENCODER OFFSETS =========="); + + try { + var modules = swerveDrive.getModules(); + for (int i = 0; i < modules.length && i < moduleNames.length; i++) { + if (modules[i] != null) { + double angle = modules[i].getAbsolutePosition(); + System.out.println(moduleNames[i] + ": " + angle + " degrees"); + SmartDashboard.putNumber("Calibration/" + moduleNames[i] + " Offset", angle); + } + } + } catch (Exception e) { + logError("Error reading encoders: " + e.getMessage()); + } + System.out.println("======================================"); + } + + public void diagnoseGyro() { + if (!isInitialized()) { + System.out.println("Drive not initialized"); + return; + } + + try { + var heading = swerveDrive.getOdometryHeading(); + System.out.println("=== GYRO DIAGNOSTICS ==="); + System.out.println("Heading: " + heading.getDegrees() + "°"); + System.out.println("If heading changes without commands, gyro has drift"); + System.out.println("========================"); + SmartDashboard.putNumber("Gyro/Heading", heading.getDegrees()); + } catch (Exception e) { + logError("Gyro diagnostic error: " + e.getMessage()); + } + } + + public void diagnoseMotorInversions() { + if (!isInitialized()) { + System.out.println("Drive not initialized"); + return; + } + + System.out.println("=== MOTOR INVERSION DIAGNOSTICS ==="); + String[] moduleNames = {"Front Left", "Front Right", "Back Left", "Back Right"}; + + try { + var modules = swerveDrive.getModules(); + for (int i = 0; i < modules.length && i < moduleNames.length; i++) { + if (modules[i] != null) { + System.out.println("\n" + moduleNames[i] + ":"); + System.out.println(" Absolute Position: " + modules[i].getAbsolutePosition() + "°"); + SmartDashboard.putNumber("Diag/" + moduleNames[i] + " Abs Pos", + modules[i].getAbsolutePosition()); + } + } + + // Check gyro heading - THIS IS OFTEN THE CAUSE OF ROTATION ISSUES + var heading = swerveDrive.getOdometryHeading(); + System.out.println("\n=== GYRO HEADING CHECK ==="); + System.out.println("Current Heading: " + heading.getDegrees() + "°"); + System.out.println("If heading is NOT 0° when robot faces forward, wheels will spin!"); + System.out.println("Press POV Up to zero gyro if needed."); + + System.out.println("\n=== EXPECTED BEHAVIOR ==="); + System.out.println("1. When driving forward (Y+), all modules should point forward"); + System.out.println("2. When driving left (X+), all modules should point left"); + System.out.println("3. When rotating CCW, modules should form an X pattern"); + System.out.println("4. If robot spins unexpectedly:"); + System.out.println(" - Check if gyro heading is 0 when facing forward"); + System.out.println(" - Check 'invertedIMU' in swervedrive.json"); + System.out.println(" - Try pressing POV Up to zero gyro"); + System.out.println("5. If robot drives wrong direction, check drive motor 'inverted' settings"); + System.out.println("==================================="); + + } catch (Exception e) { + logError("Motor inversion diagnostic error: " + e.getMessage()); + } + } + + @Override + public void periodic() { + if (isInitialized()) { + try { + Pose2d pose = swerveDrive.getPose(); + field2d.setRobotPose(pose); + + // Essential telemetry — always available on dashboard + SmartDashboard.putNumber("Drive/X (m)", Math.round(pose.getX() * 100.0) / 100.0); + SmartDashboard.putNumber("Drive/Y (m)", Math.round(pose.getY() * 100.0) / 100.0); + SmartDashboard.putNumber("Drive/Heading (deg)", + Math.round(pose.getRotation().getDegrees() * 10.0) / 10.0); + SmartDashboard.putNumber("Drive/Speed Multiplier", speedMultiplier); + + // Battery voltage — critical for knowing when to swap + double voltage = RobotController.getBatteryVoltage(); + SmartDashboard.putNumber("Robot/Battery (V)", Math.round(voltage * 100.0) / 100.0); + if (voltage < 11.5) { + SmartDashboard.putString("Robot/Battery Warning", "LOW BATTERY!"); + } else { + SmartDashboard.putString("Robot/Battery Warning", "OK"); + } + } catch (Exception e) { + // silently handle + } + } + } + + private void updateModuleData() { + if (!isInitialized()) return; + + String[] moduleNames = {"FL", "FR", "BL", "BR"}; + + try { + var modules = swerveDrive.getModules(); + if (modules == null) return; + + for (int i = 0; i < modules.length && i < moduleNames.length; i++) { + try { + if (modules[i] != null) { + double absPos = modules[i].getAbsolutePosition(); + boolean isValid = !Double.isNaN(absPos) && !Double.isInfinite(absPos); + SmartDashboard.putNumber("Swerve/" + moduleNames[i] + " Angle", isValid ? absPos : 0.0); + SmartDashboard.putBoolean("Swerve/" + moduleNames[i] + " Encoder OK", isValid); + } + } catch (Exception e) { + SmartDashboard.putBoolean("Swerve/" + moduleNames[i] + " Encoder OK", false); + } + } + } catch (Exception e) { + // Silently handle + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/LimelightVision.java b/src/main/java/frc/robot/subsystems/LimelightVision.java new file mode 100644 index 0000000..a9e3035 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LimelightVision.java @@ -0,0 +1,405 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * Limelight vision subsystem for AprilTag detection. + */ +public class LimelightVision extends SubsystemBase { + // NetworkTable entries + private final NetworkTable limelightTable; + private final NetworkTableEntry tx; + private final NetworkTableEntry ty; + private final NetworkTableEntry ta; + private final NetworkTableEntry tv; + private final NetworkTableEntry botpose; + private final NetworkTableEntry tl; // Pipeline latency + private final NetworkTableEntry cl; // Capture latency + private final NetworkTableEntry tid; // AprilTag ID + + // Cached values + private double horizontalOffset = 0.0; + private double verticalOffset = 0.0; + private double targetArea = 0.0; + private boolean hasTarget = false; // Fixed: int to boolean + private int currentTagId = -1; + private double totalLatencyMs = 0.0; + + // Connection status + private boolean isConnected = false; + private double lastUpdateTime = 0.0; + private static final double CONNECTION_TIMEOUT_SECONDS = 1.0; // Fixed: Added value + + // Reference to drive subsystem + private DriveSubsystem driveSubsystem = null; + + // Configuration + private boolean autoUpdatePose = true; + private double minTargetArea = 0.1; // Minimum area to consider valid + private double maxPoseAmbiguity = 0.7; // Maximum ambiguity to accept pose + + // Field boundaries for pose validation (meters) + private static final double FIELD_LENGTH = 16.54; + private static final double FIELD_WIDTH = 8.02; + private static final double POSE_MARGIN = 0.5; // Allow small margin outside field + + public LimelightVision() { + this("limelight"); + System.out.println("[LimelightVision] Initialized"); + } + + /** + * Create a LimelightVision with a custom NetworkTable name. + * @param tableName The name of the Limelight's NetworkTable (e.g., "limelight", "limelight-front") + */ public LimelightVision(String tableName) { + limelightTable = NetworkTableInstance.getDefault().getTable(tableName); + + // Get entries with null safety + tx = limelightTable.getEntry("tx"); + ty = limelightTable.getEntry("ty"); + ta = limelightTable.getEntry("ta"); + tv = limelightTable.getEntry("tv"); + botpose = limelightTable.getEntry("botpose_wpiblue"); // Use WPILib Blue alliance origin + tl = limelightTable.getEntry("tl"); + cl = limelightTable.getEntry("cl"); + tid = limelightTable.getEntry("tid"); + + // Initialize with safe defaults + try { + setLEDMode(LEDMode.OFF); + setCameraMode(CameraMode.VISION); + setPipeline(0); + } catch (Exception e) { + DriverStation.reportWarning("Limelight initialization warning: " + e.getMessage(), false); + } + + SmartDashboard.putBoolean("Limelight/Auto Update Pose", autoUpdatePose); + SmartDashboard.putNumber("Limelight/Min Target Area", minTargetArea); + } + + /** + * Set the drive subsystem to update with vision measurements. + */ + public void setDriveSubsystem(DriveSubsystem drive) { // Fixed: Changed return type to void + this.driveSubsystem = drive; + } + + /** + * Enable or disable automatic pose updates to the drive subsystem. + */ + public void setAutoUpdatePose(boolean enabled) { + this.autoUpdatePose = enabled; + SmartDashboard.putBoolean("Limelight/Auto Update Pose", autoUpdatePose); + } + + /** + * Set the minimum target area required to consider a detection valid. + */ + public void setMinTargetArea(double area) { + this.minTargetArea = Math.max(0.0, area); + SmartDashboard.putNumber("Limelight/Min Target Area", minTargetArea); + } + + @Override + public void periodic() { // Fixed: Added missing opening brace + try { + updateValues(); + updateConnectionStatus(); + updateDashboard(); + + // Automatically update drive subsystem with vision measurements + if (autoUpdatePose && driveSubsystem != null && hasValidTarget()) { + updateDriveWithVision(); + } + } catch (Exception e) { + DriverStation.reportError("LimelightVision periodic error: " + e.getMessage(), false); + } + } + + /** + * Update cached values from NetworkTables. + */ + private void updateValues() { + horizontalOffset = tx.getDouble(0.0); + verticalOffset = ty.getDouble(0.0); + targetArea = ta.getDouble(0.0); + hasTarget = tv.getDouble(0.0) == 1.0; + currentTagId = (int) tid.getDouble(-1.0); + + // Calculate total latency + double pipelineLatency = tl.getDouble(0.0); + double captureLatency = cl.getDouble(0.0); + totalLatencyMs = pipelineLatency + captureLatency; + } + + /** + * Check if we're receiving data from the Limelight. + */ + private void updateConnectionStatus() { + double currentTime = Timer.getFPGATimestamp(); + + // Check if we're getting updates (tx changes or we have a target) + double currentTx = tx.getDouble(Double.NaN); + if (!Double.isNaN(currentTx)) { + lastUpdateTime = currentTime; + isConnected = true; + } else if (currentTime - lastUpdateTime > CONNECTION_TIMEOUT_SECONDS) { + isConnected = false; + } + } + + /** + * Update SmartDashboard with current values. + */ + private void updateDashboard() { + SmartDashboard.putBoolean("Limelight/Connected", isConnected); + SmartDashboard.putBoolean("Limelight/Has Target", hasTarget); + SmartDashboard.putNumber("Limelight/TX", horizontalOffset); + SmartDashboard.putNumber("Limelight/TY", verticalOffset); + SmartDashboard.putNumber("Limelight/TA", targetArea); + SmartDashboard.putNumber("Limelight/Tag ID", currentTagId); + SmartDashboard.putNumber("Limelight/Latency (ms)", totalLatencyMs); + + if (hasTarget) { + Pose2d pose = getRobotPose(); + SmartDashboard.putString("Limelight/Pose", + String.format("(%.2f, %.2f) @ %.1f°", + pose.getX(), pose.getY(), pose.getRotation().getDegrees())); + } + } + + /** + * Send vision measurement to drive subsystem with latency compensation. + */ + private void updateDriveWithVision() { + Pose2d visionPose = getRobotPose(); + + // Validate pose before sending + if (!isPoseValid(visionPose)) { + SmartDashboard.putBoolean("Limelight/Pose Valid", false); + return; + } + + // Check target area is sufficient + if (targetArea < minTargetArea) { + SmartDashboard.putBoolean("Limelight/Pose Valid", false); + return; + } + + SmartDashboard.putBoolean("Limelight/Pose Valid", true); + + // Calculate timestamp with latency compensation + double timestamp = Timer.getFPGATimestamp() - (totalLatencyMs / 1000.0); + + updateVisionPose(visionPose, timestamp); + } + + /** + * Update vision pose in the drive subsystem. + */ + public void updateVisionPose(Pose2d visionPose, double timestamp) { + if (driveSubsystem != null && visionPose != null) { + driveSubsystem.addVisionMeasurement(visionPose, timestamp); + } + } + + /** + * Check if a pose is within valid field boundaries. + */ + private boolean isPoseValid(Pose2d pose) { + if (pose == null) { + return false; + } + + double x = pose.getX(); + double y = pose.getY(); + + // Check if pose is within field boundaries (with margin) + boolean xValid = x >= -POSE_MARGIN && x <= FIELD_LENGTH + POSE_MARGIN; + boolean yValid = y >= -POSE_MARGIN && y <= FIELD_WIDTH + POSE_MARGIN; + + // Check for obviously invalid values + boolean notZero = !(x == 0.0 && y == 0.0); + boolean notNaN = !Double.isNaN(x) && !Double.isNaN(y); + boolean notInfinite = !Double.isInfinite(x) && !Double.isInfinite(y); + + // Check pose ambiguity + boolean ambiguityValid = maxPoseAmbiguity >= 0.0 && maxPoseAmbiguity <= 1.0; + + return xValid && yValid && notZero && notNaN && notInfinite && ambiguityValid; + } + + /** + * Check if we have a valid target that meets all criteria. + */ + public boolean hasValidTarget() { + return hasTarget && isConnected && targetArea >= minTargetArea; + } + + /** + * Check if the Limelight is connected. + */ + public boolean isConnected() { + return isConnected; + } + + public double getHorizontalOffset() { + return horizontalOffset; + } + + public double getVerticalOffset() { + return verticalOffset; + } + + public double getTargetArea() { + return targetArea; + } + + public int getCurrentTagId() { + return currentTagId; + } + + public double getLatencyMs() { + return totalLatencyMs; + } + + /** + * Calculate distance to target using trigonometry. + */ + public double getDistanceToTarget(double targetHeightMeters, double cameraHeightMeters, double cameraMountAngleDegrees) { + if (!hasValidTarget()) { + return -1.0; // Return -1 to indicate no valid measurement + } + + double angleToGoalDegrees = cameraMountAngleDegrees + verticalOffset; + double angleToGoalRadians = Math.toRadians(angleToGoalDegrees); + + if (Math.abs(Math.tan(angleToGoalRadians)) < 0.001) { + return -1.0; // Avoid division by near-zero + } + + return (targetHeightMeters - cameraHeightMeters) / Math.tan(angleToGoalRadians); + } + + /** + * Get the robot pose from vision data. + */ + public Pose2d getRobotPose() { + try { + double[] poseArray = botpose.getDoubleArray(new double[0]); + + if (poseArray == null || poseArray.length < 6) { + return new Pose2d(); + } + + return new Pose2d( + poseArray[0], + poseArray[1], + Rotation2d.fromDegrees(poseArray[5]) + ); + } catch (Exception e) { + DriverStation.reportWarning("Error getting robot pose: " + e.getMessage(), false); + return new Pose2d(); + } + } + + /** + * Get the full pose array including Z, roll, and pitch. + */ + public double[] getFullPoseArray() { + try { + return botpose.getDoubleArray(new double[7]); + } catch (Exception e) { + return new double[7]; + } + } + + public void setPipeline(int pipeline) { + try { + limelightTable.getEntry("pipeline").setNumber(Math.max(0, Math.min(9, pipeline))); + SmartDashboard.putNumber("Limelight/Pipeline", pipeline); + } catch (Exception e) { + DriverStation.reportWarning("Error setting pipeline: " + e.getMessage(), false); + } + } + + public void setLEDMode(LEDMode mode) { + try { + limelightTable.getEntry("ledMode").setNumber(mode.getValue()); + SmartDashboard.putString("Limelight/LED Mode", mode.name()); + } catch (Exception e) { + DriverStation.reportWarning("Error setting LED mode: " + e.getMessage(), false); + } + } + + public void setCameraMode(CameraMode mode) { + try { + limelightTable.getEntry("camMode").setNumber(mode.getValue()); + SmartDashboard.putString("Limelight/Camera Mode", mode.name()); + } catch (Exception e) { + DriverStation.reportWarning("Error setting camera mode: " + e.getMessage(), false); + } + } + + /** + * Take a snapshot for later analysis. + */ + public void takeSnapshot() { + try { + limelightTable.getEntry("snapshot").setNumber(1); + } catch (Exception e) { + DriverStation.reportWarning("Error taking snapshot: " + e.getMessage(), false); + } + } + + /** + * Reset snapshot mode. + */ + public void resetSnapshot() { + try { + limelightTable.getEntry("snapshot").setNumber(0); + } catch (Exception e) { + DriverStation.reportWarning("Error resetting snapshot: " + e.getMessage(), false); + } + } + + public enum LEDMode { + PIPELINE(0), + OFF(1), + BLINK(2), + ON(3); + + private final int value; + + LEDMode(int value) { + this.value = value; + } + + public int getValue() { + return value; + } + } + + public enum CameraMode { + VISION(0), + DRIVER(1); + + private final int value; + + CameraMode(int value) { + this.value = value; + } + + public int getValue() { + return value; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java new file mode 100644 index 0000000..b1ff632 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkAbsoluteEncoder; + +/** + * Individual swerve module controller (not used with YAGSL, kept for reference). + * Each module controls one drive motor and one turning motor for swerve drive. + * + * Motor Layout: + * - Drive Motor: Controls forward/backward speed (brushless) + * - Turning Motor: Controls wheel direction (brushless) + * - Turning Encoder: Absolute encoder for wheel angle feedback + */ +public class SwerveModule { + private static final double kMaxSpeed = 4.5; // m/s + + private final SparkMax driveMotor; + private final SparkMax turningMotor; + private final SparkAbsoluteEncoder turningEncoder; + private final PIDController turningPIDController; + + public SwerveModule(int driveMotorId, int turningMotorId) { + driveMotor = new SparkMax(driveMotorId, MotorType.kBrushless); + turningMotor = new SparkMax(turningMotorId, MotorType.kBrushless); + turningEncoder = turningMotor.getAbsoluteEncoder(); + + // PID for wheel angle control + turningPIDController = new PIDController(0.5, 0.0, 0.0); + turningPIDController.enableContinuousInput(-Math.PI, Math.PI); + } + + private double getTurningRadians() { + return turningEncoder.getPosition(); + } + + public SwerveModuleState getState() { + return new SwerveModuleState( + driveMotor.getEncoder().getVelocity(), + new Rotation2d(getTurningRadians()) + ); + } + + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + driveMotor.getEncoder().getPosition(), new Rotation2d(getTurningRadians()) + ); + } + + @SuppressWarnings("deprecation") + public void setDesiredState(SwerveModuleState desiredState) { + if (desiredState == null) { + stop(); + return; + } + + // Optimize the desired state to avoid spinning more than 90 degrees + desiredState = SwerveModuleState.optimize(desiredState, getState().angle); + + driveMotor.set(desiredState.speedMetersPerSecond / kMaxSpeed); + + double turnOutput = turningPIDController.calculate( + getTurningRadians(), + desiredState.angle.getRadians() + ); + + turningMotor.set(turnOutput); + } + + public void stop() { + driveMotor.set(0.0); + turningMotor.set(0.0); + } +} diff --git a/temp_yagsl/org/ironmaple/simulation/drivesims/AbstractDriveTrainSimulation.java b/temp_yagsl/org/ironmaple/simulation/drivesims/AbstractDriveTrainSimulation.java new file mode 100644 index 0000000..451442f --- /dev/null +++ b/temp_yagsl/org/ironmaple/simulation/drivesims/AbstractDriveTrainSimulation.java @@ -0,0 +1,169 @@ +package org.ironmaple.simulation.drivesims; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import org.dyn4j.dynamics.Body; +import org.dyn4j.geometry.Geometry; +import org.dyn4j.geometry.MassType; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.utils.mathutils.GeometryConvertor; + +/**p + * + * + *

Represents an Abstract Drivetrain Simulation.

+ * + *

Simulates the Mass, Collision Space, and Friction of the Drivetrain.

+ * + *

This class models the physical properties of a drivetrain, including mass and collision space. + * + *

It also provides APIs to obtain the status (position, velocity etc.) in WPILib geometry classes. + * + *

The propelling forces generated by motors are simulated in its subclass, or {@link SwerveDriveSimulation}. + */ +public abstract class AbstractDriveTrainSimulation extends Body { + public static final double + BUMPER_COEFFICIENT_OF_FRICTION = 0.65, // https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction + BUMPER_COEFFICIENT_OF_RESTITUTION = 0.08; // https://simple.wikipedia.org/wiki/Coefficient_of_restitution + + public final DriveTrainSimulationConfig config; + + /** + * + * + *

Creates a Simulation of a Drivetrain.

+ * + *

Sets Up the Collision Space and Mass of the Chassis.

+ * + *

Since this is an abstract class, the constructor must be called from a subclass. + * + *

Note that the chassis does not appear on the simulation field upon creation. Refer to + * {@link org.ironmaple.simulation.SimulatedArena} for instructions on how to add it to the simulation world. + * + * @param config a {@link DriveTrainSimulationConfig} instance containing the configurations of this drivetrain + * @param initialPoseOnField the initial pose of the drivetrain in the simulation world + */ + protected AbstractDriveTrainSimulation(DriveTrainSimulationConfig config, Pose2d initialPoseOnField) { + + this.config = config; + /* width and height in world reference is flipped */ + final double WIDTH_IN_WORLD_REFERENCE = config.bumperLengthX.in(Meters), + HEIGHT_IN_WORLD_REFERENCE = config.bumperWidthY.in(Meters); + + super.addFixture( + Geometry.createRectangle(WIDTH_IN_WORLD_REFERENCE, HEIGHT_IN_WORLD_REFERENCE), + config.getDensityKgPerSquaredMeters(), + BUMPER_COEFFICIENT_OF_FRICTION, + BUMPER_COEFFICIENT_OF_RESTITUTION); + + super.setMass(MassType.NORMAL); + super.setLinearDamping(0.1); + super.setAngularDamping(0.1); + setSimulationWorldPose(initialPoseOnField); + } + + /** + * + * + *

Sets the Robot's Current Pose in the Simulation World.

+ * + *

This method instantly teleports the robot to the specified pose in the simulation world. The robot does not + * drive to the new pose; it is moved directly. + * + * @param robotPose the desired robot pose, represented as a {@link Pose2d} + */ + public void setSimulationWorldPose(Pose2d robotPose) { + super.transform.set(GeometryConvertor.toDyn4jTransform(robotPose)); + super.linearVelocity.set(0, 0); + } + + /** + * + * + *

Sets the Robot's Speeds to the Given Chassis Speeds.

+ * + *

This method sets the robot's current velocity to the specified chassis speeds. + * + *

The robot does not accelerate smoothly to these speeds; instead, it jumps to the velocity + * Instantaneously. + * + * @param givenSpeeds the desired chassis speeds, represented as a {@link ChassisSpeeds} object + */ + public void setRobotSpeeds(ChassisSpeeds givenSpeeds) { + super.setLinearVelocity(GeometryConvertor.toDyn4jLinearVelocity(givenSpeeds)); + super.setAngularVelocity(givenSpeeds.omegaRadiansPerSecond); + } + + /** + * + * + *

Abstract Simulation Sub-Tick Method.

+ * + *

This method is called every time the simulation world is updated. + * + *

It is implemented in the sub-classes of {@link AbstractDriveTrainSimulation}. + * + *

It is responsible for applying the propelling forces to the robot during each sub-tick of the simulation. + */ + public abstract void simulationSubTick(); + + /** + * + * + *

Gets the Actual Pose of the Drivetrain in the Simulation World.

+ * + *

This method is used to display the robot on AdvantageScope Field3d or to update + * vision simulations. + * + *

Note: Do not use this method to simulate odometry! For a more realistic odometry simulation, + * use a {@link SwerveDriveSimulation} together with a + * {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator}. + * + * @return a {@link Pose2d} object yielding the current world pose of the robot in the simulation + */ + public Pose2d getSimulatedDriveTrainPose() { + return GeometryConvertor.toWpilibPose2d(getTransform()); + } + + /** + * + * + *

Gets the Actual Robot-Relative Chassis Speeds from the Simulation.

+ * + *

This method returns the actual chassis speeds of the drivetrain in the simulation, relative to the robot. + * + *

To simulate the chassis speeds calculated by encoders, use a {@link SwerveDriveSimulation} together with + * {@link edu.wpi.first.math.kinematics.SwerveDriveKinematics#toChassisSpeeds(SwerveModuleState...)} for a more + * realistic simulation. + * + * @return the actual chassis speeds in the simulation world, Robot-Relative + */ + public ChassisSpeeds getDriveTrainSimulatedChassisSpeedsRobotRelative() { + ChassisSpeeds speeds = getDriveTrainSimulatedChassisSpeedsFieldRelative(); + speeds = ChassisSpeeds.fromFieldRelativeSpeeds( + speeds, getSimulatedDriveTrainPose().getRotation()); + return speeds; + } + + /** + * + * + *

Gets the Actual Field-Relative Chassis Speeds from the Simulation.

+ * + *

This method returns the actual chassis speeds of the drivetrain in the simulation, relative to the robot. + * + *

To simulate the chassis speeds calculated by encoders, use a {@link SwerveDriveSimulation} together with + * {@link edu.wpi.first.math.kinematics.SwerveDriveKinematics#toChassisSpeeds(SwerveModuleState...)} for a more + * realistic simulation. + * + * @return the actual chassis speeds in the simulation world, Field-Relative + */ + public ChassisSpeeds getDriveTrainSimulatedChassisSpeedsFieldRelative() { + return GeometryConvertor.toWpilibChassisSpeeds(getLinearVelocity(), getAngularVelocity()); + } +} diff --git a/temp_yagsl/org/ironmaple/simulation/drivesims/SelfControlledSwerveDriveSimulation.java b/temp_yagsl/org/ironmaple/simulation/drivesims/SelfControlledSwerveDriveSimulation.java new file mode 100644 index 0000000..3c27341 --- /dev/null +++ b/temp_yagsl/org/ironmaple/simulation/drivesims/SelfControlledSwerveDriveSimulation.java @@ -0,0 +1,580 @@ +package org.ironmaple.simulation.drivesims; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +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.kinematics.*; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.Timer; +import java.util.Arrays; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.motorsims.SimulatedMotorController; +import org.ironmaple.utils.mathutils.SwerveStateProjection; + +/** + * + * + *

An easier way to simulate swerve drive.

+ * + *

Check Online Documentation + * + *

This class owns and controls a {@link SwerveDriveSimulation}, running closed loops/open loops on the simulated + * motors. + * + *

It works identically to how the real swerve simulation code. + * + *

Note: The order for swerve modules is: front-left, front-right, back-left, back-right. + */ +public class SelfControlledSwerveDriveSimulation { + private final SwerveDriveSimulation swerveDriveSimulation; + private final SelfControlledModuleSimulation[] moduleSimulations; + private final SwerveDriveKinematics kinematics; + private final SwerveDrivePoseEstimator poseEstimator; + private final SwerveModuleState[] setPointsOptimized; + + /** + * + * + *

Default Constructor.

+ * + *

Constructs a simplified swerve simulation with default standard deviations for odometry & vision pose + * estimates. + * + *

Odometry is simulated as high-frequency odometry, assuming a robot period of 0.02 seconds and an odometry + * frequency of 250 Hz. + * + * @param swerveDriveSimulation the {@link SwerveDriveSimulation} to control. + */ + public SelfControlledSwerveDriveSimulation(SwerveDriveSimulation swerveDriveSimulation) { + this(swerveDriveSimulation, VecBuilder.fill(0.1, 0.1, 0.1), VecBuilder.fill(0.9, 0.9, 0.9)); + } + + /** + * + * + *

Constructs an instance with given odometry standard deviations.

+ * + *

Constructs a simplified swerve simulation with specified standard deviations for odometry & vision pose + * estimates. + * + * @param swerveDriveSimulation the {@link SwerveDriveSimulation} to control. + * @param stateStdDevs the standard deviations for odometry encoders. + * @param visionMeasurementStdDevs the standard deviations for vision pose estimates. + */ + public SelfControlledSwerveDriveSimulation( + SwerveDriveSimulation swerveDriveSimulation, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + this.swerveDriveSimulation = swerveDriveSimulation; + this.moduleSimulations = Arrays.stream(swerveDriveSimulation.getModules()) + .map(SelfControlledModuleSimulation::new) + .toArray(SelfControlledModuleSimulation[]::new); + + this.kinematics = swerveDriveSimulation.kinematics; + + this.poseEstimator = new SwerveDrivePoseEstimator( + kinematics, + getRawGyroAngle(), + getLatestModulePositions(), + getActualPoseInSimulationWorld(), + stateStdDevs, + visionMeasurementStdDevs); + + this.setPointsOptimized = new SwerveModuleState[moduleSimulations.length]; + Arrays.fill(setPointsOptimized, new SwerveModuleState()); + } + + /** + * + * + *

Periodic Method for Simplified Swerve Sim.

+ * + *

Call this method in the {@link edu.wpi.first.wpilibj2.command.Subsystem#periodic()} of your swerve subsystem. + * + *

Updates the odometry by fetching cached inputs. + */ + public void periodic() { + final SwerveModulePosition[][] cachedModulePositions = getCachedModulePositions(); + for (int i = 0; i < SimulatedArena.getSimulationSubTicksIn1Period(); i++) + poseEstimator.updateWithTime( + Timer.getFPGATimestamp() + - SimulatedArena.getSimulationDt().in(Seconds) + * (SimulatedArena.getSimulationDt().in(Seconds) - i), + swerveDriveSimulation.gyroSimulation.getCachedGyroReadings()[i], + cachedModulePositions[i]); + } + + /** + * + * + *

Obtains the LATEST module positions measured by the encoders.

+ * + *

The order for swerve modules is: front-left, front-right, back-left, back-right. + * + * @return the module positions + */ + public SwerveModulePosition[] getLatestModulePositions() { + return Arrays.stream(moduleSimulations) + .map(SelfControlledModuleSimulation::getModulePosition) + .toArray(SwerveModulePosition[]::new); + } + + /** + * + * + *

Obtains the CACHED module positions measured by the encoders.

+ * + *

This simulates high-frequency odometry. + * + *

The module positions, or the value of {@link #getLatestModulePositions()} are cached in every simulation + * sub-tick. + * + *

The array is ordered in a [timeStampIndex][moduleIndex] format. + * + *

The order for swerve modules is: front-left, front-right, back-left, back-right. + * + * @return the cached module positions + */ + public SwerveModulePosition[][] getCachedModulePositions() { + final SwerveModulePosition[][] cachedModulePositions = + new SwerveModulePosition[SimulatedArena.getSimulationSubTicksIn1Period()][moduleSimulations.length]; + + for (int moduleIndex = 0; moduleIndex < moduleSimulations.length; moduleIndex++) { + final Angle[] wheelPosition = moduleSimulations[moduleIndex].instance.getCachedDriveWheelFinalPositions(); + final Rotation2d[] swerveModuleFacings = + moduleSimulations[moduleIndex].instance.getCachedSteerAbsolutePositions(); + for (int timeStamp = 0; timeStamp < SimulatedArena.getSimulationSubTicksIn1Period(); timeStamp++) + cachedModulePositions[timeStamp][moduleIndex] = new SwerveModulePosition( + wheelPosition[timeStamp].in(Radians) + * moduleSimulations[0].instance.config.WHEEL_RADIUS.in(Meters), + swerveModuleFacings[timeStamp]); + } + + return cachedModulePositions; + } + + /** + * + * + *

Obtains the raw angle of the gyro.

+ * + *

Note that the simulated gyro also drifts/skids like real gyros, especially if the robot collides. + * + *

To obtain the facing of the robot retrieved from the odometry, use {@link #getOdometryEstimatedPose()}; to + * obtain the actual facing of the robot, use {@link #getActualPoseInSimulationWorld()}. + * + * @deprecated This rotation is NOT the actual facing of the robot; it is uncalibrated and is only + * used for features like rotation lock. + * @return the raw (uncalibrated) angle of the simulated gyro. + */ + @Deprecated + public Rotation2d getRawGyroAngle() { + return swerveDriveSimulation.gyroSimulation.getGyroReading(); + } + + /** + * + * + *

Obtains the robot pose measured by the odometry.

+ * + *

Retrieves the pose estimated by the odometry system (and vision, if applicable). + * + *

This method wraps around {@link SwerveDrivePoseEstimator#getEstimatedPosition()}. + * + *

This represents the estimated position of the robot. + * + *

Note that this estimation includes realistic simulations of measurement errors due to skidding and odometry + * drift. + * + *

To obtain the ACTUAL pose of the robot, with no measurement errors, use + * {@link #getActualPoseInSimulationWorld()}. + */ + public Pose2d getOdometryEstimatedPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** + * + * + *

Resets the odometry to a specified position.

+ * + *

This method wraps around {@link SwerveDrivePoseEstimator#resetPosition(Rotation2d, SwerveModulePosition[], + * Pose2d)}. + * + *

It resets the position of the pose estimator to the given pose. + * + * @param pose The position on the field where the robot is located. + */ + public void resetOdometry(Pose2d pose) { + this.poseEstimator.resetPosition(getRawGyroAngle(), getLatestModulePositions(), pose); + } + + /** + * + * + *

Adds a vision estimation to the pose estimator.

+ * + *

This method wraps around {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)}. + * + *

Adds a vision measurement to the Kalman Filter, correcting the odometry pose estimate while accounting for + * measurement noise. + * + * @param robotPoseMeters The pose of the robot as measured by the vision camera. + * @param timeStampSeconds The timestamp of the vision measurement, in seconds. + */ + public void addVisionEstimation(Pose2d robotPoseMeters, double timeStampSeconds) { + this.poseEstimator.addVisionMeasurement(robotPoseMeters, timeStampSeconds); + } + + /** + * + * + *

Adds a vision estimation to the pose estimator.

+ * + *

This method wraps around {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double, Matrix)}. + * + *

Adds a vision measurement to the Kalman Filter, correcting the odometry pose estimate while accounting for + * measurement noise. + * + * @param robotPoseMeters The pose of the robot as measured by the vision camera. + * @param timeStampSeconds The timestamp of the vision measurement, in seconds. + * @param measurementStdDevs Standard deviations of the vision pose measurement (x position in meters, y position in + * meters, and heading in radians). Increase these values to reduce the trust in the vision pose measurement. + */ + public void addVisionEstimation( + Pose2d robotPoseMeters, double timeStampSeconds, Matrix measurementStdDevs) { + this.poseEstimator.addVisionMeasurement(robotPoseMeters, timeStampSeconds, measurementStdDevs); + } + + /** + * + * + *

Runs chassis speeds on the simulated swerve drive.

+ * + *

Runs the specified chassis speeds, either robot-centric or field-centric. + * + * @param chassisSpeeds The speeds to run, in either robot-centric or field-centric coordinates. + * @param centerOfRotationMeters The center of rotation. For example, if you set the center of rotation at one + * corner of the robot and provide a chassis speed that has only a dtheta component, the robot will rotate + * around that corner. + * @param fieldCentricDrive Whether to execute field-centric drive with the provided speed. + * @param discretizeSpeeds Whether to apply {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} to the provided + * speed. + */ + public void runChassisSpeeds( + ChassisSpeeds chassisSpeeds, + Translation2d centerOfRotationMeters, + boolean fieldCentricDrive, + boolean discretizeSpeeds) { + if (fieldCentricDrive) { + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + chassisSpeeds, getOdometryEstimatedPose().getRotation()); + } + if (discretizeSpeeds) { + chassisSpeeds = ChassisSpeeds.discretize( + chassisSpeeds, + SimulatedArena.getSimulationDt().in(Seconds) * SimulatedArena.getSimulationSubTicksIn1Period()); + } + final SwerveModuleState[] setPoints = kinematics.toSwerveModuleStates(chassisSpeeds, centerOfRotationMeters); + runSwerveStates(setPoints); + } + + /** + * + * + *

Runs a raw module states on the chassis.

+ * + *

Runs the specified module states on the modules. + * + * @param setPoints an array of {@link SwerveModuleState} yielding the requested states + */ + public void runSwerveStates(SwerveModuleState[] setPoints) { + for (int i = 0; i < moduleSimulations.length; i++) + setPointsOptimized[i] = moduleSimulations[i].optimizeAndRunModuleState(setPoints[i]); + } + + /** + * + * + *

Obtain the MEASURED swerve states.

+ * + *

The order of the swerve modules is: front-left, front-right, back-left, back-right. + * + * @return The actual measured swerve states of the simulated swerve. + */ + public SwerveModuleState[] getMeasuredStates() { + return Arrays.stream(moduleSimulations) + .map(SelfControlledModuleSimulation::getMeasuredState) + .toArray(SwerveModuleState[]::new); + } + + /** + * + * + *

Obtain the optimized SETPOINTS of the swerve.

+ * + *

The setpoints are calculated using {@link SwerveDriveKinematics#toSwerveModuleStates(ChassisSpeeds)} in the + * most recent call to {@link #runChassisSpeeds(ChassisSpeeds, Translation2d, boolean, boolean)}. + * + *

The setpoints are optimized using {@link SwerveModuleState#optimize(SwerveModuleState, Rotation2d)}. + * + *

The order of the swerve modules is: front-left, front-right, back-left, back-right. + * + * @return The optimized setpoints of the swerve, calculated during the last chassis speed run. + */ + public SwerveModuleState[] getSetPointsOptimized() { + return setPointsOptimized; + } + + /** + * + * + *

Obtain the field-relative chassis speeds measured from the encoders.

+ * + *

The speeds are measured from the simulated swerve modules. + * + * @param useGyroForAngularVelocity Whether to use the gyro for a more accurate angular velocity measurement. + * @return The measured chassis speeds, field-relative. + */ + public ChassisSpeeds getMeasuredSpeedsFieldRelative(boolean useGyroForAngularVelocity) { + ChassisSpeeds speeds = getMeasuredSpeedsRobotRelative(useGyroForAngularVelocity); + speeds = ChassisSpeeds.fromRobotRelativeSpeeds( + speeds, getOdometryEstimatedPose().getRotation()); + return speeds; + } + + /** + * + * + *

Obtain the robot-relative chassis speeds measured from the encoders.

+ * + *

The speeds are measured from the simulated swerve modules. + * + * @param useGyroForAngularVelocity Whether to use the gyro for a more accurate angular velocity measurement. + * @return The measured chassis speeds, robot-relative. + */ + public ChassisSpeeds getMeasuredSpeedsRobotRelative(boolean useGyroForAngularVelocity) { + final ChassisSpeeds swerveSpeeds = kinematics.toChassisSpeeds(getMeasuredStates()); + return new ChassisSpeeds( + swerveSpeeds.vxMetersPerSecond, + swerveSpeeds.vyMetersPerSecond, + useGyroForAngularVelocity + ? swerveDriveSimulation + .gyroSimulation + .getMeasuredAngularVelocity() + .in(RadiansPerSecond) + : swerveSpeeds.omegaRadiansPerSecond); + } + + /** + * + * + *

Obtain the {@link SwerveDriveSimulation} object controlled by this simplified swerve simulation.

+ * + * @return The swerve drive simulation. + */ + public SwerveDriveSimulation getDriveTrainSimulation() { + return this.swerveDriveSimulation; + } + + /** + * + * + *

Obtain the ACTUAL robot pose.

+ * + *

Obtains the ACTUAL robot pose with zero measurement error. + * + *

To obtain the pose calculated by odometry (where the robot thinks it is), use + * {@link #getOdometryEstimatedPose()}. + */ + public Pose2d getActualPoseInSimulationWorld() { + return swerveDriveSimulation.getSimulatedDriveTrainPose(); + } + + /** + * + * + *

Get the ACTUAL field-relative chassis speeds of the robot.

+ * + *

Wraps around {@link SwerveDriveSimulation#getDriveTrainSimulatedChassisSpeedsFieldRelative()}. + * + * @return the actual chassis speeds in the simulation world, field-relative + */ + public ChassisSpeeds getActualSpeedsFieldRelative() { + return this.swerveDriveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(); + } + + /** + * + * + *

Get the ACTUAL robot-relative chassis speeds of the robot.

+ * + *

Wraps around {@link SwerveDriveSimulation#getDriveTrainSimulatedChassisSpeedsRobotRelative()}. + * + * @return the actual chassis speeds in the simulation world, robot-relative + */ + public ChassisSpeeds getActualSpeedsRobotRelative() { + return this.swerveDriveSimulation.getDriveTrainSimulatedChassisSpeedsRobotRelative(); + } + + /** + * + * + *

Teleport the robot to a specified location on the simulated field.

+ * + *

This method moves the drivetrain instantly to the specified location on the field, bypassing any obstacles in + * its path. + * + *

Wraps around {@link SwerveDriveSimulation#setSimulationWorldPose(Pose2d)}. + * + * @param robotPose the pose of the robot to teleport to + */ + public void setSimulationWorldPose(Pose2d robotPose) { + this.swerveDriveSimulation.setSimulationWorldPose(robotPose); + } + + public SelfControlledSwerveDriveSimulation withSteerPID(PIDController steerController) { + for (SelfControlledModuleSimulation moduleSimulation : moduleSimulations) + moduleSimulation.withSteerPID( + new PIDController(steerController.getP(), steerController.getI(), steerController.getD())); + return this; + } + + public SelfControlledSwerveDriveSimulation withCurrentLimits(Current driveCurrentLimit, Current steerCurrentLimit) { + for (SelfControlledModuleSimulation moduleSimulation : moduleSimulations) + moduleSimulation.withCurrentLimits(driveCurrentLimit, steerCurrentLimit); + + return this; + } + + public static class SelfControlledModuleSimulation { + public final SwerveModuleSimulation instance; + private Current driveCurrentLimit; + + private PIDController steerController; + + private final SimulatedMotorController.GenericMotorController driveMotor; + private final SimulatedMotorController.GenericMotorController steerMotor; + + public SelfControlledModuleSimulation(SwerveModuleSimulation moduleSimulation) { + this.instance = moduleSimulation; + steerController = new PIDController(5.0, 0, 0); + + this.driveMotor = this.instance.useGenericMotorControllerForDrive(); + this.driveMotor.withCurrentLimit(this.driveCurrentLimit = Amps.of(60)); + this.steerMotor = this.instance.useGenericControllerForSteer(); + } + + public SelfControlledModuleSimulation withSteerPID(PIDController steerController) { + this.steerController = steerController; + steerController.enableContinuousInput(-Math.PI, Math.PI); + return this; + } + + public SelfControlledModuleSimulation withCurrentLimits(Current driveCurrentLimit, Current steerCurrentLimit) { + this.driveMotor.withCurrentLimit(this.driveCurrentLimit = driveCurrentLimit); + this.steerMotor.withCurrentLimit(steerCurrentLimit); + steerController.enableContinuousInput(-Math.PI, Math.PI); + return this; + } + + /** + * + * + *

Runs the control loops for swerve states on a simulated module.

+ * + *

Optimizes the set-point using {@link SwerveModuleState#optimize(SwerveModuleState, Rotation2d)}. + * + *

Executes a closed-loop control on the swerve module. + * + * @param setPoint the desired state to optimize and apply + * @return the optimized swerve module state after control execution + */ + public SwerveModuleState optimizeAndRunModuleState(SwerveModuleState setPoint) { + setPoint.optimize(instance.getSteerAbsoluteFacing()); + runModuleState(setPoint); + return setPoint; + } + + public void runModuleState(SwerveModuleState setPoint) { + final double + cosProjectedSpeedMPS = SwerveStateProjection.project(setPoint, instance.getSteerAbsoluteFacing()), + driveWheelVelocitySetPointRadPerSec = + cosProjectedSpeedMPS / instance.config.WHEEL_RADIUS.in(Meters); + + driveMotor.requestVoltage(instance.config.driveMotorConfigs.calculateVoltage( + Amps.of(0), RadiansPerSecond.of(driveWheelVelocitySetPointRadPerSec))); + + steerMotor.requestVoltage(Volts.of(steerController.calculate( + instance.getSteerAbsoluteFacing().getRadians(), setPoint.angle.getRadians()))); + } + + public void runDriveMotorCharacterization(Rotation2d desiredModuleFacing, double volts) { + driveMotor.requestVoltage(Volts.of(volts)); + + steerMotor.requestVoltage(Volts.of(steerController.calculate( + instance.getSteerAbsoluteFacing().getRadians(), desiredModuleFacing.getRadians()))); + } + + public void runSteerMotorCharacterization(double volts) { + driveMotor.requestVoltage(Volts.zero()); + + steerMotor.requestVoltage(Volts.of(volts)); + } + + public SwerveModuleState getMeasuredState() { + return instance.getCurrentState(); + } + + public SwerveModulePosition getModulePosition() { + return new SwerveModulePosition( + instance.getDriveWheelFinalPosition().in(Radians) * instance.config.WHEEL_RADIUS.in(Meters), + instance.getSteerAbsoluteFacing()); + } + } + + /** @see SwerveDriveSimulation#maxLinearVelocity() */ + public LinearVelocity maxLinearVelocity() { + return swerveDriveSimulation.maxLinearVelocity(); + } + + /** @see SwerveDriveSimulation#maxLinearAcceleration(Current) */ + public LinearAcceleration maxLinearAcceleration() { + return swerveDriveSimulation.maxLinearAcceleration(moduleSimulations[0].driveCurrentLimit); + } + + /** @see DriveTrainSimulationConfig#trackWidthY() */ + public Distance trackWidthY() { + return swerveDriveSimulation.config.trackWidthY(); + } + + /** @see DriveTrainSimulationConfig#trackLengthX() */ + public Distance trackLengthX() { + return swerveDriveSimulation.config.trackLengthX(); + } + + /** @see SwerveDriveSimulation#driveBaseRadius() */ + public Distance driveBaseRadius() { + return swerveDriveSimulation.config.driveBaseRadius(); + } + + /** @see SwerveDriveSimulation#maxAngularVelocity() */ + public AngularVelocity maxAngularVelocity() { + return RadiansPerSecond.of( + maxLinearVelocity().in(MetersPerSecond) / driveBaseRadius().in(Meters)); + } + + /** @see SwerveDriveSimulation#maxAngularAcceleration(Current) */ + public AngularAcceleration maxAngularAcceleration() { + return swerveDriveSimulation.maxAngularAcceleration(moduleSimulations[0].driveCurrentLimit); + } +} diff --git a/temp_yagsl/org/ironmaple/simulation/drivesims/SwerveDriveSimulation.java b/temp_yagsl/org/ironmaple/simulation/drivesims/SwerveDriveSimulation.java new file mode 100644 index 0000000..84f2ea0 --- /dev/null +++ b/temp_yagsl/org/ironmaple/simulation/drivesims/SwerveDriveSimulation.java @@ -0,0 +1,367 @@ +package org.ironmaple.simulation.drivesims; + +import static edu.wpi.first.units.Units.*; + +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.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.measure.*; +import java.util.Arrays; +import java.util.function.Supplier; +import org.dyn4j.geometry.Vector2; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.utils.mathutils.GeometryConvertor; +import org.ironmaple.utils.mathutils.MapleCommonMath; + +/** + * + * + *

Simulates a Swerve Drivetrain.

+ * + *

Check Online + * Documentation + * + *

1. Purpose

+ * + *

This class simulates a swerve drivetrain composed of more than two {@link SwerveModuleSimulation} modules. + * + *

It provides a realistic modeling of drivetrain physics, replicating wheel grip and motor propulsion for an actual + * swerve drive. + * + *

2. Simulation Dynamics

+ * + * + * + *

3. Odometry Simulation

+ * + *

To simulate odometry, follow these steps: + * + *

+ * + *

Refer to the ModuleIOSim.java + * example project for more details. + * + *

Vision Simulation

+ * + *

You can obtain the real robot pose from {@link #getSimulatedDriveTrainPose()} and feed it to the PhotonVision + * simulation to simulate vision. + */ +public class SwerveDriveSimulation extends AbstractDriveTrainSimulation { + private final SwerveModuleSimulation[] moduleSimulations; + protected final GyroSimulation gyroSimulation; + protected final Translation2d[] moduleTranslations; + protected final SwerveDriveKinematics kinematics; + private final double gravityForceOnEachModule; + + /** + * + * + *

Creates a Swerve Drive Simulation.

+ * + *

This constructor initializes a swerve drive simulation with the given robot mass, bumper dimensions, module + * simulations, module translations, gyro simulation, and initial pose on the field. + * + * @param config a {@link DriveTrainSimulationConfig} instance containing the configurations of * this drivetrain + * @param initialPoseOnField the initial pose of the drivetrain in the simulation world, represented as a + * {@link Pose2d} + */ + public SwerveDriveSimulation(DriveTrainSimulationConfig config, Pose2d initialPoseOnField) { + super(config, initialPoseOnField); + this.moduleTranslations = config.moduleTranslations; + this.moduleSimulations = Arrays.stream(config.swerveModuleSimulationFactories) + .map(Supplier::get) + .toArray(SwerveModuleSimulation[]::new); + this.gyroSimulation = config.gyroSimulationFactory.get(); + + super.setLinearDamping(1.4); + super.setAngularDamping(1.4); + this.kinematics = new SwerveDriveKinematics(moduleTranslations); + + this.gravityForceOnEachModule = config.robotMass.in(Kilograms) * 9.8 / moduleSimulations.length; + } + + /** + * + * + *

Updates the Swerve Drive Simulation.

+ * + *

This method performs the following actions during each sub-tick of the simulation: + * + *

+ */ + @Override + public void simulationSubTick() { + simulateChassisFrictionForce(); + + simulateChassisFrictionTorque(); + + simulateModulePropellingForces(); + + gyroSimulation.updateSimulationSubTick(super.getAngularVelocity()); + } + + private Translation2d previousModuleSpeedsFieldRelative = new Translation2d(); + + /** + * + * + *

Simulates the Translational Friction Force and Applies It to the Physics Engine.

+ * + *

This method simulates the translational friction forces acting on the robot and applies them to the physics + * engine. There are two components of the friction forces: + * + *

+ * + *

The total friction force should not exceed the tire's grip limit. + */ + private void simulateChassisFrictionForce() { + final ChassisSpeeds moduleSpeeds = getModuleSpeeds(); + + /* The friction force that tries to bring the chassis from floor speeds to module speeds */ + final ChassisSpeeds differenceBetweenFloorSpeedAndModuleSpeedsRobotRelative = + moduleSpeeds.minus(getDriveTrainSimulatedChassisSpeedsRobotRelative()); + final Translation2d floorAndModuleSpeedsDiffFieldRelative = new Translation2d( + differenceBetweenFloorSpeedAndModuleSpeedsRobotRelative.vxMetersPerSecond, + differenceBetweenFloorSpeedAndModuleSpeedsRobotRelative.vyMetersPerSecond) + .rotateBy(getSimulatedDriveTrainPose().getRotation()); + final double FRICTION_FORCE_GAIN = 3.0, + totalGrippingForce = + moduleSimulations[0].config.getGrippingForceNewtons(gravityForceOnEachModule) + * moduleSimulations.length; + final Vector2 speedsDifferenceFrictionForce = Vector2.create( + Math.min( + FRICTION_FORCE_GAIN * totalGrippingForce * floorAndModuleSpeedsDiffFieldRelative.getNorm(), + totalGrippingForce), + MapleCommonMath.getAngle(floorAndModuleSpeedsDiffFieldRelative).getRadians()); + + /* the centripetal friction force during turning */ + final ChassisSpeeds moduleSpeedsFieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds( + moduleSpeeds, getSimulatedDriveTrainPose().getRotation()); + final Rotation2d dTheta = MapleCommonMath.getAngle( + GeometryConvertor.getChassisSpeedsTranslationalComponent(moduleSpeedsFieldRelative)) + .minus(MapleCommonMath.getAngle(previousModuleSpeedsFieldRelative)); + + final double orbitalAngularVelocity = + dTheta.getRadians() / SimulatedArena.getSimulationDt().in(Seconds); + final Rotation2d centripetalForceDirection = + MapleCommonMath.getAngle(previousModuleSpeedsFieldRelative).plus(Rotation2d.fromDegrees(90)); + final Vector2 centripetalFrictionForce = Vector2.create( + previousModuleSpeedsFieldRelative.getNorm() * orbitalAngularVelocity * config.robotMass.in(Kilograms), + centripetalForceDirection.getRadians()); + previousModuleSpeedsFieldRelative = + GeometryConvertor.getChassisSpeedsTranslationalComponent(moduleSpeedsFieldRelative); + + /* apply force to physics engine */ + final Vector2 + totalFrictionForceUnlimited = centripetalFrictionForce.copy().add(speedsDifferenceFrictionForce), + totalFrictionForce = + Vector2.create( + Math.min(totalGrippingForce, totalFrictionForceUnlimited.getMagnitude()), + totalFrictionForceUnlimited.getDirection()); + super.applyForce(totalFrictionForce); + } + + /** + * + * + *

Simulates the Rotational Friction Torque and Applies It to the Physics Engine.

+ * + *

This method simulates the rotational friction torque acting on the robot and applies them to the physics + * engine. + * + *

The friction torque pushes the robot from its current ground angular velocity + * ({@link #getDriveTrainSimulatedChassisSpeedsRobotRelative()}) toward its current modules' angular velocity + * ({@link #getModuleSpeeds()}). + */ + private void simulateChassisFrictionTorque() { + final double + desiredRotationalMotionPercent = + Math.abs(getDesiredSpeed().omegaRadiansPerSecond + / maxAngularVelocity().in(RadiansPerSecond)), + actualRotationalMotionPercent = + Math.abs(getAngularVelocity() / maxAngularVelocity().in(RadiansPerSecond)), + differenceBetweenFloorSpeedAndModuleSpeed = + getModuleSpeeds().omegaRadiansPerSecond - getAngularVelocity(), + grippingTorqueMagnitude = + moduleSimulations[0].config.getGrippingForceNewtons(gravityForceOnEachModule) + * moduleTranslations[0].getNorm() + * moduleSimulations.length, + FRICTION_TORQUE_GAIN = 1; + + if (actualRotationalMotionPercent < 0.01 && desiredRotationalMotionPercent < 0.02) super.setAngularVelocity(0); + else + super.applyTorque(Math.copySign( + Math.min( + FRICTION_TORQUE_GAIN + * grippingTorqueMagnitude + * Math.abs(differenceBetweenFloorSpeedAndModuleSpeed), + grippingTorqueMagnitude), + differenceBetweenFloorSpeedAndModuleSpeed)); + } + + /** + * + * + *

Simulates the Translational Friction Force and Applies It to the Physics Engine.

+ * + *

This method simulates the translational friction forces acting on the robot and applies them to the physics + * engine. There are two components of the friction forces: + * + *

+ * + *

The total friction force should not exceed the tire's grip limit. + */ + private void simulateModulePropellingForces() { + for (int i = 0; i < moduleSimulations.length; i++) { + final Vector2 moduleWorldPosition = getWorldPoint(GeometryConvertor.toDyn4jVector2(moduleTranslations[i])); + final Vector2 moduleForce = moduleSimulations[i].updateSimulationSubTickGetModuleForce( + super.getLinearVelocity(moduleWorldPosition), + getSimulatedDriveTrainPose().getRotation(), + gravityForceOnEachModule); + super.applyForce(moduleForce, moduleWorldPosition); + } + } + + /** + * + * + *

Obtains the Chassis Speeds the Modules Are Attempting to Achieve.

+ * + *

This method returns the desired chassis speeds that the modules are trying to reach. If the robot maintains + * the current driving voltage and steering position for a long enough period, it will achieve these speeds. + * + * @return the desired chassis speeds, robot-relative + */ + private ChassisSpeeds getDesiredSpeed() { + return kinematics.toChassisSpeeds(Arrays.stream(moduleSimulations) + .map((SwerveModuleSimulation::getFreeSpinState)) + .toArray(SwerveModuleState[]::new)); + } + + /** + * + * + *

Obtains the Current Chassis Speeds of the Modules.

+ * + *

This method estimates the chassis speeds of the robot based on the swerve states of the modules. + * + *

Note: These speeds might not represent the actual floor speeds due to potential skidding. + * + * @return the module speeds, robot-relative + */ + private ChassisSpeeds getModuleSpeeds() { + return kinematics.toChassisSpeeds(Arrays.stream(moduleSimulations) + .map((SwerveModuleSimulation::getCurrentState)) + .toArray(SwerveModuleState[]::new)); + } + + /** + * + * + *

Obtains the maximum achievable linear velocity of the chassis.

+ * + * @return the maximum linear velocity + * @see org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig#maximumGroundSpeed() + */ + public LinearVelocity maxLinearVelocity() { + return moduleSimulations[0].config.maximumGroundSpeed(); + } + + /** + * + * + *

Obtains the maximum achievable linear acceleration of the chassis.

+ * + * @return the maximum linear acceleration + * @see org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig#maxAcceleration(Mass, int, Current) + */ + public LinearAcceleration maxLinearAcceleration(Current statorCurrentLimit) { + return moduleSimulations[0].config.maxAcceleration( + config.robotMass, moduleSimulations.length, statorCurrentLimit); + } + + /** + * + * + *

Obtains the drive base radius of the swerve drive.

+ * + * @return the drive base radius. + */ + public Distance driveBaseRadius() { + return config.driveBaseRadius(); + } + + /** + * + * + *

Obtains the maximum achievable angular velocity of the chassis.

+ * + * @return the maximum angular velocity + */ + public AngularVelocity maxAngularVelocity() { + return RadiansPerSecond.of(maxLinearVelocity().in(MetersPerSecond) + / config.driveBaseRadius().in(Meters)); + } + + /** + * + * + *

Obtains the maximum achievable angular acceleration of the chassis.

+ * + * @return the maximum angular acceleration + */ + public AngularAcceleration maxAngularAcceleration(Current statorCurrentLimit) { + return RadiansPerSecondPerSecond.of(moduleSimulations[0] + .config + .getTheoreticalPropellingForcePerModule( + config.robotMass, moduleSimulations.length, statorCurrentLimit) + .in(Newtons) + * moduleTranslations[0].getNorm() + * moduleSimulations.length + / super.getMass().getInertia()); + } + + public SwerveModuleSimulation[] getModules() { + return moduleSimulations; + } + + public GyroSimulation getGyroSimulation() { + return this.gyroSimulation; + } +} diff --git a/temp_yagsl/org/ironmaple/simulation/drivesims/SwerveModuleSimulation.java b/temp_yagsl/org/ironmaple/simulation/drivesims/SwerveModuleSimulation.java new file mode 100644 index 0000000..4608d07 --- /dev/null +++ b/temp_yagsl/org/ironmaple/simulation/drivesims/SwerveModuleSimulation.java @@ -0,0 +1,538 @@ +package org.ironmaple.simulation.drivesims; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.measure.*; +import java.util.Queue; +import java.util.concurrent.ConcurrentLinkedQueue; +import org.dyn4j.geometry.Vector2; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; +import org.ironmaple.simulation.motorsims.*; + +/** + * + * + *

Simulation for a Single Swerve Module.

+ * + *

Check Online + * Documentation + * + *

This class provides a simulation for a single swerve module in the {@link SwerveDriveSimulation}. + * + *

1. Purpose

+ * + *

This class serves as the bridge between your code and the physics engine. + * + *

You will apply voltage outputs to the drive/steer motor of the module and obtain their encoder readings in your + * code, just as how you deal with your physical motors. + * + *

2. Perspectives

+ * + * + * + *

3. Simulating Odometry

+ * + * + * + *

An example of how to simulate odometry using this class is the ModuleIOSim.java + * from the Advanced Swerve Drive with maple-sim example. + */ +public class SwerveModuleSimulation { + public final SwerveModuleSimulationConfig config; + + private final MapleMotorSim steerMotorSim; + + private Voltage driveMotorAppliedVoltage = Volts.zero(); + private Current driveMotorStatorCurrent = Amps.zero(); + private Angle driveWheelFinalPosition = Radians.zero(); + private AngularVelocity driveWheelFinalSpeed = RadiansPerSecond.zero(); + + private SimulatedMotorController driveMotorController; + + private final Angle steerRelativeEncoderOffSet = Radians.of((Math.random() - 0.5) * 30); + private final Queue driveWheelFinalPositionCache; + private final Queue steerAbsolutePositionCache; + + /** + * + * + *

Constructs a Swerve Module Simulation.

+ * + *

If you are using {@link SimulatedArena#overrideSimulationTimings(Time, int)} to use custom timings, you must + * call the method before constructing any swerve module simulations using this constructor. + * + * @param config the configuration + */ + public SwerveModuleSimulation(SwerveModuleSimulationConfig config) { + this.config = config; + + SimulatedBattery.addElectricalAppliances(this::getDriveMotorSupplyCurrent); + this.steerMotorSim = new MapleMotorSim(config.steerMotorConfigs); + + this.driveWheelFinalPositionCache = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedArena.getSimulationSubTicksIn1Period(); i++) + driveWheelFinalPositionCache.offer(driveWheelFinalPosition); + this.steerAbsolutePositionCache = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedArena.getSimulationSubTicksIn1Period(); i++) + steerAbsolutePositionCache.offer(getSteerAbsoluteFacing()); + + this.driveMotorController = new SimulatedMotorController.GenericMotorController(config.driveMotorConfigs.motor); + this.steerMotorSim.useSimpleDCMotorController(); + } + + public SimMotorConfigs getDriveMotorConfigs() { + return config.driveMotorConfigs; + } + + public SimMotorConfigs getSteerMotorConfigs() { + return steerMotorSim.getConfigs(); + } + + /** + * + * + *

Sets the motor controller for the drive motor.

+ * + *

The configured controller runs control loop on the motor. + * + * @param driveMotorController the motor controller to control the drive motor + */ + public T useDriveMotorController(T driveMotorController) { + this.driveMotorController = driveMotorController; + return driveMotorController; + } + + public SimulatedMotorController.GenericMotorController useGenericMotorControllerForDrive() { + return useDriveMotorController( + new SimulatedMotorController.GenericMotorController(config.driveMotorConfigs.motor)); + } + + /** + * + * + *

Requests the Steering Motor to Run at a Specified Output.

+ * + *

Think of it as the requestOutput() of your physical steering motor. + * + * @param steerMotorController the motor controller to control the steer motor + */ + public T useSteerMotorController(T steerMotorController) { + return this.steerMotorSim.useMotorController(steerMotorController); + } + + public SimulatedMotorController.GenericMotorController useGenericControllerForSteer() { + return this.steerMotorSim.useSimpleDCMotorController(); + } + + /** + * + * + *

Updates the Simulation for This Module.

+ * + *

Note: Friction forces are not simulated in this method. + * + * @param moduleCurrentGroundVelocityWorldRelative the current ground velocity of the module, relative to the world + * @param robotFacing the absolute facing of the robot, relative to the world + * @param gravityForceOnModuleNewtons the gravitational force acting on this module, in newtons + * @return the propelling force generated by the module, as a {@link Vector2} object + */ + public Vector2 updateSimulationSubTickGetModuleForce( + Vector2 moduleCurrentGroundVelocityWorldRelative, + Rotation2d robotFacing, + double gravityForceOnModuleNewtons) { + /* Step1: Update the steer mechanism simulation */ + steerMotorSim.update(SimulatedArena.getSimulationDt()); + + /* Step2: Simulate the amount of propelling force generated by the module. */ + final double grippingForceNewtons = config.getGrippingForceNewtons(gravityForceOnModuleNewtons); + final Rotation2d moduleWorldFacing = this.getSteerAbsoluteFacing().plus(robotFacing); + final Vector2 propellingForce = + getPropellingForce(grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative); + + /* Step3: Updates and caches the encoder readings for odometry simulation. */ + updateEncoderCaches(); + + return propellingForce; + } + + /** + * + * + *

Calculates the amount of propelling force that the module generates.

+ * + *

For most of the time, that propelling force is directly applied to the drivetrain. And the drive wheel runs as + * fast as the ground velocity + * + *

However, if the propelling force exceeds the gripping, only the max gripping force is applied. The rest of the + * propelling force will cause the wheel to start skidding and make the odometry inaccurate. + * + * @param grippingForceNewtons the amount of gripping force that wheel can generate, in newtons + * @param moduleWorldFacing the current world facing of the module + * @param moduleCurrentGroundVelocity the current ground velocity of the module, world-reference + * @return a vector representing the propelling force that the module generates, world-reference + */ + private Vector2 getPropellingForce( + double grippingForceNewtons, Rotation2d moduleWorldFacing, Vector2 moduleCurrentGroundVelocity) { + final double driveWheelTorque = getDriveWheelTorque(); + double propellingForceNewtons = driveWheelTorque / config.WHEEL_RADIUS.in(Meters); + final boolean skidding = Math.abs(propellingForceNewtons) > grippingForceNewtons; + if (skidding) propellingForceNewtons = Math.copySign(grippingForceNewtons, propellingForceNewtons); + + final double floorVelocityProjectionOnWheelDirectionMPS = moduleCurrentGroundVelocity.getMagnitude() + * Math.cos(moduleCurrentGroundVelocity.getAngleBetween(new Vector2(moduleWorldFacing.getRadians()))); + + // if the chassis is tightly gripped on floor, the floor velocity is projected to the wheel + this.driveWheelFinalSpeed = + RadiansPerSecond.of(floorVelocityProjectionOnWheelDirectionMPS / config.WHEEL_RADIUS.in(Meters)); + + // if the module is skidding + if (skidding) { + final AngularVelocity skiddingEquilibriumWheelSpeed = config.driveMotorConfigs.calculateMechanismVelocity( + config.driveMotorConfigs.calculateCurrent( + NewtonMeters.of(propellingForceNewtons * config.WHEEL_RADIUS.in(Meters))), + driveMotorAppliedVoltage); + this.driveWheelFinalSpeed = driveWheelFinalSpeed.times(0.5).plus(skiddingEquilibriumWheelSpeed.times(0.5)); + } + + return Vector2.create(propellingForceNewtons, moduleWorldFacing.getRadians()); + } + + /** + * + * + *

Calculates the amount of torque that the drive motor can generate on the wheel.

+ * + * @return the amount of torque on the wheel by the drive motor, in Newton * Meters + */ + private double getDriveWheelTorque() { + driveMotorAppliedVoltage = driveMotorController.updateControlSignal( + driveWheelFinalPosition, + driveWheelFinalSpeed, + getDriveEncoderUnGearedPosition(), + getDriveEncoderUnGearedSpeed()); + + driveMotorAppliedVoltage = SimulatedBattery.clamp(driveMotorAppliedVoltage); + + /* calculate the stator current */ + driveMotorStatorCurrent = + config.driveMotorConfigs.calculateCurrent(driveWheelFinalSpeed, driveMotorAppliedVoltage); + + /* calculate the torque generated */ + Torque driveWheelTorque = config.driveMotorConfigs.calculateTorque(driveMotorStatorCurrent); + + /* calculates the torque if you included losses from friction */ + Torque driveWheelTorqueWithFriction = NewtonMeters.of(MathUtil.applyDeadband( + driveWheelTorque.in(NewtonMeters), + config.driveMotorConfigs.friction.in(NewtonMeters), + Double.POSITIVE_INFINITY)); + return driveWheelTorqueWithFriction.in(NewtonMeters); + } + + /** @return the current module state of this simulation module */ + public SwerveModuleState getCurrentState() { + return new SwerveModuleState( + MetersPerSecond.of(getDriveWheelFinalSpeed().in(RadiansPerSecond) * config.WHEEL_RADIUS.in(Meters)), + getSteerAbsoluteFacing()); + } + + /** + * + * + *

Obtains the "free spin" state of the module

+ * + *

The "free spin" state of a simulated module refers to its state after spinning freely for a long time under + * the current input voltage + * + * @return the free spinning module state + */ + protected SwerveModuleState getFreeSpinState() { + return new SwerveModuleState( + config.driveMotorConfigs + .calculateMechanismVelocity( + config.driveMotorConfigs.calculateCurrent(config.driveMotorConfigs.friction), + driveMotorAppliedVoltage) + .in(RadiansPerSecond) + * config.WHEEL_RADIUS.in(Meters), + getSteerAbsoluteFacing()); + } + + /** + * + * + *

Cache the encoder values for high-frequency odometry.

+ * + *

An internal method to cache the encoder values to their queues. + */ + private void updateEncoderCaches() { + /* Increment of drive wheel position */ + this.driveWheelFinalPosition = + this.driveWheelFinalPosition.plus(this.driveWheelFinalSpeed.times(SimulatedArena.getSimulationDt())); + + /* cache sensor readings to queue for high-frequency odometry */ + this.steerAbsolutePositionCache.poll(); + this.steerAbsolutePositionCache.offer(getSteerAbsoluteFacing()); + + this.driveWheelFinalPositionCache.poll(); + this.driveWheelFinalPositionCache.offer(driveWheelFinalPosition); + } + + /** + * + * + *

Obtains the Actual Output Voltage of the Drive Motor.

+ * + * @return the actual output voltage of the drive motor + */ + public Voltage getDriveMotorAppliedVoltage() { + return driveMotorAppliedVoltage; + } + + /** + * + * + *

Obtains the Actual Output Voltage of the Steering Motor.

+ * + * @return the actual output voltage of the steering motor + * @see MapleMotorSim#getAppliedVoltage() + */ + public Voltage getSteerMotorAppliedVoltage() { + return steerMotorSim.getAppliedVoltage(); + } + + /** + * + * + *

Obtains the Amount of Current Supplied to the Drive Motor.

+ * + * @return the current supplied to the drive motor + */ + public Current getDriveMotorSupplyCurrent() { + return getDriveMotorStatorCurrent().times(driveMotorAppliedVoltage.div(SimulatedBattery.getBatteryVoltage())); + } + + /** + * + * + *

Obtains the Stator current the Drive Motor.

+ * + * @return the stator current of the drive motor + */ + public Current getDriveMotorStatorCurrent() { + return driveMotorStatorCurrent; + } + + /** + * + * + *

Obtains the Amount of Current Supplied to the Steer Motor.

+ * + * @return the current supplied to the steer motor + * @see MapleMotorSim#getSupplyCurrent() + */ + public Current getSteerMotorSupplyCurrent() { + return steerMotorSim.getSupplyCurrent(); + } + + /** + * + * + *

Obtains the Stator current the Steer Motor.

+ * + * @return the stator current of the drive motor + * @see MapleMotorSim#getSupplyCurrent() + */ + public Current getSteerMotorStatorCurrent() { + return steerMotorSim.getStatorCurrent(); + } + + /** + * + * + *

Obtains the Position of the Drive Encoder.

+ * + *

This value represents the un-geared position of the encoder, i.e., the amount of radians the drive motor's + * encoder has rotated. + * + * @return the position of the drive motor's encoder (un-geared) + */ + public Angle getDriveEncoderUnGearedPosition() { + return getDriveWheelFinalPosition().times(config.DRIVE_GEAR_RATIO); + } + + /** + * + * + *

Obtains the Final Position of the Wheel.

+ * + *

This method provides the final position of the drive encoder in terms of wheel angle. + * + * @return the final position of the drive encoder (wheel rotations) + */ + public Angle getDriveWheelFinalPosition() { + return driveWheelFinalPosition; + } + + /** + * + * + *

Obtains the Speed of the Drive Encoder.

+ * + * @return the un-geared speed of the drive encoder + */ + public AngularVelocity getDriveEncoderUnGearedSpeed() { + return getDriveWheelFinalSpeed().times(config.DRIVE_GEAR_RATIO); + } + + /** + * + * + *

Obtains the Final Speed of the Wheel.

+ * + * @return the final speed of the drive wheel + */ + public AngularVelocity getDriveWheelFinalSpeed() { + return driveWheelFinalSpeed; + } + + /** + * + * + *

Obtains the Relative Position of the Steer Encoder.

+ * + * @return the relative encoder position of the steer motor + * @see MapleMotorSim#getEncoderPosition() + */ + public Angle getSteerRelativeEncoderPosition() { + return getSteerAbsoluteFacing() + .getMeasure() + .times(config.STEER_GEAR_RATIO) + .plus(steerRelativeEncoderOffSet); + } + + /** + * + * + *

Obtains the Speed of the Steer Relative Encoder (Geared).

+ * + * @return the speed of the steer relative encoder + * @see MapleMotorSim#getEncoderVelocity() + */ + public AngularVelocity getSteerRelativeEncoderVelocity() { + return getSteerAbsoluteEncoderSpeed().times(config.STEER_GEAR_RATIO); + } + + /** + * + * + *

Obtains the Absolute Facing of the Steer Mechanism.

+ * + * @return the absolute facing of the steer mechanism, as a {@link Rotation2d} + */ + public Rotation2d getSteerAbsoluteFacing() { + return new Rotation2d(getSteerAbsoluteAngle()); + } + + /** + * + * + *

Obtains the Absolute Angle of the Steer Mechanism.

+ * + * @return the (continuous) final angle of the steer mechanism, as a {@link Angle} + * @see MapleMotorSim#getAngularPosition() + */ + public Angle getSteerAbsoluteAngle() { + return steerMotorSim.getAngularPosition(); + } + + /** + * + * + *

Obtains the Absolute Rotational Velocity of the Steer Mechanism.

+ * + * @return the absolute angular velocity of the steer mechanism + */ + public AngularVelocity getSteerAbsoluteEncoderSpeed() { + return steerMotorSim.getVelocity(); + } + + /** + * + * + *

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

+ * + *

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 + */ + public Angle[] getCachedDriveEncoderUnGearedPositions() { + return driveWheelFinalPositionCache.stream() + .map(value -> value.times(config.DRIVE_GEAR_RATIO)) + .toArray(Angle[]::new); + } + + /** + * + * + *

Obtains the Cached Readings of the Drive Encoder's Final Position (Wheel Rotations).

+ * + *

The values of {@link #getDriveWheelFinalPosition()} are cached at each sub-tick and are divided by the gear + * ratio to obtain the final wheel rotations. + * + * @return an array of cached drive encoder final positions (wheel rotations) + */ + public Angle[] getCachedDriveWheelFinalPositions() { + return driveWheelFinalPositionCache.toArray(Angle[]::new); + } + + /** + * + * + *

Obtains the Cached Readings of the Steer Relative Encoder's Position.

+ * + *

The values of {@link #getSteerRelativeEncoderPosition()} are cached at each sub-tick and can be retrieved + * using this method. + * + * @return an array of cached steer relative encoder positions + */ + public Angle[] getCachedSteerRelativeEncoderPositions() { + return steerAbsolutePositionCache.stream() + .map(absoluteFacing -> absoluteFacing + .getMeasure() + .times(config.STEER_GEAR_RATIO) + .plus(steerRelativeEncoderOffSet)) + .toArray(Angle[]::new); + } + + /** + * + * + *

Obtains the Cached Readings of the Steer Absolute Positions.

+ * + *

The values of {@link #getSteerAbsoluteFacing()} are cached at each sub-tick and can be retrieved using this + * method. + * + * @return an array of cached absolute steer positions, as {@link Rotation2d} objects + */ + public Rotation2d[] getCachedSteerAbsolutePositions() { + return steerAbsolutePositionCache.toArray(Rotation2d[]::new); + } +} diff --git a/temp_yagsl/org/ironmaple/simulation/drivesims/configs/DriveTrainSimulationConfig.java b/temp_yagsl/org/ironmaple/simulation/drivesims/configs/DriveTrainSimulationConfig.java new file mode 100644 index 0000000..c8cf906 --- /dev/null +++ b/temp_yagsl/org/ironmaple/simulation/drivesims/configs/DriveTrainSimulationConfig.java @@ -0,0 +1,323 @@ +package org.ironmaple.simulation.drivesims.configs; + +import static edu.wpi.first.units.Units.Kilograms; +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Mass; +import java.util.Arrays; +import java.util.OptionalDouble; +import java.util.function.Supplier; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.GyroSimulation; +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; + +/** + * + * + *

Stores the configurations for a swerve drive simulation.

+ * + *

This class is used to hold all the parameters necessary for simulating a swerve drivetrain, allowing for realistic + * performance testing and evaluation. + */ +public class DriveTrainSimulationConfig { + public Mass robotMass; + public Distance bumperLengthX, bumperWidthY; + public Supplier[] swerveModuleSimulationFactories; + public Supplier gyroSimulationFactory; + public Translation2d[] moduleTranslations; + + /** + * + * + *

Ordinary Constructor

+ * + *

Creates an instance of {@link DriveTrainSimulationConfig} with specified parameters. + * + * @param robotMass the mass of the robot, including bumpers. + * @param bumperLengthX the length of the bumper (distance from front to back). + * @param bumperWidthY the width of the bumper (distance from left to right). + * @param trackLengthX the distance between the front and rear wheels. + * @param trackWidthY the distance between the left and right wheels. + * @param swerveModuleSimulationFactory the factory that creates appropriate swerve module simulation for the + * drivetrain. You can specify one factory to apply the same configuration over all modules or specify four + * factories in the order (FL, FR, BL, BR). + * @param gyroSimulationFactory the factory that creates appropriate gyro simulation for the drivetrain. + */ + public DriveTrainSimulationConfig( + Mass robotMass, + Distance bumperLengthX, + Distance bumperWidthY, + Distance trackLengthX, + Distance trackWidthY, + Supplier gyroSimulationFactory, + Supplier... swerveModuleSimulationFactory) { + this.robotMass = robotMass; + this.bumperLengthX = bumperLengthX; + this.bumperWidthY = bumperWidthY; + this.withTrackLengthTrackWidth(trackLengthX, trackWidthY); + + if (swerveModuleSimulationFactory.length == 1) this.withSwerveModule(swerveModuleSimulationFactory[0]); + else if (swerveModuleSimulationFactory.length == 4) this.withSwerveModules(swerveModuleSimulationFactory); + else + throw new IllegalArgumentException("Module simulation factories length must be 1 or 4, provided " + + swerveModuleSimulationFactory.length); + this.gyroSimulationFactory = gyroSimulationFactory; + + checkRobotMass(); + checkBumperSize(); + } + + /** + * + * + *

Default Constructor.

+ * + *

Creates a {@link DriveTrainSimulationConfig} with all the data set to default values. + * + *

Though the config starts with default values, any configuration can be modified after creation. + * + *

The default configurations are: + * + *

+ * + * @return a new instance of {@link DriveTrainSimulationConfig} with all configs set to default values. + */ + public static DriveTrainSimulationConfig Default() { + return new DriveTrainSimulationConfig( + Kilograms.of(45), + Meters.of(0.76), + Meters.of(.76), + Meters.of(0.52), + Meters.of(0.52), + COTS.ofPigeon2(), + COTS.ofMark4(DCMotor.getFalcon500(1), DCMotor.getFalcon500(1), COTS.WHEELS.COLSONS.cof, 2)); + } + + /** + * + * + *

Sets the robot mass.

+ * + *

Updates the mass of the robot in kilograms. + * + * @param robotMass the new mass of the robot. + * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining. + */ + public DriveTrainSimulationConfig withRobotMass(Mass robotMass) { + this.robotMass = robotMass; + checkRobotMass(); + return this; + } + + /** + * + * + *

Sets the bumper size.

+ * + *

Updates the dimensions of the bumper. + * + * @param bumperLengthX the length of the bumper. + * @param bumperWidthY the width of the bumper. + * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining. + */ + public DriveTrainSimulationConfig withBumperSize(Distance bumperLengthX, Distance bumperWidthY) { + this.bumperLengthX = bumperLengthX; + this.bumperWidthY = bumperWidthY; + + checkBumperSize(); + return this; + } + + /** + * + * + *

Sets the track length and width.

+ * + *

Updates the translations for the swerve modules based on the specified track length and track width. + * + *

For non-rectangular chassis configuration, use {@link #withCustomModuleTranslations(Translation2d[])} instead. + * + * @param trackLengthX the distance between the front and rear wheels. + * @param trackWidthY the distance between the left and right wheels. + * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining. + */ + public DriveTrainSimulationConfig withTrackLengthTrackWidth(Distance trackLengthX, Distance trackWidthY) { + BoundingCheck.check(trackLengthX.in(Meters), 0.5, 1.5, "track length", "meters"); + BoundingCheck.check(trackWidthY.in(Meters), 0.5, 1.5, "track width", "meters"); + + this.moduleTranslations = new Translation2d[] { + new Translation2d(trackLengthX.in(Meters) / 2, trackWidthY.in(Meters) / 2), + new Translation2d(trackLengthX.in(Meters) / 2, -trackWidthY.in(Meters) / 2), + new Translation2d(-trackLengthX.in(Meters) / 2, trackWidthY.in(Meters) / 2), + new Translation2d(-trackLengthX.in(Meters) / 2, -trackWidthY.in(Meters) / 2) + }; + return this; + } + + /** + * + * + *

Sets custom module translations.

+ * + *

Updates the translations of the swerve modules with user-defined values. + * + *

For ordinary rectangular modules configuration, use {@link #withTrackLengthTrackWidth(Distance, Distance)} + * instead. + * + * @param moduleTranslations the custom translations for the swerve modules. + * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining. + */ + public DriveTrainSimulationConfig withCustomModuleTranslations(Translation2d[] moduleTranslations) { + checkModuleTranslations(); + this.moduleTranslations = moduleTranslations; + return this; + } + + /** + * + * + *

Sets the swerve module simulation factory.

+ * + *

Updates the factory used to create swerve module simulations. + * + * @param swerveModuleSimulationFactory the new factory (or factories) for swerve module simulations. You can + * specify one factory to apply the same configuration over all modules, or specify four factories in the order + * (FL, FR, BL, BR) + * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining. + */ + public DriveTrainSimulationConfig withSwerveModules( + Supplier... swerveModuleSimulationFactory) { + if (swerveModuleSimulationFactory.length == 1) return withSwerveModule(swerveModuleSimulationFactory[0]); + + if (swerveModuleSimulationFactory.length != moduleTranslations.length) + throw new IllegalArgumentException("Module simulation factories length must be 1 or 4, provided " + + swerveModuleSimulationFactory.length); + + this.swerveModuleSimulationFactories = swerveModuleSimulationFactory; + return this; + } + + /** + * + * + *

Sets the swerve module simulation factory.

+ * + *

Updates the factory used to create swerve module simulations. + * + *

Uses the same configuration over all the modules + * + * @param swerveModuleSimulationFactory the new factory for swerve module simulations. + * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining. + */ + public DriveTrainSimulationConfig withSwerveModule(Supplier swerveModuleSimulationFactory) { + this.swerveModuleSimulationFactories = new Supplier[moduleTranslations.length]; + Arrays.fill(this.swerveModuleSimulationFactories, swerveModuleSimulationFactory); + return this; + } + + /** + * + * + *

Sets the gyro simulation factory.

+ * + *

Updates the factory used to create gyro simulations. + * + * @param gyroSimulationFactory the new factory for gyro simulations. + * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining. + */ + public DriveTrainSimulationConfig withGyro(Supplier gyroSimulationFactory) { + this.gyroSimulationFactory = gyroSimulationFactory; + return this; + } + + /** + * + * + *

Calculates the density of the robot.

+ * + *

Returns the density of the robot based on its mass and bumper dimensions. + * + * @return the density in kilograms per square meter. + */ + public double getDensityKgPerSquaredMeters() { + return robotMass.in(Kilograms) / (bumperLengthX.in(Meters) * bumperWidthY.in(Meters)); + } + + /** + * + * + *

Calculates the track length in the X direction.

+ * + *

Returns the total distance between the frontmost and rearmost module translations in the X direction. + * + * @return the track length. + * @throws IllegalStateException if the module translations are empty. + */ + public Distance trackLengthX() { + final OptionalDouble maxModuleX = Arrays.stream(moduleTranslations) + .mapToDouble(Translation2d::getX) + .max(); + final OptionalDouble minModuleX = Arrays.stream(moduleTranslations) + .mapToDouble(Translation2d::getX) + .min(); + if (maxModuleX.isEmpty() || minModuleX.isEmpty()) + throw new IllegalStateException("Modules translations are empty"); + return Meters.of(maxModuleX.getAsDouble() - minModuleX.getAsDouble()); + } + + /** + * + * + *

Calculates the track width in the Y direction.

+ * + *

Returns the total distance between the leftmost and rightmost module translations in the Y direction. + * + * @return the track width. + * @throws IllegalStateException if the module translations are empty. + */ + public Distance trackWidthY() { + final OptionalDouble maxModuleY = Arrays.stream(moduleTranslations) + .mapToDouble(Translation2d::getY) + .max(); + final OptionalDouble minModuleY = Arrays.stream(moduleTranslations) + .mapToDouble(Translation2d::getY) + .min(); + if (maxModuleY.isEmpty() || minModuleY.isEmpty()) + throw new IllegalStateException("Modules translations are empty"); + return Meters.of(maxModuleY.getAsDouble() - minModuleY.getAsDouble()); + } + + public Distance driveBaseRadius() { + return Meters.of(Math.hypot(trackLengthX().in(Meters), trackWidthY().in(Meters))); + } + + private void checkRobotMass() { + BoundingCheck.check(robotMass.in(Kilograms), 10, 80, "robot mass", "kg"); + } + + private void checkBumperSize() { + BoundingCheck.check(bumperLengthX.in(Meters), 0.5, 1.5, "bumper length", "meters"); + BoundingCheck.check(bumperWidthY.in(Meters), 0.5, 1.5, "bumper width", "meters"); + } + + private void checkModuleTranslations() { + for (int i = 0; i < moduleTranslations.length; i++) + BoundingCheck.check( + moduleTranslations[i].getNorm(), + 0.2, + 1.2, + "module number " + i + " translation magnitude", + "meters"); + } +} diff --git a/temp_yagsl/org/ironmaple/simulation/drivesims/configs/SwerveModuleSimulationConfig.java b/temp_yagsl/org/ironmaple/simulation/drivesims/configs/SwerveModuleSimulationConfig.java new file mode 100644 index 0000000..eb2b414 --- /dev/null +++ b/temp_yagsl/org/ironmaple/simulation/drivesims/configs/SwerveModuleSimulationConfig.java @@ -0,0 +1,128 @@ +package org.ironmaple.simulation.drivesims.configs; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.*; +import java.util.function.Supplier; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.motorsims.SimMotorConfigs; + +public class SwerveModuleSimulationConfig implements Supplier { + public final SimMotorConfigs driveMotorConfigs, steerMotorConfigs; + public final double DRIVE_GEAR_RATIO, STEER_GEAR_RATIO, WHEELS_COEFFICIENT_OF_FRICTION; + public final Voltage DRIVE_FRICTION_VOLTAGE; + public final Distance WHEEL_RADIUS; + + /** + * + * + *

Constructs a Configuration for Swerve Module Simulation.

+ * + *

If you are using {@link SimulatedArena#overrideSimulationTimings(Time, int)} to use custom timings, you must + * call the method before constructing any swerve module simulations using this constructor. + * + * @param driveMotorModel the model of the driving motor + * @param steerMotorModel; the model of the steering motor + * @param driveGearRatio the gear ratio for the driving motor, >1 is reduction + * @param steerGearRatio the gear ratio for the steering motor, >1 is reduction + * @param driveFrictionVoltage the measured minimum amount of voltage that can turn the driving rotter + * @param steerFrictionVoltage the measured minimum amount of voltage that can turn the steering rotter + * @param wheelRadius the radius of the wheels. + * @param steerRotationalInertia the rotational inertia of the entire steering mechanism + * @param wheelsCoefficientOfFriction the coefficient + * of friction of the tires, normally around 1.2 {@link Units#inchesToMeters(double)}. + */ + public SwerveModuleSimulationConfig( + DCMotor driveMotorModel, + DCMotor steerMotorModel, + double driveGearRatio, + double steerGearRatio, + Voltage driveFrictionVoltage, + Voltage steerFrictionVoltage, + Distance wheelRadius, + MomentOfInertia steerRotationalInertia, + double wheelsCoefficientOfFriction) { + BoundingCheck.check(driveGearRatio, 4, 24, "drive gear ratio", "times reduction"); + BoundingCheck.check(steerGearRatio, 6, 50, "steer gear ratio", "times reduction"); + BoundingCheck.check(driveFrictionVoltage.in(Volts), 0.01, 0.35, "drive friction voltage", "volts"); + BoundingCheck.check(steerFrictionVoltage.in(Volts), 0.01, 0.6, "steer friction voltage", "volts"); + BoundingCheck.check(wheelRadius.in(Inches), 1, 3.2, "drive wheel radius", "inches"); + BoundingCheck.check( + steerRotationalInertia.in(KilogramSquareMeters), 0.005, 0.06, "steer rotation inertia", "kg * m^2"); + BoundingCheck.check(wheelsCoefficientOfFriction, 0.6, 1.9, "tire coefficient of friction", ""); + + this.driveMotorConfigs = + new SimMotorConfigs(driveMotorModel, driveGearRatio, KilogramSquareMeters.zero(), driveFrictionVoltage); + this.steerMotorConfigs = + new SimMotorConfigs(steerMotorModel, steerGearRatio, steerRotationalInertia, steerFrictionVoltage); + DRIVE_GEAR_RATIO = driveGearRatio; + STEER_GEAR_RATIO = steerGearRatio; + WHEELS_COEFFICIENT_OF_FRICTION = wheelsCoefficientOfFriction; + DRIVE_FRICTION_VOLTAGE = driveFrictionVoltage; + WHEEL_RADIUS = wheelRadius; + } + + @Override + public SwerveModuleSimulation get() { + return new SwerveModuleSimulation(this); + } + + public double getGrippingForceNewtons(double gravityForceOnModuleNewtons) { + return gravityForceOnModuleNewtons * WHEELS_COEFFICIENT_OF_FRICTION; + } + + /** + * + * + *

Obtains the theoretical speed that the module can achieve.

+ * + * @return the theoretical maximum ground speed that the module can achieve, in m/s + */ + public LinearVelocity maximumGroundSpeed() { + return MetersPerSecond.of( + driveMotorConfigs.freeSpinMechanismVelocity().in(RadiansPerSecond) * WHEEL_RADIUS.in(Meters)); + } + + /** + * + * + *

Obtains the theoretical maximum propelling force of ONE module.

+ * + *

Calculates the maximum propelling force with respect to the gripping force and the drive motor's torque under + * its current limit. + * + * @param robotMass the mass of the robot + * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally + * @return the maximum propelling force of EACH module + */ + public Force getTheoreticalPropellingForcePerModule(Mass robotMass, int modulesCount, Current statorCurrentLimit) { + final double + maxThrustNewtons = + driveMotorConfigs.calculateTorque(statorCurrentLimit).in(NewtonMeters) + / WHEEL_RADIUS.in(Meters), + maxGrippingNewtons = 9.8 * robotMass.in(Kilograms) / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION; + + return Newtons.of(Math.min(maxThrustNewtons, maxGrippingNewtons)); + } + + /** + * + * + *

Obtains the theatrical linear acceleration that the robot can achieve.

+ * + *

Calculates the maximum linear acceleration of a robot, with respect to its mass and + * {@link #getTheoreticalPropellingForcePerModule(Mass, int, Current)}. + * + * @param robotMass the mass of the robot + * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally + */ + public LinearAcceleration maxAcceleration(Mass robotMass, int modulesCount, Current statorCurrentLimit) { + return getTheoreticalPropellingForcePerModule(robotMass, modulesCount, statorCurrentLimit) + .times(modulesCount) + .div(robotMass); + } +} diff --git a/temp_yagsl/org/ironmaple/simulation/motorsims/MapleMotorSim.java b/temp_yagsl/org/ironmaple/simulation/motorsims/MapleMotorSim.java new file mode 100644 index 0000000..7cf9a48 --- /dev/null +++ b/temp_yagsl/org/ironmaple/simulation/motorsims/MapleMotorSim.java @@ -0,0 +1,194 @@ +package org.ironmaple.simulation.motorsims; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.units.measure.*; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +/** + * + * + *

{@link edu.wpi.first.wpilibj.simulation.DCMotorSim} with a bit of extra spice.

+ * + *

This class extends the functionality of the original {@link edu.wpi.first.wpilibj.simulation.DCMotorSim} and + * models the following aspects in addition: + * + *

+ */ +public class MapleMotorSim { + private final SimMotorConfigs configs; + + private SimMotorState state; + private SimulatedMotorController controller; + private Voltage appliedVoltage; + private Current statorCurrent; + + /** + * + * + *

Constructs a Brushless Motor Simulation Instance.

+ * + * @param configs the configuration for this motor + */ + public MapleMotorSim(SimMotorConfigs configs) { + this.configs = configs; + this.state = new SimMotorState(Radians.zero(), RadiansPerSecond.zero()); + this.controller = (mechanismAngle, mechanismVelocity, encoderAngle, encoderVelocity) -> Volts.of(0); + this.appliedVoltage = Volts.zero(); + this.statorCurrent = Amps.zero(); + + SimulatedBattery.addMotor(this); + } + + /** + * + * + *

Updates the simulation.

+ * + *

This is equivalent to{@link edu.wpi.first.wpilibj.simulation.DCMotorSim#update(double)}. + */ + public void update(Time dt) { + this.appliedVoltage = controller.updateControlSignal( + state.mechanismAngularPosition, + state.mechanismAngularVelocity, + state.mechanismAngularPosition.times(configs.gearing), + state.mechanismAngularVelocity.times(configs.gearing)); + this.appliedVoltage = SimulatedBattery.clamp(appliedVoltage); + this.statorCurrent = configs.calculateCurrent(state.mechanismAngularVelocity, appliedVoltage); + this.state.step(configs.calculateTorque(statorCurrent), configs.friction, configs.loadMOI, dt); + + if (state.mechanismAngularPosition.lte(configs.reverseHardwareLimit)) + state = new SimMotorState(configs.reverseHardwareLimit, RadiansPerSecond.zero()); + else if (state.mechanismAngularPosition.gte(configs.forwardHardwareLimit)) + state = new SimMotorState(configs.forwardHardwareLimit, RadiansPerSecond.zero()); + } + + public T useMotorController(T motorController) { + this.controller = motorController; + return motorController; + } + + public SimulatedMotorController.GenericMotorController useSimpleDCMotorController() { + return useMotorController(new SimulatedMotorController.GenericMotorController(configs.motor)); + } + + /** + * + * + *

Obtains the final position of the mechanism.

+ * + *

This is equivalent to {@link edu.wpi.first.wpilibj.simulation.DCMotorSim#getAngularPosition()}. + * + * @return the angular position of the mechanism, continuous + */ + public Angle getAngularPosition() { + return state.mechanismAngularPosition; + } + + /** + * + * + *

Obtains the angular position measured by the relative encoder of the motor.

+ * + * @return the angular position measured by the encoder, continuous + */ + public Angle getEncoderPosition() { + return getAngularPosition().times(configs.gearing); + } + + /** + * + * + *

Obtains the final velocity of the mechanism.

+ * + *

This is equivalent to {@link edu.wpi.first.wpilibj.simulation.DCMotorSim#getAngularVelocity()}. + * + * @return the final angular velocity of the mechanism + */ + public AngularVelocity getVelocity() { + return state.mechanismAngularVelocity; + } + + /** + * + * + *

Obtains the angular velocity measured by the relative encoder of the motor.

+ * + * @return the angular velocity measured by the encoder + */ + public AngularVelocity getEncoderVelocity() { + return getVelocity().times(configs.gearing); + } + + /** + * + * + *

Obtains the applied voltage by the motor controller.

+ * + *

The applied voltage is calculated by the motor controller in the previous call to {@link #update(Time)} + * + *

The motor controller specified by {@link #useMotorController(SimulatedMotorController)} is used to calculate + * the applied voltage. + * + *

The applied voltage is also restricted for current limit and battery voltage. + * + * @return the applied voltage + */ + public Voltage getAppliedVoltage() { + return appliedVoltage; + } + + /** + * + * + *

Obtains the stator current.

+ * + *

This is equivalent to {@link DCMotorSim#getCurrentDrawAmps()} + * + * @return the stator current of the motor + */ + public Current getStatorCurrent() { + return statorCurrent; + } + + /** + * + * + *

Obtains the supply current.

+ * + *

The supply current is different from the stator current, as described here. + * + * @return the supply current of the motor + */ + public Current getSupplyCurrent() { + // Supply Power = Stator Power (Conservation of Energy) + // Hence, + // Battery Voltage x Supply Current = Applied Voltage x Stator Current + // Supply Current = Stator Current * Applied Voltage / Battery Voltage + return getStatorCurrent().times(appliedVoltage.div(SimulatedBattery.getBatteryVoltage())); + } + + /** + * + * + *

Obtains the configuration of the motor.

+ * + *

You can modify the configuration of this motor by: + * + * 


+     *     mapleMotorSim.getConfigs()
+     *          .with...(...)
+     *          .with...(...);
+     *
+ * + * @return the configuration of the motor + */ + public SimMotorConfigs getConfigs() { + return this.configs; + } +} diff --git a/temp_yagsl/org/ironmaple/simulation/motorsims/SimulatedMotorController.java b/temp_yagsl/org/ironmaple/simulation/motorsims/SimulatedMotorController.java new file mode 100644 index 0000000..a22c0ff --- /dev/null +++ b/temp_yagsl/org/ironmaple/simulation/motorsims/SimulatedMotorController.java @@ -0,0 +1,108 @@ +package org.ironmaple.simulation.motorsims; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + +public interface SimulatedMotorController { + Voltage updateControlSignal( + Angle mechanismAngle, + AngularVelocity mechanismVelocity, + Angle encoderAngle, + AngularVelocity encoderVelocity); + + final class GenericMotorController implements SimulatedMotorController { + private final DCMotor model; + private Current currentLimit = Amps.of(150); + private Angle forwardSoftwareLimit = Radians.of(Double.POSITIVE_INFINITY), + reverseSoftwareLimit = Radians.of(-Double.POSITIVE_INFINITY); + + private Voltage requestedVoltage = Volts.zero(); + private Voltage appliedVoltage = Volts.zero(); + + public GenericMotorController(DCMotor model) { + this.model = model; + } + + public GenericMotorController withCurrentLimit(Current currentLimit) { + this.currentLimit = currentLimit; + return this; + } + + public GenericMotorController withSoftwareLimits(Angle forwardSoftwareLimit, Angle reverseSoftwareLimit) { + this.forwardSoftwareLimit = forwardSoftwareLimit; + this.reverseSoftwareLimit = reverseSoftwareLimit; + return this; + } + + public void requestVoltage(Voltage voltage) { + this.requestedVoltage = voltage; + } + + /** + * + * + *

(Utility Function) Constrains the Output Voltage of a Motor.

+ * + *

Constrains the output voltage of a motor such that the stator current does not exceed the + * current limit + * + *

Prevents motor from exceeding software limits + * + * @param encoderAngle the angle of the encoder + * @param encoderVelocity the velocity of the encoder + * @param requestedVoltage the requested voltage + * @return the constrained voltage that satisfied the limits + */ + public Voltage constrainOutputVoltage( + Angle encoderAngle, AngularVelocity encoderVelocity, Voltage requestedVoltage) { + final double kCurrentThreshold = 1.2; + + // don't use WpiLib Units for calculations + final double motorCurrentVelocityRadPerSec = encoderVelocity.in(RadiansPerSecond); + final double currentLimitAmps = currentLimit.in(Amps); + final double requestedOutputVoltageVolts = requestedVoltage.in(Volts); + final double currentAtRequestedVoltageAmps = + model.getCurrent(motorCurrentVelocityRadPerSec, requestedOutputVoltageVolts); + + // Resource for current limiting: + // https://file.tavsys.net/control/controls-engineering-in-frc.pdf (sec 12.1.3) + double limitedVoltage = requestedOutputVoltageVolts; + final boolean currentTooHigh = + Math.abs(currentAtRequestedVoltageAmps) > (kCurrentThreshold * currentLimitAmps); + if (currentTooHigh) { + final double limitedCurrent = Math.copySign(currentLimitAmps, currentAtRequestedVoltageAmps); + limitedVoltage = model.getVoltage(model.getTorque(limitedCurrent), motorCurrentVelocityRadPerSec); + } + + // ensure the current limit doesn't cause an increase to output voltage + if (Math.abs(limitedVoltage) > Math.abs(requestedOutputVoltageVolts)) + limitedVoltage = requestedOutputVoltageVolts; + + // apply software limits + if (encoderAngle.gte(forwardSoftwareLimit) && limitedVoltage > 0) limitedVoltage = 0; + if (encoderAngle.lte(reverseSoftwareLimit) && limitedVoltage < 0) limitedVoltage = 0; + + // constrain the output voltage to the battery voltage + return Volts.of(limitedVoltage); + } + + @Override + public Voltage updateControlSignal( + Angle mechanismAngle, + AngularVelocity mechanismVelocity, + Angle encoderAngle, + AngularVelocity encoderVelocity) { + appliedVoltage = constrainOutputVoltage(encoderAngle, encoderVelocity, requestedVoltage); + return appliedVoltage; + } + + public Voltage getAppliedVoltage() { + return appliedVoltage; + } + } +} diff --git a/temp_yagsl/org/ironmaple/utils/mathutils/SwerveStateProjection.java b/temp_yagsl/org/ironmaple/utils/mathutils/SwerveStateProjection.java new file mode 100644 index 0000000..5310f62 --- /dev/null +++ b/temp_yagsl/org/ironmaple/utils/mathutils/SwerveStateProjection.java @@ -0,0 +1,26 @@ +// Created by Team 5516 https://github.com/Shenzhen-Robotics-Alliance/ using ChatGPT4o +package org.ironmaple.utils.mathutils; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; + +public class SwerveStateProjection { + /** + * Projects the swerve module speed onto the direction of the current swerve facing. + * + * @param swerveSpeed The current speed and direction of the swerve module. + * @param currentSwerveFacing The desired direction to project onto. + * @return The projected speed in the direction of currentSwerveFacing. + */ + public static double project(SwerveModuleState swerveSpeed, Rotation2d currentSwerveFacing) { + // Get the angle of the swerve module's current direction + Rotation2d swerveModuleAngle = swerveSpeed.angle; + + // Calculate the cosine of the angle difference between swerve module direction and the desired + // direction + double cosTheta = Math.cos(swerveModuleAngle.minus(currentSwerveFacing).getRadians()); + + // Scale the speed by the cosine value to get the projection + return swerveSpeed.speedMetersPerSecond * cosTheta; + } +} diff --git a/temp_yagsl/swervelib/SwerveDrive.java b/temp_yagsl/swervelib/SwerveDrive.java new file mode 100644 index 0000000..ddccf84 --- /dev/null +++ b/temp_yagsl/swervelib/SwerveDrive.java @@ -0,0 +1,1614 @@ +package swervelib; + +import static edu.wpi.first.hal.FRCNetComm.tInstances.kRobotDriveSwerve_YAGSL; +import static edu.wpi.first.hal.FRCNetComm.tResourceType.kResourceType_RobotDrive; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Kilograms; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Newtons; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Force; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Optional; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; +import swervelib.encoders.CANCoderSwerve; +import swervelib.imu.Pigeon2Swerve; +import swervelib.imu.SwerveIMU; +import swervelib.math.SwerveMath; +import swervelib.motors.TalonFXSwerve; +import swervelib.parser.Cache; +import swervelib.parser.SwerveControllerConfiguration; +import swervelib.parser.SwerveDriveConfiguration; +import swervelib.simulation.SwerveIMUSimulation; +import swervelib.telemetry.SwerveDriveTelemetry; +import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; + +/** + * Swerve Drive class representing and controlling the swerve drive. + */ +public class SwerveDrive implements AutoCloseable +{ + + /** + * Swerve Kinematics object. + */ + public final SwerveDriveKinematics kinematics; + /** + * Swerve drive configuration. + */ + public final SwerveDriveConfiguration swerveDriveConfiguration; + /** + * Swerve odometry. + */ + public final SwerveDrivePoseEstimator swerveDrivePoseEstimator; + /** + * IMU reading cache for robot readings. + */ + public final Cache imuReadingCache; + /** + * Swerve modules. + */ + private final SwerveModule[] swerveModules; + /** + * WPILib {@link Notifier} to keep odometry up to date. + */ + private final Notifier odometryThread; + /** + * Odometry lock to ensure thread safety. + */ + private final Lock odometryLock = new ReentrantLock(); + /** + * Alert to recommend Tuner X if the configuration is compatible. + */ + private final Alert tunerXRecommendation = new Alert("Swerve Drive", + "Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" + + "https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html", + AlertType.kWarning); + /** + * NT4 Publisher for the IMU reading. + */ + private final DoublePublisher rawIMUPublisher + = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") + .getDoubleTopic( + "swerve/imu/raw") + .publish(); + /** + * NT4 Publisher for the IMU reading adjusted by offset and inversion. + */ + private final DoublePublisher adjustedIMUPublisher + = NetworkTableInstance.getDefault() + .getTable( + "SmartDashboard") + .getDoubleTopic( + "swerve/imu/adjusted") + .publish(); + /** + * Field object. + */ + public Field2d field = new Field2d(); + /** + * Swerve controller for controlling heading of the robot. + */ + public SwerveController swerveController; + /** + * Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's + * correction. + */ + public boolean chassisVelocityCorrection = true; + /** + * Correct chassis velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} (auto) using 254's + * correction during auto. + */ + public boolean autonomousChassisVelocityCorrection = false; + /** + * Correct for skew that scales with angular velocity in + * {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} + */ + public boolean angularVelocityCorrection = false; + /** + * Correct for skew that scales with angular velocity in + * {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} during auto. + */ + public boolean autonomousAngularVelocityCorrection = false; + /** + * Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15). + */ + public double angularVelocityCoefficient = 0; + /** + * Whether to correct heading when driving translationally. Set to true to enable. + */ + public boolean headingCorrection = false; + /** + * MapleSim SwerveDrive. + */ + private SwerveDriveSimulation mapleSimDrive; + /** + * Amount of seconds the duration of the timestep the speeds should be applied for. + */ + private double discretizationdtSeconds = 0.02; + /** + * Deadband for speeds in heading correction. + */ + private double HEADING_CORRECTION_DEADBAND = 0.01; + /** + * Swerve IMU device for sensing the heading of the robot. + */ + private SwerveIMU imu; + /** + * Simulation of the swerve drive. + */ + private SwerveIMUSimulation simIMU; + /** + * Counter to synchronize the modules relative encoder with absolute encoder when not moving. + */ + private int moduleSynchronizationCounter = 0; + /** + * The last heading set in radians. + */ + private double lastHeadingRadians = 0; + /** + * The absolute max speed that your robot can reach while translating in meters per second. + */ + private double attainableMaxTranslationalSpeedMetersPerSecond = 0; + /** + * The absolute max speed the robot can reach while rotating radians per second. + */ + private double attainableMaxRotationalVelocityRadiansPerSecond = 0; + /** + * Maximum speed of the robot in meters per second. + */ + private double maxChassisSpeedMPS; + + /** + * Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the + * {@link SwerveDrive#setRawModuleStates} method. The {@link SwerveDrive#drive} method incorporates kinematics-- it + * takes a translation and rotation, as well as parameters for field-centric and closed-loop velocity control. + * {@link SwerveDrive#setRawModuleStates} takes a list of SwerveModuleStates and directly passes them to the modules. + * This subsystem also handles odometry. + * + * @param config The {@link SwerveDriveConfiguration} configuration to base the swerve drive off of. + * @param controllerConfig The {@link SwerveControllerConfiguration} to use when creating the + * {@link SwerveController}. + * @param maxSpeedMPS Maximum speed of the robot in meters per second, remember to use + * {@link Units#feetToMeters(double)} if you have feet per second! + * @param startingPose Starting {@link Pose2d} on the field. + */ + public SwerveDrive( + SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS, + Pose2d startingPose) + { + this.attainableMaxTranslationalSpeedMetersPerSecond = this.maxChassisSpeedMPS = maxSpeedMPS; + this.attainableMaxRotationalVelocityRadiansPerSecond = Math.PI * + 2; // Defaulting to something reasonable for most robots + swerveDriveConfiguration = config; + swerveController = new SwerveController(controllerConfig); + // Create Kinematics from swerve module locations. + kinematics = new SwerveDriveKinematics(config.moduleLocationsMeters); + odometryThread = new Notifier(this::updateOdometry); + + this.swerveModules = config.modules; + + // Create an integrator for angle if the robot is being simulated to emulate an IMU + // If the robot is real, instantiate the IMU instead. + if (SwerveDriveTelemetry.isSimulation) + { + DriveTrainSimulationConfig simulationConfig = DriveTrainSimulationConfig.Default() + .withBumperSize( + Meters.of(config.getTracklength()) + .plus(Inches.of(5)), + Meters.of(config.getTrackwidth()) + .plus(Inches.of(5))) + .withRobotMass(Kilograms.of(config.physicalCharacteristics.robotMassKg)) + .withCustomModuleTranslations(config.moduleLocationsMeters) + .withGyro(config.getGyroSim()) + .withSwerveModule(new SwerveModuleSimulationConfig( + config.getDriveMotorSim(), + config.getAngleMotorSim(), + config.physicalCharacteristics.conversionFactor.drive.gearRatio, + config.physicalCharacteristics.conversionFactor.angle.gearRatio, + Volts.of(config.physicalCharacteristics.driveFrictionVoltage), + Volts.of(config.physicalCharacteristics.angleFrictionVoltage), + Inches.of( + config.physicalCharacteristics.conversionFactor.drive.diameter / + 2), + KilogramSquareMeters.of(0.02), + config.physicalCharacteristics.wheelGripCoefficientOfFriction) + ); + + mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose); + + // feed module simulation instances to modules + for (int i = 0; i < swerveModules.length; i++) + { + this.swerveModules[i].configureModuleSimulation(mapleSimDrive.getModules()[i], config.physicalCharacteristics); + } + + // register the drivetrain simulation + SimulatedArena.getInstance().addDriveTrainSimulation(mapleSimDrive); + simIMU = new SwerveIMUSimulation(mapleSimDrive.getGyroSimulation()); + imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L); + } else + { + imu = config.imu; + imu.factoryDefault(); + imuReadingCache = new Cache<>(imu::getRotation3d, 5L); + } + + // odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions()); + swerveDrivePoseEstimator = + new SwerveDrivePoseEstimator( + kinematics, + getYaw(), + getModulePositions(), + startingPose); // x,y,heading in radians; Vision measurement std dev, higher=less weight +// +// Rotation3d currentGyro = imuReadingCache.getValue(); +// double offset = currentGyro.getZ() + +// startingPose.getRotation().getRadians(); +// setGyroOffset(new Rotation3d(currentGyro.getX(), currentGyro.getY(), offset)); + + // Initialize Telemetry + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) + { + SmartDashboard.putData("Field", field); + } + + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) + { + SwerveDriveTelemetry.maxSpeed = maxSpeedMPS; + SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity; + SwerveDriveTelemetry.moduleCount = swerveModules.length; + SwerveDriveTelemetry.sizeFrontBack = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, true, + false).moduleLocation.getX() + + SwerveMath.getSwerveModule(swerveModules, + false, + false).moduleLocation.getX()); + SwerveDriveTelemetry.sizeLeftRight = Units.metersToInches(SwerveMath.getSwerveModule(swerveModules, false, + true).moduleLocation.getY() + + SwerveMath.getSwerveModule(swerveModules, + false, + false).moduleLocation.getY()); + SwerveDriveTelemetry.wheelLocations = new double[SwerveDriveTelemetry.moduleCount * 2]; + for (SwerveModule module : swerveModules) + { + SwerveDriveTelemetry.wheelLocations[module.moduleNumber * 2] = Units.metersToInches( + module.configuration.moduleLocation.getX()); + SwerveDriveTelemetry.wheelLocations[(module.moduleNumber * 2) + 1] = Units.metersToInches( + module.configuration.moduleLocation.getY()); + } + SwerveDriveTelemetry.measuredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; + SwerveDriveTelemetry.desiredStates = new double[SwerveDriveTelemetry.moduleCount * 2]; + SwerveDriveTelemetry.desiredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount]; + SwerveDriveTelemetry.measuredStatesObj = new SwerveModuleState[SwerveDriveTelemetry.moduleCount]; + } + + setOdometryPeriod(SwerveDriveTelemetry.isSimulation ? 0.004 : 0.02); + + checkIfTunerXCompatible(); + + HAL.report(kResourceType_RobotDrive, kRobotDriveSwerve_YAGSL); + } + + @Override + public void close() + { + imu.close(); + tunerXRecommendation.close(); + + for (var module : swerveModules) + { + module.close(); + } + } + + /** + * Update the cache validity period for the robot. + * + * @param imu IMU reading cache validity period in milliseconds. + * @param driveMotor Drive motor reading cache in milliseconds. + * @param absoluteEncoder Absolute encoder reading cache in milliseconds. + */ + public void updateCacheValidityPeriods(long imu, long driveMotor, long absoluteEncoder) + { + imuReadingCache.updateValidityPeriod(imu); + for (SwerveModule module : swerveModules) + { + module.drivePositionCache.updateValidityPeriod(driveMotor); + module.driveVelocityCache.updateValidityPeriod(driveMotor); + module.absolutePositionCache.updateValidityPeriod(absoluteEncoder); + } + } + + /** + * Check all components to ensure that Tuner X Swerve Generator is recommended instead. + */ + private void checkIfTunerXCompatible() + { + boolean compatible = imu instanceof Pigeon2Swerve; + for (SwerveModule module : swerveModules) + { + compatible = compatible && module.getDriveMotor() instanceof TalonFXSwerve && + module.getAngleMotor() instanceof TalonFXSwerve && + module.getAbsoluteEncoder() instanceof CANCoderSwerve; + if (!compatible) + { + break; + } + } + if (compatible) + { + tunerXRecommendation.set(true); + } + + } + + /** + * Set the odometry update period in seconds. + * + * @param period period in seconds. + */ + public void setOdometryPeriod(double period) + { + odometryThread.stop(); + if (SwerveDriveTelemetry.isSimulation) + { + SimulatedArena.overrideSimulationTimings(Seconds.of(period), 1); + } + odometryThread.startPeriodic(period); + } + + /** + * Stop the odometry thread in favor of manually updating odometry. + */ + public void stopOdometryThread() + { + odometryThread.stop(); + if (SwerveDriveTelemetry.isSimulation) + { + SimulatedArena.overrideSimulationTimings(Seconds.of(TimedRobot.kDefaultPeriod), 5); + } + } + + /** + * Set the conversion factor for the angle/azimuth motor controller. + * + * @param conversionFactor Angle motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. + */ + public void setAngleMotorConversionFactor(double conversionFactor) + { + for (SwerveModule module : swerveModules) + { + module.setAngleMotorConversionFactor(conversionFactor); + } + } + + /** + * Set the conversion factor for the drive motor controller. + * + * @param conversionFactor Drive motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. + */ + public void setDriveMotorConversionFactor(double conversionFactor) + { + for (SwerveModule module : swerveModules) + { + module.setDriveMotorConversionFactor(conversionFactor); + } + } + + /** + * Fetch the latest odometry heading, should be trusted over {@link SwerveDrive#getYaw()}. + * + * @return {@link Rotation2d} of the robot heading. + */ + public Rotation2d getOdometryHeading() + { + return swerveDrivePoseEstimator.getEstimatedPosition().getRotation(); + } + + /** + * Set the heading correction capabilities of YAGSL. + * + * @param state {@link SwerveDrive#headingCorrection} state. + */ + public void setHeadingCorrection(boolean state) + { + setHeadingCorrection(state, HEADING_CORRECTION_DEADBAND); + } + + /** + * Set the heading correction capabilities of YAGSL. + * + * @param state {@link SwerveDrive#headingCorrection} state. + * @param deadband {@link SwerveDrive#HEADING_CORRECTION_DEADBAND} deadband. + */ + public void setHeadingCorrection(boolean state, double deadband) + { + headingCorrection = state; + HEADING_CORRECTION_DEADBAND = deadband; + } + + /** + * Tertiary method of controlling the drive base given velocity in both field oriented and robot oriented at the same + * time. The inputs are added together so this is not intended to be used to give the driver both methods of control. + * + * @param fieldOrientedVelocity The field oriented velocties to use + * @param robotOrientedVelocity The robot oriented velocties to use + */ + public void driveFieldOrientedAndRobotOriented(ChassisSpeeds fieldOrientedVelocity, + ChassisSpeeds robotOrientedVelocity) + { + + drive(ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading()) + .plus(robotOrientedVelocity)); + } + + /** + * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use. + * + * @param fieldRelativeSpeeds Velocity of the robot desired. + */ + public void driveFieldOriented(ChassisSpeeds fieldRelativeSpeeds) + { + drive(ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getOdometryHeading())); + } + + /** + * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use. + * + * @param fieldRelativeSpeeds Velocity of the robot desired. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void driveFieldOriented(ChassisSpeeds fieldRelativeSpeeds, Translation2d centerOfRotationMeters) + { + drive(ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, getOdometryHeading()), centerOfRotationMeters); + } + + /** + * Secondary method for controlling the drivebase. Given a simple {@link ChassisSpeeds} set the swerve module states, + * to achieve the goal. + * + * @param velocity The desired robot-oriented {@link ChassisSpeeds} for the robot to achieve. + */ + public void drive(ChassisSpeeds velocity) + { + drive(velocity, false, new Translation2d()); + } + + /** + * Secondary method for controlling the drivebase. Given a simple {@link ChassisSpeeds} set the swerve module states, + * to achieve the goal. + * + * @param velocity The desired robot-oriented {@link ChassisSpeeds} for the robot to achieve. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void drive(ChassisSpeeds velocity, Translation2d centerOfRotationMeters) + { + drive(velocity, false, centerOfRotationMeters); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and calculates + * and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel + * velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. + * + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters + * per second. In robot-relative mode, positive x is torwards the bow (front) and + * positive y is torwards port (left). In field-relative mode, positive x is away from + * the alliance wall (field North) and positive y is torwards the left wall when looking + * through the driver station glass (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot + * relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void drive( + Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop, + Translation2d centerOfRotationMeters) + { + // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if + // necessary. + ChassisSpeeds velocity = new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + if (fieldRelative) + { + velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading()); + } + drive(velocity, isOpenLoop, centerOfRotationMeters); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link Translation2d} and a rotation rate, and calculates + * and commands module states accordingly. Can use either open-loop or closed-loop velocity control for the wheel + * velocities. Also has field- and robot-relative modes, which affect how the translation vector is used. + * + * @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per + * second. In robot-relative mode, positive x is torwards the bow (front) and positive y is + * torwards port (left). In field-relative mode, positive x is away from the alliance wall (field + * North) and positive y is torwards the left wall when looking through the driver station glass + * (field West). + * @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot + * relativity. + * @param fieldRelative Drive mode. True for field-relative, false for robot-relative. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + */ + public void drive( + Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop) + { + // Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if + // necessary. + ChassisSpeeds velocity = new ChassisSpeeds(translation.getX(), translation.getY(), rotation); + + if (fieldRelative) + { + velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading()); + } + drive(velocity, isOpenLoop, new Translation2d()); + } + + /** + * The primary method for controlling the drivebase. Takes a {@link ChassisSpeeds}, and calculates and commands module + * states accordingly. Can use either open-loop or closed-loop velocity control for the wheel velocities. Applies + * heading correction if enabled and necessary. + * + * @param robotRelativeVelocity The chassis speeds to set the robot to achieve. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + * @param centerOfRotationMeters The center of rotation in meters, 0 is the center of the robot. + */ + public void drive(ChassisSpeeds robotRelativeVelocity, boolean isOpenLoop, Translation2d centerOfRotationMeters) + { + SwerveDriveTelemetry.startCtrlCycle(); + robotRelativeVelocity = movementOptimizations(robotRelativeVelocity, + chassisVelocityCorrection, + angularVelocityCorrection); + + // Heading Angular Velocity Deadband, might make a configuration option later. + // Originally made by Team 1466 Webb Robotics. + // Modified by Team 7525 Pioneers and BoiledBurntBagel of 6036 + if (headingCorrection) + { + if (Math.abs(robotRelativeVelocity.omegaRadiansPerSecond) < HEADING_CORRECTION_DEADBAND + && (Math.abs(robotRelativeVelocity.vxMetersPerSecond) > HEADING_CORRECTION_DEADBAND + || Math.abs(robotRelativeVelocity.vyMetersPerSecond) > HEADING_CORRECTION_DEADBAND)) + { + robotRelativeVelocity.omegaRadiansPerSecond = + swerveController.headingCalculate(getOdometryHeading().getRadians(), lastHeadingRadians); + } else + { + lastHeadingRadians = getOdometryHeading().getRadians(); + } + } + + // Display commanded speed for testing + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + { + SwerveDriveTelemetry.desiredChassisSpeedsObj = robotRelativeVelocity; + } + + // Calculate required module states via kinematics + SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(robotRelativeVelocity, + centerOfRotationMeters); + + setRawModuleStates(swerveModuleStates, robotRelativeVelocity, isOpenLoop); + } + + /** + * Set the maximum attainable speeds for desaturation. + * + * @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot can reach while + * translating in meters per second. + * @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating in + * radians per second. + */ + public void setMaximumAttainableSpeeds( + double attainableMaxTranslationalSpeedMetersPerSecond, + double attainableMaxRotationalVelocityRadiansPerSecond) + { + this.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond; + this.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond; + } + + /** + * Set the maximum allowable speeds for desaturation. + * + * @param maxTranslationalSpeedMetersPerSecond The allowable max speed that your robot should reach while translating + * in meters per second. + * @param maxRotationalVelocityRadiansPerSecond The allowable max speed the robot should reach while rotating in + * radians per second. + */ + public void setMaximumAllowableSpeeds( + double maxTranslationalSpeedMetersPerSecond, + double maxRotationalVelocityRadiansPerSecond) + { + this.maxChassisSpeedMPS = maxTranslationalSpeedMetersPerSecond; + this.swerveController.config.maxAngularVelocity = maxRotationalVelocityRadiansPerSecond; + } + + /** + * Get the maximum velocity from {@link SwerveDrive#attainableMaxTranslationalSpeedMetersPerSecond} or + * {@link SwerveDrive#maxChassisSpeedMPS} whichever is the lower limit on the robot's speed. + * + * @return Minimum speed in meters/second of physically attainable and user allowable limits. + */ + public double getMaximumChassisVelocity() + { + return Math.min(this.attainableMaxTranslationalSpeedMetersPerSecond, maxChassisSpeedMPS); + } + + /** + * Get the maximum drive velocity of a module as a {@link LinearVelocity}. + * + * @return {@link LinearVelocity} representing the maximum drive speed of a module. + */ + public double getMaximumModuleDriveVelocity() + { + return swerveModules[0].getMaxDriveVelocityMetersPerSecond(); + } + + /** + * Get the maximum angular velocity of an azimuth/angle motor in the swerve module. + * + * @return {@link AngularVelocity} of the maximum azimuth/angle motor. + */ + public AngularVelocity getMaximumModuleAngleVelocity() + { + return swerveModules[0].getMaxAngularVelocity(); + } + + /** + * Get the maximum angular velocity, either {@link SwerveDrive#attainableMaxRotationalVelocityRadiansPerSecond} or + * {@link SwerveControllerConfiguration#maxAngularVelocity}, whichever is the lower limit on the robot's speed. + * + * @return Minimum angular velocity in radians per second of physically attainable and user allowable limits. + */ + public double getMaximumChassisAngularVelocity() + { + return Math.min(this.attainableMaxRotationalVelocityRadiansPerSecond, swerveController.config.maxAngularVelocity); + } + + /** + * Set the module states (azimuth and velocity) directly. + * + * @param desiredStates A list of SwerveModuleStates to send to the modules. + * @param desiredChassisSpeed The desired chassis speeds to set the robot to achieve. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + */ + private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed, + boolean isOpenLoop) + { + // Desaturates wheel speeds + double maxModuleSpeedMPS = getMaximumModuleDriveVelocity(); + if ((attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) && + attainableMaxTranslationalSpeedMetersPerSecond != maxChassisSpeedMPS) + { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, desiredChassisSpeed, + maxModuleSpeedMPS, + attainableMaxTranslationalSpeedMetersPerSecond, + attainableMaxRotationalVelocityRadiansPerSecond); + } else + { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxModuleSpeedMPS); + } + + // Sets states + for (SwerveModule module : swerveModules) + { + module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false); + } + } + + /** + * Set the module states (azimuth and velocity) directly. Used primarily for auto paths. Does not allow for usage of + * {@link SwerveDriveKinematics#desaturateWheelSpeeds(SwerveModuleState[] moduleStates, ChassisSpeeds + * desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond, double + * attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond)} + * + * @param desiredStates A list of SwerveModuleStates to send to the modules. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + */ + public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop) + { + SwerveDriveTelemetry.startCtrlCycle(); + double maxModuleSpeedMPS = getMaximumModuleDriveVelocity(); + desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxModuleSpeedMPS); + + // Sets states + for (SwerveModule module : swerveModules) + { + module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false); + } + } + + /** + * Drive the robot using the {@link SwerveModuleState}, it is recommended to have + * {@link SwerveDrive#setCosineCompensator(boolean)} set to false for this.
+ * + * @param robotRelativeVelocity Robot relative {@link ChassisSpeeds} + * @param states Corresponding {@link SwerveModuleState} to use (not checked against the + * {@param robotRelativeVelocity}). + * @param feedforwardForces Feedforward forces generated by set-point generator + */ + public void drive(ChassisSpeeds robotRelativeVelocity, SwerveModuleState[] states, Force[] feedforwardForces) + { + SwerveDriveTelemetry.startCtrlCycle(); + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + { + SwerveDriveTelemetry.desiredChassisSpeedsObj = robotRelativeVelocity; + } + for (SwerveModule module : swerveModules) + { + module.applyStateOptimizations(states[module.moduleNumber]); + module.applyAntiJitter(states[module.moduleNumber], false); + + // from the module configuration, obtain necessary information to calculate feed-forward + // Warning: Will not work well if motor is not what we are expecting. + // Warning: Should replace module.getDriveMotor().simMotor with expected motor type first. + DCMotor driveMotorModel = module.configuration.driveMotor.getSimMotor(); + double driveGearRatio = module.configuration.conversionFactors.drive.gearRatio; + double wheelRadiusMeters = Units.inchesToMeters(module.configuration.conversionFactors.drive.diameter) / 2; + + // calculation: + double desiredGroundSpeedMPS = states[module.moduleNumber].speedMetersPerSecond; + double feedforwardVoltage = driveMotorModel.getVoltage( + // Since: (1) torque = force * momentOfForce; (2) torque (on wheel) = torque (on motor) * gearRatio + // torque (on motor) = force * wheelRadius / gearRatio + feedforwardForces[module.moduleNumber].in(Newtons) * wheelRadiusMeters / driveGearRatio, + // Since: (1) linear velocity = angularVelocity * wheelRadius; (2) wheelVelocity = motorVelocity / gearRatio + // motorAngularVelocity = linearVelocity / wheelRadius * gearRatio + desiredGroundSpeedMPS / wheelRadiusMeters * driveGearRatio + ); + module.setDesiredState( + states[module.moduleNumber], + false, + feedforwardVoltage + ); + } + } + + /** + * Set chassis speeds with closed-loop velocity control. + * + * @param robotRelativeSpeeds Chassis speeds to set. + */ + public void setChassisSpeeds(ChassisSpeeds robotRelativeSpeeds) + { + SwerveDriveTelemetry.startCtrlCycle(); + robotRelativeSpeeds = movementOptimizations(robotRelativeSpeeds, + autonomousChassisVelocityCorrection, + autonomousAngularVelocityCorrection); + + SwerveDriveTelemetry.desiredChassisSpeedsObj = robotRelativeSpeeds; + + setRawModuleStates(kinematics.toSwerveModuleStates(robotRelativeSpeeds), robotRelativeSpeeds, false); + } + + /** + * Gets the measured pose (position and rotation) of the robot, as reported by odometry. + * + * @return The robot's pose + */ + public Pose2d getPose() + { + + odometryLock.lock(); + Pose2d poseEstimation = swerveDrivePoseEstimator.getEstimatedPosition(); + odometryLock.unlock(); + return poseEstimation; + } + + /** + * Gets the maple-sim drivetrain simulation instance This is used to add intake simulation / launch game pieces from + * the robot + * + * @return an optional maple-sim {@link SwerveDriveSimulation} object, or {@link Optional#empty()} when calling from a + * real robot + */ + public Optional getMapleSimDrive() + { + if (SwerveDriveTelemetry.isSimulation) + { + return Optional.of(mapleSimDrive); + } + + return Optional.empty(); + } + + /** + * Gets the actual pose of the drivetrain during simulation + * + * @return an {@link Optional} {@link Pose2d}, representing the drivetrain pose during simulation, or an empty + * optional when running on real robot + */ + public Optional getSimulationDriveTrainPose() + { + odometryLock.lock(); + Optional simulationPose = getMapleSimDrive().map(AbstractDriveTrainSimulation::getSimulatedDriveTrainPose); + odometryLock.unlock(); + return simulationPose; + } + + /** + * Gets the measured field-relative robot velocity (x, y and omega) + * + * @return A ChassisSpeeds object of the current field-relative velocity + */ + public ChassisSpeeds getFieldVelocity() + { + // ChassisSpeeds has a method to convert from field-relative to robot-relative speeds, + // but not the reverse. However, because this transform is a simple rotation, negating the + // angle given as the robot angle reverses the direction of rotation, and the conversion is reversed. + ChassisSpeeds robotRelativeSpeeds = kinematics.toChassisSpeeds(getStates()); + return ChassisSpeeds.fromRobotRelativeSpeeds(robotRelativeSpeeds, getOdometryHeading()); + // Might need to be this instead + //return ChassisSpeeds.fromFieldRelativeSpeeds( + // kinematics.toChassisSpeeds(getStates()), getOdometryHeading().unaryMinus()); + } + + /** + * Gets the current robot-relative velocity (x, y and omega) of the robot + * + * @return A ChassisSpeeds object of the current robot-relative velocity + */ + public ChassisSpeeds getRobotVelocity() + { + return kinematics.toChassisSpeeds(getStates()); + } + + /** + * Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this + * method. However, if either gyro angle or module position is reset, this must be called in order for odometry to + * keep working. + * + * @param pose The pose to set the odometry to. Field relative, blue-origin where 0deg is facing towards RED alliance. + */ + public void resetOdometry(Pose2d pose) + { + odometryLock.lock(); + swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose); + if (SwerveDriveTelemetry.isSimulation) + { + mapleSimDrive.setSimulationWorldPose(pose); + } + odometryLock.unlock(); + ChassisSpeeds robotRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(new ChassisSpeeds(0, 0, 0), getYaw()); + kinematics.toSwerveModuleStates(robotRelativeSpeeds); + + } + + /** + * Post the trajectory to the field + * + * @param trajectory the trajectory to post. + */ + public void postTrajectory(Trajectory trajectory) + { + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) + { + field.getObject("Trajectory").setTrajectory(trajectory); + } + } + + /** + * Gets the current module states (azimuth and velocity) + * + * @return A list of SwerveModuleStates containing the current module states + */ + public SwerveModuleState[] getStates() + { + SwerveModuleState[] states = new SwerveModuleState[swerveDriveConfiguration.moduleCount]; + for (SwerveModule module : swerveModules) + { + states[module.moduleNumber] = module.getState(); + } + return states; + } + + /** + * Gets the current module positions (azimuth and wheel position (meters)). + * + * @return A list of SwerveModulePositions containg the current module positions + */ + public SwerveModulePosition[] getModulePositions() + { + SwerveModulePosition[] positions = + new SwerveModulePosition[swerveDriveConfiguration.moduleCount]; + for (SwerveModule module : swerveModules) + { + positions[module.moduleNumber] = module.getPosition(); + } + return positions; + } + + /** + * Getter for the {@link SwerveIMU}. + * + * @return generated {@link SwerveIMU} + */ + public SwerveIMU getGyro() + { + return swerveDriveConfiguration.imu; + } + + /** + * Set the expected gyroscope angle using a {@link Rotation3d} object. To reset gyro, set to a new {@link Rotation3d} + * subtracted from the current gyroscopic readings {@link SwerveIMU#getRotation3d()}. + * + * @param gyro expected gyroscope angle as {@link Rotation3d}. + */ + public void setGyro(Rotation3d gyro) + { + if (SwerveDriveTelemetry.isSimulation) + { + setGyroOffset(simIMU.getGyroRotation3d().minus(gyro)); + } else + { + setGyroOffset(imu.getRawRotation3d().minus(gyro)); + } + imuReadingCache.update(); + } + + /** + * Resets the gyro angle to zero and resets odometry to the same position, but facing toward 0 (red alliance station). + */ + public void zeroGyro() + { + // Resets the real gyro or the angle accumulator, depending on whether the robot is being + // simulated + if (SwerveDriveTelemetry.isSimulation) + { + simIMU.setAngle(0); + } else + { + setGyroOffset(imu.getRawRotation3d()); + } + imuReadingCache.update(); + swerveController.lastAngleScalar = 0; + lastHeadingRadians = 0; + resetOdometry(new Pose2d(getPose().getTranslation(), new Rotation2d())); + } + + /** + * Gets the current yaw angle of the robot, as reported by the imu. CCW positive, not wrapped. + * + * @return The yaw as a {@link Rotation2d} angle + */ + public Rotation2d getYaw() + { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + return Rotation2d.fromRadians(imuReadingCache.getValue().getZ()); + } + + /** + * Gets the current pitch angle of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation2d} angle + */ + public Rotation2d getPitch() + { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + return Rotation2d.fromRadians(imuReadingCache.getValue().getY()); + } + + /** + * Gets the current roll angle of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation2d} angle + */ + public Rotation2d getRoll() + { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + return Rotation2d.fromRadians(imuReadingCache.getValue().getX()); + } + + /** + * Gets the current gyro {@link Rotation3d} of the robot, as reported by the imu. + * + * @return The heading as a {@link Rotation3d} angle + */ + public Rotation3d getGyroRotation3d() + { + // Read the imu if the robot is real or the accumulator if the robot is simulated. + return imuReadingCache.getValue(); + } + + /** + * Gets current acceleration of the robot in m/s/s. If gyro unsupported returns empty. + * + * @return acceleration of the robot as a {@link Translation3d} + */ + public Optional getAccel() + { + if (!SwerveDriveTelemetry.isSimulation) + { + return imu.getAccel(); + } else + { + return simIMU.getAccel(); + } + } + + /** + * Sets the drive motors to brake/coast mode. + * + * @param brake True to set motors to brake mode, false for coast. + */ + public void setMotorIdleMode(boolean brake) + { + for (SwerveModule swerveModule : swerveModules) + { + swerveModule.setMotorBrake(brake); + } + } + + /** + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * + * @param enabled Enable state + * @param deadband Deadband in degrees, default is 3 degrees. + */ + public void setModuleEncoderAutoSynchronize(boolean enabled, double deadband) + { + for (SwerveModule swerveModule : swerveModules) + { + swerveModule.setEncoderAutoSynchronize(enabled, deadband); + } + } + + + /** + * Point all modules toward the robot center, thus making the robot very difficult to move. Forcing the robot to keep + * the current pose. + */ + public void lockPose() + { + // Sets states + for (SwerveModule swerveModule : swerveModules) + { + SwerveModuleState desiredState = + new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle()); + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) + { + SwerveDriveTelemetry.desiredStatesObj[swerveModule.moduleNumber] = desiredState; + } + swerveModule.setDesiredState(desiredState, false, true); + + } + + // Update kinematics because we are not using setModuleStates + kinematics.toSwerveModuleStates(new ChassisSpeeds()); + } + + /** + * Get the swerve module poses and on the field relative to the robot. + * + * @param robotPose Robot pose. + * @return Swerve module poses. + */ + public Pose2d[] getSwerveModulePoses(Pose2d robotPose) + { + Pose2d[] poseArr = new Pose2d[swerveDriveConfiguration.moduleCount]; + List poses = new ArrayList<>(); + for (SwerveModule module : swerveModules) + { + poses.add( + robotPose.plus( + new Transform2d(module.configuration.moduleLocation, module.getState().angle))); + } + return poses.toArray(poseArr); + } + + /** + * Setup the swerve module feedforward. + * + * @param driveFeedforward Feedforward for the drive motor on swerve modules. + */ + public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward driveFeedforward) + { + for (SwerveModule swerveModule : swerveModules) + { + swerveModule.setFeedforward(driveFeedforward); + } + } + + /** + * Update odometry should be run every loop. Synchronizes module absolute encoders with relative encoders + * periodically. In simulation mode will also post the pose of each module. Updates SmartDashboard with module encoder + * readings and states. + */ + public void updateOdometry() + { + SwerveDriveTelemetry.startOdomCycle(); + odometryLock.lock(); +// invalidateCache(); + try + { + // Update odometry + swerveDrivePoseEstimator.update(getYaw(), getModulePositions()); + + if (SwerveDriveTelemetry.isSimulation) + { + try + { + SimulatedArena.getInstance().simulationPeriodic(); + } catch (Exception e) + { + DriverStation.reportError("MapleSim error", false); + } + } + + // Update angle accumulator if the robot is simulated + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) + { + SwerveDriveTelemetry.measuredChassisSpeedsObj = getRobotVelocity(); + SwerveDriveTelemetry.robotRotationObj = getOdometryHeading(); + } + + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) + { + if (SwerveDriveTelemetry.isSimulation) + { + field.setRobotPose(mapleSimDrive.getSimulatedDriveTrainPose()); + field.getObject("OdometryPose").setPose(swerveDrivePoseEstimator.getEstimatedPosition()); + field.getObject("XModules").setPoses(getSwerveModulePoses(mapleSimDrive.getSimulatedDriveTrainPose())); + + } else + { + field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); + } + } + + double sumVelocity = 0; + for (SwerveModule module : swerveModules) + { + SwerveModuleState moduleState = module.getState(); + sumVelocity += Math.abs(moduleState.speedMetersPerSecond); + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + module.updateTelemetry(); + rawIMUPublisher.set(getYaw().getDegrees()); + adjustedIMUPublisher.set(getOdometryHeading().getDegrees()); + } + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) + { + SwerveDriveTelemetry.measuredStatesObj[module.moduleNumber] = moduleState; + } + } + + // If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS + // lib) + // To ensure that everytime we initialize it works. + if (sumVelocity <= .01 && ++moduleSynchronizationCounter > 5) + { + synchronizeModuleEncoders(); + moduleSynchronizationCounter = 0; + } + + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) + { + SwerveDriveTelemetry.updateData(); + } + } catch (Exception e) + { + odometryLock.unlock(); + throw e; + } + odometryLock.unlock(); + SwerveDriveTelemetry.endOdomCycle(); + } + + /** + * Invalidate all {@link Cache} object used by the {@link SwerveDrive} + */ + public void invalidateCache() + { + imuReadingCache.update(); + for (SwerveModule module : swerveModules) + { + module.invalidateCache(); + } + } + + /** + * Synchronize angle motor integrated encoders with data from absolute encoders. + */ + public void synchronizeModuleEncoders() + { + for (SwerveModule module : swerveModules) + { + module.queueSynchronizeEncoders(); + } + } + + /** + * Set the gyro scope offset to a desired known rotation. Unlike {@link SwerveDrive#setGyro(Rotation3d)} it DOES NOT + * take the current rotation into account. + * + * @param offset {@link Rotation3d} known offset of the robot for gyroscope to use. + */ + public void setGyroOffset(Rotation3d offset) + { + if (SwerveDriveTelemetry.isSimulation) + { + simIMU.setAngle(offset.getZ()); + } else + { + imu.setOffset(offset); + } + imuReadingCache.update(); + } + + /** + * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with + * the given timestamp of the vision measurement. + * + * @param robotPose Robot {@link Pose2d} as measured by vision. + * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from + * {@link Timer#getFPGATimestamp()} or similar sources. + * @param visionMeasurementStdDevs Vision measurement standard deviation that will be sent to the + * {@link SwerveDrivePoseEstimator}.The standard deviation of the vision measurement, + * for best accuracy calculate the standard deviation at 2 or more points and fit a + * line to it with the calculated optimal standard deviation. (Units should be meters + * per pixel). By optimizing this you can get * vision accurate to inches instead of + * feet. + */ + public void addVisionMeasurement(Pose2d robotPose, double timestamp, + Matrix visionMeasurementStdDevs) + { + odometryLock.lock(); + swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp, visionMeasurementStdDevs); + odometryLock.unlock(); + } + + /** + * Sets the pose estimator's trust of global measurements. This might be used to change trust in vision measurements + * after the autonomous period, or to change trust as distance to a vision target increases. + * + * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these numbers to trust + * global measurements from vision less. This matrix is in the form [x, y, theta], + * with units in meters and radians. + */ + public void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) + { + odometryLock.lock(); + swerveDrivePoseEstimator.setVisionMeasurementStdDevs(visionMeasurementStdDevs); + odometryLock.unlock(); + } + + /** + * Add a vision measurement to the {@link SwerveDrivePoseEstimator} and update the {@link SwerveIMU} gyro reading with + * the given timestamp of the vision measurement. + * + * @param robotPose Robot {@link Pose2d} as measured by vision. + * @param timestamp Timestamp the measurement was taken as time since startup, should be taken from + * {@link Timer#getFPGATimestamp()} or similar sources. + */ + public void addVisionMeasurement(Pose2d robotPose, double timestamp) + { + odometryLock.lock(); + swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp); +// Pose2d newOdometry = new Pose2d(swerveDrivePoseEstimator.getEstimatedPosition().getTranslation(), +// robotPose.getRotation()); + odometryLock.unlock(); + +// setGyroOffset(new Rotation3d(0, 0, robotPose.getRotation().getRadians())); +// resetOdometry(newOdometry); + } + + /** + * Helper function to get the {@link SwerveDrive#swerveController} for the {@link SwerveDrive} which can be used to + * generate {@link ChassisSpeeds} for the robot to orient it correctly given axis or angles, and apply + * {@link edu.wpi.first.math.filter.SlewRateLimiter} to given inputs. Important functions to look at are + * {@link SwerveController#getTargetSpeeds(double, double, double, double, double)}, + * {@link SwerveController#addSlewRateLimiters(SlewRateLimiter, SlewRateLimiter, SlewRateLimiter)}, + * {@link SwerveController#getRawTargetSpeeds(double, double, double)}. + * + * @return {@link SwerveController} for the {@link SwerveDrive}. + */ + public SwerveController getSwerveController() + { + return swerveController; + } + + /** + * Get the {@link SwerveModule}s associated with the {@link SwerveDrive}. + * + * @return {@link SwerveModule} array specified by configurations. + */ + public SwerveModule[] getModules() + { + return swerveDriveConfiguration.modules; + } + + /** + * Get the {@link SwerveModule}'s as a {@link HashMap} where the key is the swerve module configuration name. + * + * @return {@link HashMap}(Module Name, SwerveModule) + */ + public Map getModuleMap() + { + Map map = new HashMap(); + for (SwerveModule module : swerveModules) + { + map.put(module.configuration.name, module); + } + return map; + } + + /** + * Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in + * autonomous. + */ + public void resetDriveEncoders() + { + for (SwerveModule module : swerveModules) + { + module.getDriveMotor().setPosition(0); + } + } + + /** + * Set the motor controller closed loop feedback device to the defined external absolute encoder, with the given + * offset from the supplied configuration, overwriting any native offset. + */ + public void useExternalFeedbackSensor() + { + for (SwerveModule module : swerveModules) + { + module.useExternalFeedbackSensor(); + } + } + + /** + * Set the motor controller closed loop feedback device to the internal encoder instead of the absolute encoder. + */ + public void useInternalFeedbackSensor() + { + for (SwerveModule module : swerveModules) + { + module.useInternalFeedbackSensor(); + } + } + + /** + * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the + * internal offsets to prevent double offsetting. + */ + @Deprecated + public void pushOffsetsToEncoders() + { + for (SwerveModule module : swerveModules) + { + module.pushOffsetsToEncoders(); + } + } + + /** + * Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0 + */ + @Deprecated + public void restoreInternalOffset() + { + for (SwerveModule module : swerveModules) + { + module.restoreInternalOffset(); + } + } + + /** + * Set module optimization to be utilized or not. Sometimes it is desirable to be enabled for debugging purposes + * only. + * + * @param enabled Optimization enabled state. + */ + public void setModuleStateOptimization(boolean enabled) + { + for (SwerveModule module : swerveModules) + { + module.setModuleStateOptimization(enabled); + } + } + + /** + * Enable auto-centering module wheels. This has a side effect of causing some jitter to the robot when a PID is not + * tuned perfectly. This function is a wrapper for {@link SwerveModule#setAntiJitter(boolean)} to perform + * auto-centering. + * + * @param enabled Enable auto-centering (disable antiJitter) + */ + public void setAutoCenteringModules(boolean enabled) + { + for (SwerveModule module : swerveModules) + { + module.setAntiJitter(!enabled); + } + } + + /** + * Enable or disable the {@link swervelib.parser.SwerveModuleConfiguration#useCosineCompensator} for all + * {@link SwerveModule}'s in the swerve drive. The cosine compensator will slow down or speed up modules that are + * close to their desired state in theory. + * + * @param enabled Usage of the cosine compensator. + */ + public void setCosineCompensator(boolean enabled) + { + for (SwerveModule module : swerveModules) + { + module.configuration.useCosineCompensator = enabled; + } + } + + /** + * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop + * + * @param enable Enable chassis velocity correction, which will use + * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)}} with the following. + * @param dtSeconds The duration of the timestep the speeds should be applied for. + */ + public void setChassisDiscretization(boolean enable, double dtSeconds) + { + if (!SwerveDriveTelemetry.isSimulation) + { + chassisVelocityCorrection = enable; + discretizationdtSeconds = dtSeconds; + } + } + + /** + * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop + * and/or auto + * + * @param useInTeleop Enable chassis velocity correction, which will use + * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop. + * @param useInAuto Enable chassis velocity correction, which will use + * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto. + * @param dtSeconds The duration of the timestep the speeds should be applied for. + */ + public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds) + { + if (!SwerveDriveTelemetry.isSimulation) + { + chassisVelocityCorrection = useInTeleop; + autonomousChassisVelocityCorrection = useInAuto; + discretizationdtSeconds = dtSeconds; + } + } + + /** + * Enables angular velocity skew correction in teleop and/or autonomous and sets the angular velocity coefficient for + * both modes + * + * @param useInTeleop Enables angular velocity correction in teleop. + * @param useInAuto Enables angular velocity correction in autonomous. + * @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15. Start with a + * value of 0.1, test in teleop. When enabling for the first time if the skew is + * significantly worse try inverting the value. Tune by moving in a straight line while + * rotating. Testing is best done with angular velocity controls on the right stick. + * Change the value until you are visually happy with the skew. Ensure your tune works + * with different translational and rotational magnitudes. If this reduces skew in teleop, + * it may improve auto. + */ + public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAuto, double angularVelocityCoeff) + { + if (!SwerveDriveTelemetry.isSimulation) + { + angularVelocityCorrection = useInTeleop; + autonomousAngularVelocityCorrection = useInAuto; + angularVelocityCoefficient = angularVelocityCoeff; + } + } + + /** + * Correct for skew that worsens as angular velocity increases + * + * @param robotRelativeVelocity The chassis speeds to set the robot to achieve. + * @return {@link ChassisSpeeds} of the robot after angular velocity skew correction. + */ + public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds robotRelativeVelocity) + { + var angularVelocity = new Rotation2d(imu.getYawAngularVelocity().in(RadiansPerSecond) * angularVelocityCoefficient); + if (angularVelocity.getRadians() != 0.0) + { + ChassisSpeeds fieldRelativeVelocity = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelativeVelocity, + getOdometryHeading()); + robotRelativeVelocity = ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeVelocity, + getOdometryHeading().plus(angularVelocity)); + } + return robotRelativeVelocity; + } + + /** + * Enable desired drive corrections + * + * @param robotRelativeVelocity The chassis speeds to set the robot to achieve. + * @param uesChassisDiscretize Correct chassis velocity using 254's correction. + * @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew. + * @return The chassis speeds after optimizations. + */ + private ChassisSpeeds movementOptimizations(ChassisSpeeds robotRelativeVelocity, boolean uesChassisDiscretize, + boolean useAngularVelocitySkewCorrection) + { + + if (useAngularVelocitySkewCorrection) + { + robotRelativeVelocity = angularVelocitySkewCorrection(robotRelativeVelocity); + } + + // Thank you to Jared Russell FRC254 for Open Loop Compensation Code + // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 + if (uesChassisDiscretize) + { + robotRelativeVelocity = ChassisSpeeds.discretize(robotRelativeVelocity, discretizationdtSeconds); + } + + return robotRelativeVelocity; + } + + /** + * Convert a {@link ChassisSpeeds} to {@link SwerveModuleState[]} for use elsewhere. + * + * @param robotRelativeVelocity {@link ChassisSpeeds} velocity to use. + * @param optimize Perform chassis velocity correction or angular velocity correction. + * @return {@link SwerveModuleState[]} for use elsewhere. + */ + public SwerveModuleState[] toServeModuleStates(ChassisSpeeds robotRelativeVelocity, boolean optimize) + { + if (optimize) + { + robotRelativeVelocity = movementOptimizations(robotRelativeVelocity, + chassisVelocityCorrection, + angularVelocityCorrection); + } + return kinematics.toSwerveModuleStates(robotRelativeVelocity); + } +} diff --git a/temp_yagsl/swervelib/SwerveModule.java b/temp_yagsl/swervelib/SwerveModule.java new file mode 100644 index 0000000..0eb060e --- /dev/null +++ b/temp_yagsl/swervelib/SwerveModule.java @@ -0,0 +1,1016 @@ +package swervelib; + +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.BooleanPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import swervelib.encoders.SparkMaxEncoderSwerve; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.math.SwerveMath; +import swervelib.motors.SparkMaxBrushedMotorSwerve; +import swervelib.motors.SparkMaxSwerve; +import swervelib.motors.SwerveMotor; +import swervelib.parser.Cache; +import swervelib.parser.PIDFConfig; +import swervelib.parser.SwerveModuleConfiguration; +import swervelib.parser.SwerveModulePhysicalCharacteristics; +import swervelib.simulation.SwerveModuleSimulation; +import swervelib.telemetry.SwerveDriveTelemetry; +import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; + +/** + * The Swerve Module class which represents and controls Swerve Modules for the swerve drive. + */ +public class SwerveModule implements AutoCloseable +{ + + /** + * Swerve module configuration options. + */ + public final SwerveModuleConfiguration configuration; + /** + * Absolute encoder position cache. + */ + public final Cache absolutePositionCache; + /** + * Drive motor position cache. + */ + public final Cache drivePositionCache; + /** + * Drive motor velocity cache. + */ + public final Cache driveVelocityCache; + /** + * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. + */ + public final int moduleNumber; + /** + * Swerve Motors. + */ + private final SwerveMotor angleMotor, driveMotor; + /** + * Absolute encoder for swerve drive. + */ + private final SwerveAbsoluteEncoder absoluteEncoder; + /** + * An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails. + */ + private final Alert encoderOffsetWarning; + /** + * An {@link Alert} for if there is no Absolute Encoder on the module. + */ + private final Alert noEncoderWarning; + /** + * An {@link Alert} for if there is no Absolute Encoder on the module. + */ + private final Alert externalSensorIsNull = new Alert("No absolute Encoder found.", + AlertType.kError); + /** + * An {@link Alert} for if the offset is 0 degrees. + */ + private final Alert internalOffsetIsZero = new Alert( + "Absolute encoder offset is 0, this may be a problem.", + AlertType.kWarning); + /** + * An {@link Alert} for if the angle/steer/azimuth motor is incompatible with the absolute encoder. + */ + private final Alert externalFeedbackIncompatible = new Alert( + "Absolute encoder is incompatible, cannot set as an external feedback device.", + AlertType.kError); + /** + * An {@link Alert} for if the absolute encoder cannot set an offset. + */ + private final Alert externalOffsetIncompatible = new Alert( + "Absolute encoder is incompatible, cannot set an offset internally.", + AlertType.kError); + /** + * NT4 Raw Absolute Angle publisher for the absolute encoder. + */ + private final DoublePublisher rawAbsoluteAnglePublisher; + /** + * NT4 Adjusted Absolute angle publisher for the absolute encoder. + */ + private final DoublePublisher adjAbsoluteAnglePublisher; + /** + * NT4 Absolute encoder read issue. + */ + private final BooleanPublisher absoluteEncoderIssuePublisher; + /** + * NT4 raw angle motor. + */ + private final DoublePublisher rawAnglePublisher; + /** + * NT4 Raw drive motor. + */ + private final DoublePublisher rawDriveEncoderPublisher; + /** + * NT4 Raw drive motor. + */ + private final DoublePublisher rawDriveVelocityPublisher; + /** + * Speed setpoint publisher for the module motor-controller PID. + */ + private final DoublePublisher speedSetpointPublisher; + /** + * Angle setpoint publisher for the module motor-controller PID. + */ + private final DoublePublisher angleSetpointPublisher; + /** + * Maximum {@link LinearVelocity} for the drive motor of the swerve module. + */ + private LinearVelocity maxDriveVelocity; + /** + * Maximum velocity for the drive motor of the swerve module. + */ + private double maxDriveVelocityMetersPerSecond; + /** + * Maximum {@link AngularVelocity} for the azimuth/angle motor of the swerve module. + */ + private AngularVelocity maxAngularVelocity; + /** + * Feedforward for the drive motor during closed loop control. + */ + private SimpleMotorFeedforward driveMotorFeedforward; + /** + * Anti-Jitter AKA auto-centering disabled. + */ + private boolean antiJitterEnabled = true; + /** + * Last swerve module state applied. + */ + private SwerveModuleState lastState; + /** + * Angle offset from the absolute encoder. + */ + private double angleOffset; + /** + * Simulated swerve module. + */ + private SwerveModuleSimulation simModule; + /** + * Enables utilization off {@link SwerveModuleState#optimize(Rotation2d)} + */ + private boolean optimizeSwerveModuleState = true; + /** + * Encoder synchronization queued. + */ + private boolean synchronizeEncoderQueued = false; + /** + * Encoder, Absolute encoder synchronization enabled. + */ + private boolean synchronizeEncoderEnabled = false; + /** + * Encoder synchronization deadband in degrees. + */ + private double synchronizeEncoderDeadband = 3; + + + /** + * Construct the swerve module and initialize the swerve module motors and absolute encoder. + * + * @param moduleNumber Module number for kinematics. + * @param moduleConfiguration Module constants containing CAN ID's and offsets. + */ + public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration) + { + // angle = 0; + // speed = 0; + // omega = 0; + // fakePos = 0; + this.moduleNumber = moduleNumber; + configuration = moduleConfiguration; + angleOffset = moduleConfiguration.angleOffset; + + // Create motors from configuration and reset them to defaults. + angleMotor = moduleConfiguration.angleMotor; + driveMotor = moduleConfiguration.driveMotor; + angleMotor.factoryDefaults(); + driveMotor.factoryDefaults(); + + // Initialize Feedforwards. + driveMotorFeedforward = getDefaultFeedforward(); + + // Configure voltage comp, current limit, and ramp rate. + angleMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); + driveMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); + angleMotor.setCurrentLimit(configuration.physicalCharacteristics.angleMotorCurrentLimit); + driveMotor.setCurrentLimit(configuration.physicalCharacteristics.driveMotorCurrentLimit); + angleMotor.setLoopRampRate(configuration.physicalCharacteristics.angleMotorRampRate); + driveMotor.setLoopRampRate(configuration.physicalCharacteristics.driveMotorRampRate); + + // Config angle encoders + absoluteEncoder = moduleConfiguration.absoluteEncoder; + if (absoluteEncoder != null) + { + absoluteEncoder.factoryDefault(); + absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted); + } + + if (SwerveDriveTelemetry.isSimulation) + { + simModule = new SwerveModuleSimulation(); + } + + // Setup the cache for the absolute encoder position. + absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 20); + + // Config angle motor/controller + if (!angleMotor.usingExternalFeedbackSensor()) + { + angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor); + } + angleMotor.configurePIDF(moduleConfiguration.anglePIDF); + angleMotor.configurePIDWrapping(0, 360); + angleMotor.setInverted(moduleConfiguration.angleMotorInverted); + angleMotor.setMotorBrake(false); + + // Set the position AFTER settings the conversion factor. + if (absoluteEncoder != null) + { + angleMotor.setPosition(getAbsolutePosition()); + } + + // Config drive motor/controller + driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive.factor); + driveMotor.configurePIDF(moduleConfiguration.velocityPIDF); + driveMotor.setInverted(moduleConfiguration.driveMotorInverted); + driveMotor.setMotorBrake(true); + + driveMotor.burnFlash(); + angleMotor.burnFlash(); + + drivePositionCache = new Cache<>(driveMotor::getPosition, 20); + driveVelocityCache = new Cache<>(driveMotor::getVelocity, 20); + + // Force a cache update on init. + driveVelocityCache.update(); + drivePositionCache.update(); + absolutePositionCache.update(); + + // Save the current state. + lastState = getState(); + + noEncoderWarning = new Alert("Motors", + "There is no Absolute Encoder on module #" + + moduleNumber, + AlertType.kWarning); + encoderOffsetWarning = new Alert("Motors", + "Pushing the Absolute Encoder offset to the encoder failed on module #" + + moduleNumber, + AlertType.kWarning); + + rawAbsoluteAnglePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic( + "swerve/modules/" + configuration.name + "/Raw Absolute Encoder").publish(); + adjAbsoluteAnglePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic( + "swerve/modules/" + configuration.name + "/Adjusted Absolute Encoder").publish(); + absoluteEncoderIssuePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getBooleanTopic( + "swerve/modules/" + configuration.name + "/Absolute Encoder Read Issue").publish(); + rawAnglePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic( + "swerve/modules/" + configuration.name + "/Raw Angle Encoder").publish(); + rawDriveEncoderPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic( + "swerve/modules/" + configuration.name + "/Raw Drive Encoder").publish(); + rawDriveVelocityPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic( + "swerve/modules/" + configuration.name + "/Raw Drive Velocity").publish(); + speedSetpointPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic( + "swerve/modules/" + configuration.name + "/Speed Setpoint").publish(); + angleSetpointPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic( + "swerve/modules/" + configuration.name + "/Angle Setpoint").publish(); + } + + @Override + public void close() + { + angleMotor.close(); + driveMotor.close(); + absoluteEncoder.close(); + } + + /** + * Get the default {@link SimpleMotorFeedforward} for the swerve module drive motor. + * + * @return {@link SimpleMotorFeedforward} using motor details. + */ + public SimpleMotorFeedforward getDefaultFeedforward() + { + double nominalVoltage = driveMotor.getSimMotor().nominalVoltageVolts; + double maxDriveSpeedMPS = getMaxVelocity().in(MetersPerSecond); + return SwerveMath.createDriveFeedforward(nominalVoltage, + maxDriveSpeedMPS, + configuration.physicalCharacteristics.wheelGripCoefficientOfFriction); + } + + /** + * Set utilization of {@link SwerveModuleState#optimize(Rotation2d)} which should be disabled for some debugging. + * + * @param optimizationState Optimization enabled. + */ + public void setModuleStateOptimization(boolean optimizationState) + { + optimizeSwerveModuleState = optimizationState; + if (!optimizeSwerveModuleState) + { + angleMotor.disablePIDWrapping(); + angleMotor.burnFlash(); + } + } + + /** + * Check if the module state optimization used by {@link SwerveModuleState#optimize(Rotation2d)} is enabled. + * + * @return optimization state. + */ + public boolean getModuleStateOptimization() + { + return optimizeSwerveModuleState; + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param optimalVoltage Nominal voltage for operation to output to. + */ + public void setAngleMotorVoltageCompensation(double optimalVoltage) + { + angleMotor.setVoltageCompensation(optimalVoltage); + } + + /** + * Set the voltage compensation for the swerve module motor. + * + * @param optimalVoltage Nominal voltage for operation to output to. + */ + public void setDriveMotorVoltageCompensation(double optimalVoltage) + { + driveMotor.setVoltageCompensation(optimalVoltage); + } + + + /** + * Queue synchronization of the integrated angle encoder with the absolute encoder. + */ + public void queueSynchronizeEncoders() + { + if (absoluteEncoder != null && synchronizeEncoderEnabled) + { + synchronizeEncoderQueued = true; + } + } + + /** + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * + * @param enabled Enable state + * @param deadband Deadband in degrees, default is 3 degrees. + */ + public void setEncoderAutoSynchronize(boolean enabled, double deadband) + { + synchronizeEncoderEnabled = enabled; + synchronizeEncoderDeadband = deadband; + } + + /** + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * + * @param enabled Enable state + */ + public void setEncoderAutoSynchronize(boolean enabled) + { + synchronizeEncoderEnabled = enabled; + } + + /** + * Set the antiJitter functionality, if true the modules will NOT auto center. Pushes the offsets to the angle motor + * controllers as well. + * + * @param antiJitter Anti-Jitter state desired. + */ + public void setAntiJitter(boolean antiJitter) + { + this.antiJitterEnabled = antiJitter; + if (antiJitter) + { + pushOffsetsToEncoders(); + } else + { + restoreInternalOffset(); + } + } + + /** + * Set the feedforward attributes to the given parameters. + * + * @param drive Drive motor feedforward for the module. + */ + public void setFeedforward(SimpleMotorFeedforward drive) + { + this.driveMotorFeedforward = drive; + } + + /** + * Get the current drive motor PIDF values. + * + * @return {@link PIDFConfig} of the drive motor. + */ + public PIDFConfig getDrivePIDF() + { + return configuration.velocityPIDF; + } + + /** + * Set the drive PIDF values. + * + * @param config {@link PIDFConfig} of that should be set. + */ + public void setDrivePIDF(PIDFConfig config) + { + configuration.velocityPIDF = config; + driveMotor.configurePIDF(config); + } + + /** + * Get the current angle/azimuth/steering motor PIDF values. + * + * @return {@link PIDFConfig} of the angle motor. + */ + public PIDFConfig getAnglePIDF() + { + return configuration.anglePIDF; + } + + /** + * Set the angle/azimuth/steering motor PID + * + * @param config {@link PIDFConfig} of that should be set. + */ + public void setAnglePIDF(PIDFConfig config) + { + configuration.anglePIDF = config; + angleMotor.configurePIDF(config); + } + + /** + * Set the desired state of the swerve module.
WARNING: If you are not using one of the functions from + * {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics} + * + * @param desiredState Desired swerve module state. + * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control. + * @param force Disables optimizations that prevent movement in the angle motor and forces the desired state + * onto the swerve module. + */ + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force) + { + applyStateOptimizations(desiredState); + applyAntiJitter(desiredState, force); + + // Cosine compensation. + double nextVelocityMetersPerSecond = configuration.useCosineCompensator + ? getCosineCompensatedVelocity(desiredState) + : desiredState.speedMetersPerSecond; + double curVelocityMetersPerSecond = lastState.speedMetersPerSecond; + desiredState.speedMetersPerSecond = nextVelocityMetersPerSecond; + + setDesiredState(desiredState, + isOpenLoop, + driveMotorFeedforward.calculateWithVelocities(curVelocityMetersPerSecond, + nextVelocityMetersPerSecond)); + } + + /** + * Set the desired state of the swerve module.
WARNING: If you are not using one of the functions from + * {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics} + * + * @param desiredState Desired swerve module state. + * @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control. + * @param driveFeedforwardVoltage Drive motor controller feedforward as a voltage. + */ + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, + double driveFeedforwardVoltage) + { + if (isOpenLoop) + { + double percentOutput = desiredState.speedMetersPerSecond / maxDriveVelocity.in(MetersPerSecond); + driveMotor.setVoltage(percentOutput * 12); + } else + { + driveMotor.setReference(desiredState.speedMetersPerSecond, driveFeedforwardVoltage); + } + + // Prevent module rotation if angle is the same as the previous angle. + // Synchronize encoders if queued and send in the current position as the value from the absolute encoder. + if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled) + { + double absoluteEncoderPosition = getAbsolutePosition(); + if (Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) + { + angleMotor.setPosition(absoluteEncoderPosition); + } + angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); + synchronizeEncoderQueued = false; + } else + { + angleMotor.setReference(desiredState.angle.getDegrees(), 0); + } + + lastState = desiredState; + + if (SwerveDriveTelemetry.isSimulation) + { + simModule.updateStateAndPosition(desiredState); + } + + // TODO: Change and move to SwerveDriveTelemetry + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) + { + SwerveDriveTelemetry.desiredStatesObj[moduleNumber] = desiredState; + } + + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + speedSetpointPublisher.set(desiredState.speedMetersPerSecond); + angleSetpointPublisher.set(desiredState.angle.getDegrees()); + } + + if (moduleNumber == SwerveDriveTelemetry.moduleCount - 1) + { + SwerveDriveTelemetry.endCtrlCycle(); + } + } + + /** + * Get the cosine compensated velocity to set the swerve module to. + * + * @param desiredState Desired {@link SwerveModuleState} to use. + * @return Cosine compensated velocity in meters/second. + */ + private double getCosineCompensatedVelocity(SwerveModuleState desiredState) + { + double cosineScalar = 1.0; + // Taken from the CTRE SwerveModule class. + // https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46 + /* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */ + /* To reduce the "skew" that occurs when changing direction */ + /* If error is close to 0 rotations, we're already there, so apply full power */ + /* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */ + cosineScalar = Rotation2d.fromDegrees(desiredState.angle.getDegrees()) + .minus(Rotation2d.fromDegrees(getAbsolutePosition())) + .getCos(); // TODO: Investigate angle modulus by 180. + /* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */ + if (cosineScalar < 0.0) + { + cosineScalar = 1; + } + + return desiredState.speedMetersPerSecond * cosineScalar; + } + + /** + * Apply the {@link SwerveModuleState#optimize(Rotation2d)} function if the module state optimization is enabled while + * debugging. + * + * @param desiredState The desired state to apply the optimization to. + */ + public void applyStateOptimizations(SwerveModuleState desiredState) + { + // SwerveModuleState optimization might be desired to be disabled while debugging. + if (optimizeSwerveModuleState) + { + desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition())); + } + } + + /** + * Apply anti-jitter to the desired state. This will prevent the module from rotating if the speed requested is too + * low. If force is true, the anti-jitter will not be applied. + * + * @param desiredState The desired state to apply the anti-jitter to. + * @param force Whether to ignore the {@link SwerveModule#antiJitterEnabled} state and apply the anti-jitter + * anyway. + */ + public void applyAntiJitter(SwerveModuleState desiredState, boolean force) + { + if (!force && antiJitterEnabled) + { + // Prevents module rotation if speed is less than 1% + SwerveMath.antiJitter(desiredState, lastState, Math.min(maxDriveVelocityMetersPerSecond, 4)); + } + } + + /** + * Set the angle for the module. + * + * @param angle Angle in degrees. + */ + public void setAngle(double angle) + { + angleMotor.setReference(angle, 0); + lastState.angle = Rotation2d.fromDegrees(angle); + } + + /** + * Get the Swerve Module state. + * + * @return Current SwerveModule state. + */ + public SwerveModuleState getState() + { + double velocity; + Rotation2d azimuth; + if (!SwerveDriveTelemetry.isSimulation) + { + velocity = driveVelocityCache.getValue(); + azimuth = Rotation2d.fromDegrees(getAbsolutePosition()); + } else + { + return simModule.getState(); + } + return new SwerveModuleState(velocity, azimuth); + } + + /** + * Get the position of the swerve module. + * + * @return {@link SwerveModulePosition} of the swerve module. + */ + public SwerveModulePosition getPosition() + { + double position; + Rotation2d azimuth; + if (!SwerveDriveTelemetry.isSimulation) + { + position = drivePositionCache.getValue(); + azimuth = Rotation2d.fromDegrees(getAbsolutePosition()); + } else + { + return simModule.getPosition(); + } + return new SwerveModulePosition(position, azimuth); + } + + /** + * Get the absolute position. Falls back to relative position on reading failure. + * + * @return Absolute encoder angle in degrees in the range [0, 360). + */ + public double getAbsolutePosition() + { + return absolutePositionCache.getValue(); + } + + /** + * Get the absolute position. Falls back to relative position on reading failure. + * + * @return Absolute encoder angle in degrees in the range [0, 360). + */ + public double getRawAbsolutePosition() + { + /* During simulation, when no absolute encoders are available, we return the state from the simulation module instead. */ + if (SwerveDriveTelemetry.isSimulation) + { + Rotation2d absolutePosition = simModule.getState().angle; + return absolutePosition.getDegrees(); + } + + double angle; + if (absoluteEncoder != null) + { + angle = absoluteEncoder.getAbsolutePosition() - angleOffset; + if (absoluteEncoder.readingError) + { + angle = getRelativePosition(); + } + } else + { + angle = getRelativePosition(); + } + if (optimizeSwerveModuleState) + { + angle %= 360; + if (angle < 0.0) + { + angle += 360; + } + } + + return angle; + } + + /** + * Get the relative angle in degrees. + * + * @return Angle in degrees. + */ + public double getRelativePosition() + { + return angleMotor.getPosition(); + } + + /** + * Set the brake mode. + * + * @param brake Set the brake mode. + */ + public void setMotorBrake(boolean brake) + { + driveMotor.setMotorBrake(brake); + } + + /** + * Set the conversion factor for the angle/azimuth motor controller. + * + * @param conversionFactor Angle motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated. + */ + public void setAngleMotorConversionFactor(double conversionFactor) + { + angleMotor.configureIntegratedEncoder(conversionFactor); + } + + /** + * Set the conversion factor for the drive motor controller. + * + * @param conversionFactor Drive motor conversion factor for PID, should be generated from + * {@link SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated. + */ + public void setDriveMotorConversionFactor(double conversionFactor) + { + driveMotor.configureIntegratedEncoder(conversionFactor); + } + + /** + * Get the angle {@link SwerveMotor} for the {@link SwerveModule}. + * + * @return {@link SwerveMotor} for the angle/steering motor of the module. + */ + public SwerveMotor getAngleMotor() + { + return angleMotor; + } + + /** + * Get the drive {@link SwerveMotor} for the {@link SwerveModule}. + * + * @return {@link SwerveMotor} for the drive motor of the module. + */ + public SwerveMotor getDriveMotor() + { + return driveMotor; + } + + /** + * Get the {@link SwerveAbsoluteEncoder} for the {@link SwerveModule}. + * + * @return {@link SwerveAbsoluteEncoder} for the swerve module. + */ + public SwerveAbsoluteEncoder getAbsoluteEncoder() + { + return absoluteEncoder; + } + + /** + * Fetch the {@link SwerveModuleConfiguration} for the {@link SwerveModule} with the parsed configurations. + * + * @return {@link SwerveModuleConfiguration} for the {@link SwerveModule}. + */ + public SwerveModuleConfiguration getConfiguration() + { + return configuration; + } + + + /** + * Use external sensors for the feedback of the angle/azimuth/steer controller. + */ + public void useExternalFeedbackSensor() + { + if (absoluteEncoder == null) + { + externalSensorIsNull.set(true); + return; + } + if (angleOffset == 0) + { + internalOffsetIsZero.set(true); + } + if (absoluteEncoder.setAbsoluteEncoderOffset(configuration.angleOffset)) + { + angleMotor.setAbsoluteEncoder(absoluteEncoder); + if (angleMotor.usingExternalFeedbackSensor()) + { + angleOffset = 0; + } else + { + externalFeedbackIncompatible.set(true); + angleMotor.setAbsoluteEncoder(null); + absoluteEncoder.setAbsoluteEncoderOffset(0); + } + + } else + { + externalOffsetIncompatible.set(true); + absoluteEncoder.setAbsoluteEncoderOffset(0); + } + } + + /** + * Use external sensors for the feedback of the angle/azimuth/steer controller. + */ + public void useInternalFeedbackSensor() + { + if (absoluteEncoder == null) + { + externalSensorIsNull.set(true); + return; + } + if (angleOffset == 0) + { + internalOffsetIsZero.set(true); + } + angleMotor.setAbsoluteEncoder(null); + absoluteEncoder.setAbsoluteEncoderOffset(0); + angleOffset = configuration.angleOffset; + } + + /** + * Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset. + */ + @Deprecated + public void pushOffsetsToEncoders() + { + if (absoluteEncoder != null && angleOffset == configuration.angleOffset) + { + // If the absolute encoder is attached. + if (angleMotor instanceof SparkMaxSwerve || angleMotor instanceof SparkMaxBrushedMotorSwerve) + { + if (absoluteEncoder instanceof SparkMaxEncoderSwerve) + { + angleMotor.setAbsoluteEncoder(absoluteEncoder); + if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset)) + { + angleOffset = 0; + } else + { + angleMotor.setAbsoluteEncoder(null); + encoderOffsetWarning.set(true); + } + } + } + + } else + { + noEncoderWarning.set(true); + } + } + + /** + * Restore internal offset in YAGSL and either sets absolute encoder offset to 0 or restores old value. + */ + public void restoreInternalOffset() + { + angleMotor.setAbsoluteEncoder(null); + absoluteEncoder.setAbsoluteEncoderOffset(0); + angleOffset = configuration.angleOffset; + } + + /** + * Get if the last Absolute Encoder had a read issue, such as it does not exist. + * + * @return If the last Absolute Encoder had a read issue, or absolute encoder does not exist. + */ + public boolean getAbsoluteEncoderReadIssue() + { + if (absoluteEncoder == null) + { + return true; + } else + { + return absoluteEncoder.readingError; + } + } + + + /** + * Get the maximum module velocity as a {@link LinearVelocity} based on the RPM and gear ratio. + * + * @return {@link LinearVelocity} max velocity of the drive wheel. + */ + public LinearVelocity getMaxVelocity() + { + getMaxDriveVelocityMetersPerSecond(); + return maxDriveVelocity; + } + + /** + * Get the maximum drive velocity of the module in Meters Per Second. + * + * @return Maximum drive motor velocity in Meters Per Second. + */ + public double getMaxDriveVelocityMetersPerSecond() + { + if (maxDriveVelocity == null) + { + maxDriveVelocity = InchesPerSecond.of( + (driveMotor.getSimMotor().freeSpeedRadPerSec / + configuration.conversionFactors.drive.gearRatio) * + configuration.conversionFactors.drive.diameter / 2.0); + maxDriveVelocityMetersPerSecond = maxDriveVelocity.in(MetersPerSecond); + } + return maxDriveVelocityMetersPerSecond; + } + + /** + * Get the maximum module angular velocity as a {@link AngularVelocity} based on the RPM and gear ratio. + * + * @return {@link AngularVelocity} max velocity of the angle/azimuth. + */ + public AngularVelocity getMaxAngularVelocity() + { + if (maxAngularVelocity == null) + { + maxAngularVelocity = RotationsPerSecond.of( + RadiansPerSecond.of(angleMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) / + configuration.conversionFactors.angle.gearRatio); + } + return maxAngularVelocity; + } + + /** + * Update data sent to {@link SmartDashboard}. + */ + public void updateTelemetry() + { + if (absoluteEncoder != null) + { + rawAbsoluteAnglePublisher.set(absoluteEncoder.getAbsolutePosition()); + } + if (SwerveDriveTelemetry.isSimulation && SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + { + SwerveModulePosition pos = simModule.getPosition(); + SwerveModuleState state = simModule.getState(); + rawAnglePublisher.set(pos.angle.getDegrees()); + rawDriveEncoderPublisher.set(pos.distanceMeters); + rawDriveVelocityPublisher.set(state.speedMetersPerSecond); + // For code coverage + angleMotor.getPosition(); + drivePositionCache.getValue(); + driveVelocityCache.getValue(); + } else + { + rawAnglePublisher.set(angleMotor.getPosition()); + rawDriveEncoderPublisher.set(drivePositionCache.getValue()); + rawDriveVelocityPublisher.set(driveVelocityCache.getValue()); + } + adjAbsoluteAnglePublisher.set(getAbsolutePosition()); + absoluteEncoderIssuePublisher.set(getAbsoluteEncoderReadIssue()); + + } + + /** + * Invalidate the {@link Cache} objects used by {@link SwerveModule}. + */ + public void invalidateCache() + { + absolutePositionCache.update(); + drivePositionCache.update(); + driveVelocityCache.update(); + } + + /** + * Obtains the {@link SwerveModuleSimulation} used in simulation. + * + * @return the module simulation, null if this method is called on a real robot + */ + public SwerveModuleSimulation getSimModule() + { + return simModule; + } + + /** + * Configure the {@link SwerveModule#simModule} with the MapleSim + * {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} + * + * @param swerveModuleSimulation MapleSim {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} to + * configure with. + * @param physicalCharacteristics {@link SwerveModulePhysicalCharacteristics} that represent the swerve drive. + */ + public void configureModuleSimulation( + org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation, + SwerveModulePhysicalCharacteristics physicalCharacteristics) + { + this.simModule.configureSimModule(swerveModuleSimulation, physicalCharacteristics); + } +} diff --git a/temp_yagsl/swervelib/math/SwerveMath.java b/temp_yagsl/swervelib/math/SwerveMath.java new file mode 100644 index 0000000..3c3de29 --- /dev/null +++ b/temp_yagsl/swervelib/math/SwerveMath.java @@ -0,0 +1,394 @@ +package swervelib.math; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +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.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import java.util.List; +import swervelib.SwerveController; +import swervelib.SwerveModule; +import swervelib.parser.SwerveDriveConfiguration; +import swervelib.parser.SwerveModuleConfiguration; + +/** Mathematical functions which pertain to swerve drive. */ +public class SwerveMath { + + /** + * Calculate the meters per rotation for the integrated encoder. Calculation: (PI * WHEEL DIAMETER + * IN METERS) / (GEAR RATIO * ENCODER RESOLUTION) + * + * @param wheelDiameter Wheel diameter in meters. + * @param driveGearRatio The gear ratio of the drive motor. + * @param pulsePerRotation The number of encoder pulses per rotation. 1 if using an integrated + * encoder. + * @return Meters per rotation for the drive motor. + */ + public static double calculateMetersPerRotation( + double wheelDiameter, double driveGearRatio, double pulsePerRotation) { + return (Math.PI * wheelDiameter) / (driveGearRatio * pulsePerRotation); + } + + /** + * Calculate the meters per rotation for the integrated encoder. Calculation: (PI * WHEEL DIAMETER + * IN METERS) / (GEAR RATIO) + * + * @param wheelDiameter Wheel diameter in meters. + * @param driveGearRatio The gear ratio of the drive motor. + * @return Meters per rotation for the drive motor. + */ + public static double calculateMetersPerRotation(double wheelDiameter, double driveGearRatio) { + return calculateMetersPerRotation(wheelDiameter, driveGearRatio, 1); + } + + /** + * Normalize an angle to be within 0 to 360. + * + * @param angle Angle in degrees. + * @return Normalized angle in degrees. + */ + public static double normalizeAngle(double angle) { + Rotation2d angleRotation = Rotation2d.fromDegrees(angle); + return new Rotation2d(angleRotation.getCos(), angleRotation.getSin()).getDegrees(); + } + + /** + * Algebraically apply a deadband using a piece wise function. + * + * @param value value to apply deadband to. + * @param scaled Use algebra to determine deadband by starting the value at 0 past deadband. + * @param deadband The deadbnad to apply. + * @return Value with deadband applied. + */ + public static double applyDeadband(double value, boolean scaled, double deadband) { + value = Math.abs(value) > deadband ? value : 0; + return scaled + ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value) + : value; + } + + /** + * Create the drive feedforward for swerve modules. + * + * @param optimalVoltage Optimal voltage to calculate kV (voltage/max Velocity) + * @param maxSpeed Maximum velocity in meters per second to use for the feed forward, should be as + * close to physical max as possible. + * @param wheelGripCoefficientOfFriction Wheel grip coefficient of friction for kA + * (voltage/(cof*9.81)) + * @return Drive feedforward for drive motor on a swerve module. + */ + public static SimpleMotorFeedforward createDriveFeedforward( + double optimalVoltage, double maxSpeed, double wheelGripCoefficientOfFriction) { + double kv = optimalVoltage / maxSpeed; + /// ^ Volt-seconds per meter (max voltage divided by max speed) + double ka = optimalVoltage / calculateMaxAcceleration(wheelGripCoefficientOfFriction); + /// ^ Volt-seconds^2 per meter (max voltage divided by max accel) + return new SimpleMotorFeedforward(0, kv, 0); + } + + /** + * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion + * values. Drive converts motor rotations to linear wheel distance and steering converts motor + * rotations to module azimuth. + * + * @param angleGearRatio The gear ratio of the steering motor. + * @param pulsePerRotation The number of pulses in a complete rotation for the encoder, 1 if + * integrated. + * @return Degrees per steering rotation for the angle motor. + */ + public static double calculateDegreesPerSteeringRotation( + double angleGearRatio, double pulsePerRotation) { + return 360 / (angleGearRatio * pulsePerRotation); + } + + /** + * Calculate the degrees per steering rotation for the integrated encoder. Encoder conversion + * values. Drive converts motor rotations to linear wheel distance and steering converts motor + * rotations to module azimuth. + * + * @param angleGearRatio The gear ratio of the steering motor. + * @return Degrees per steering rotation for the angle motor. + */ + public static double calculateDegreesPerSteeringRotation(double angleGearRatio) { + return calculateDegreesPerSteeringRotation(angleGearRatio, 1); + } + + /** + * Calculate the maximum angular velocity. + * + * @param maxSpeed Max speed of the robot in meters per second. + * @param furthestModuleX X of the furthest module in meters. + * @param furthestModuleY Y of the furthest module in meters. + * @return Maximum angular velocity in rad/s. + */ + public static double calculateMaxAngularVelocity( + double maxSpeed, double furthestModuleX, double furthestModuleY) { + return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY); + } + + /** + * Calculate the practical maximum acceleration of the robot using the wheel coefficient of + * friction. + * + * @param cof Coefficient of Friction of the wheel grip tape. + * @return Practical maximum acceleration in m/s/s. + */ + public static double calculateMaxAcceleration(double cof) { + return cof * 9.81; + } + + /** + * Calculate the maximum theoretical acceleration without friction. + * + * @param stallTorqueNm Stall torque of driving motor in nM. + * @param gearRatio Gear ratio for driving motor number of motor rotations until one wheel + * rotation. + * @param moduleCount Number of swerve modules. + * @param wheelDiameter Wheel diameter in meters. + * @param robotMass Mass of the robot in kg. + * @return Theoretical maximum acceleration in m/s/s. + */ + public static double calculateMaxAcceleration( + double stallTorqueNm, + double gearRatio, + double moduleCount, + double wheelDiameter, + double robotMass) { + return (stallTorqueNm * gearRatio * moduleCount) / ((wheelDiameter / 2) * robotMass); + } + + /** + * Calculates the maximum acceleration allowed in a direction without tipping the robot. Reads arm + * position from NetworkTables and is passed the direction in question. + * + * @param angle The direction in which to calculate max acceleration, as a Rotation2d. Note that + * this is robot-relative. + * @param matter Matter that the robot is composed of in kg. (Includes chassis) + * @param robotMass The weight of the robot in kg. (Including manipulators, etc). + * @param config The swerve drive configuration. + * @return Maximum acceleration allowed in the robot direction. + */ + private static double calcMaxAccel( + Rotation2d angle, List matter, double robotMass, SwerveDriveConfiguration config) { + // Calculate the vertical mass moment using the floor as the datum. This will be used later to + // calculate max acceleration + Translation3d centerMass = new Translation3d(); + for (Matter object : matter) { + centerMass = centerMass.plus(object.massMoment()); + } + Translation3d robotCG = centerMass.div(robotMass); + Translation2d horizontalCG = robotCG.toTranslation2d(); + + Translation2d projectedHorizontalCg = + new Translation2d( + (angle.getSin() * angle.getCos() * horizontalCG.getY()) + + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()), + (angle.getSin() * angle.getCos() * horizontalCG.getX()) + + (Math.pow(angle.getSin(), 2) * horizontalCG.getY())); + + // Projects the edge of the wheelbase onto the direction line. Assumes the wheelbase is + // rectangular. + // Because a line is being projected, rather than a point, one of the coordinates of the + // projected point is + // already known. + Translation2d projectedWheelbaseEdge; + double angDeg = angle.getDegrees(); + if (angDeg <= 45 && angDeg >= -45) { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); + } else if (135 >= angDeg && angDeg > 45) { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); + } else if (-135 <= angDeg && angDeg < -45) { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, false); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY()); + } else { + SwerveModuleConfiguration conf = getSwerveModule(config.modules, false, true); + projectedWheelbaseEdge = + new Translation2d( + conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan()); + } + + double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm(); + return 9.81 * horizontalDistance / robotCG.getZ(); + } + + /** + * Logical inverse of the Pose exponential from 254. Taken from team 3181. + * + * @param transform Pose to perform the log on. + * @return {@link Twist2d} of the transformed pose. + */ + public static Twist2d PoseLog(final Pose2d transform) { + + final double kEps = 1E-9; + final double dtheta = transform.getRotation().getRadians(); + final double half_dtheta = 0.5 * dtheta; + final double cos_minus_one = transform.getRotation().getCos() - 1.0; + double halftheta_by_tan_of_halfdtheta; + if (Math.abs(cos_minus_one) < kEps) { + halftheta_by_tan_of_halfdtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta; + } else { + halftheta_by_tan_of_halfdtheta = + -(half_dtheta * transform.getRotation().getSin()) / cos_minus_one; + } + final Translation2d translation_part = + transform + .getTranslation() + .rotateBy(new Rotation2d(halftheta_by_tan_of_halfdtheta, -half_dtheta)); + return new Twist2d(translation_part.getX(), translation_part.getY(), dtheta); + } + + /** + * Limits a commanded velocity to prevent exceeding the maximum acceleration given by {@link + * SwerveMath#calcMaxAccel}. Note that this takes and returns field-relative velocities. + * + * @param commandedVelocity The desired velocity + * @param fieldVelocity The velocity of the robot within a field relative state. + * @param robotPose The current pose of the robot. + * @param loopTime The time it takes to update the velocity in seconds. Note: this should + * include the 100ms that it takes for a SparkMax velocity to update. + * @param matter Matter that the robot is composed of with position in meters and mass in kg. + * @param robotMass The weight of the robot in kg. (Including manipulators, etc). + * @param config The swerve drive configuration. + * @return The limited velocity. This is either the commanded velocity, if attainable, or the + * closest attainable velocity. + */ + public static Translation2d limitVelocity( + Translation2d commandedVelocity, + ChassisSpeeds fieldVelocity, + Pose2d robotPose, + double loopTime, + double robotMass, + List matter, + SwerveDriveConfiguration config) { + // Get the robot's current field-relative velocity + Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity); + + // Calculate the commanded change in velocity by subtracting current velocity + // from commanded velocity + Translation2d deltaV = commandedVelocity.minus(currentVelocity); + + // Creates an acceleration vector with the direction of delta V and a magnitude + // of the maximum allowed acceleration in that direction + Translation2d maxAccel = + new Translation2d( + calcMaxAccel( + deltaV + // Rotates the velocity vector to convert from field-relative to robot-relative + .rotateBy(robotPose.getRotation().unaryMinus()) + .getAngle(), + matter, + robotMass, + config), + deltaV.getAngle()); + + // Calculate the maximum achievable velocity by the next loop cycle. + // delta V = Vf - Vi = at + Translation2d maxAchievableDeltaVelocity = maxAccel.times(loopTime); + + if (deltaV.getNorm() > maxAchievableDeltaVelocity.getNorm()) { + return maxAchievableDeltaVelocity.plus(currentVelocity); + } else { + // If the commanded velocity is attainable, use that. + return commandedVelocity; + } + } + + /** + * Get the fruthest module from center based on the module locations. + * + * @param modules Swerve module list. + * @param front True = furthest front, False = furthest back. + * @param left True = furthest left, False = furthest right. + * @return Module location which is the furthest from center and abides by parameters. + */ + public static SwerveModuleConfiguration getSwerveModule( + SwerveModule[] modules, boolean front, boolean left) { + Translation2d target = modules[0].configuration.moduleLocation, current, temp; + SwerveModuleConfiguration configuration = modules[0].configuration; + for (SwerveModule module : modules) { + current = module.configuration.moduleLocation; + temp = + front + ? (target.getY() >= current.getY() ? current : target) + : (target.getY() <= current.getY() ? current : target); + target = + left + ? (target.getX() >= temp.getX() ? temp : target) + : (target.getX() <= temp.getX() ? temp : target); + configuration = current.equals(target) ? module.configuration : configuration; + } + return configuration; + } + + /** + * Put an angle within the 360 deg scope of a reference. For example, given a scope reference of + * 756 degrees, assumes the full scope is (720-1080), and places an angle of 22 degrees into it, + * returning 742 deg. + * + *

A more formal definition: returns the closest angle {@code n} to {@code scopeReference} such + * that {@code n} is congruent to {@code newAngle}. + * + * @param scopeReference Current Angle (deg) + * @param newAngle Target Angle (deg) + * @return Closest angle within scope (deg) + */ + public static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) { + // Figure out how many revolutions from the angle to the reference + double diffRevs = Math.round((scopeReference - newAngle) / 360) * 360; + + // Add that many revolutions + return diffRevs + newAngle; + } + + /** + * Perform anti-jitter within modules if the speed requested is too low. + * + * @param moduleState Current {@link SwerveModuleState} requested. + * @param lastModuleState Previous {@link SwerveModuleState} used. + * @param maxSpeed Maximum speed of the modules. + */ + public static void antiJitter( + SwerveModuleState moduleState, SwerveModuleState lastModuleState, double maxSpeed) { + if (Math.abs(moduleState.speedMetersPerSecond) <= (maxSpeed * 0.01)) { + moduleState.angle = lastModuleState.angle; + } + } + + /** + * Cube the {@link Translation2d} magnitude given in Polar coordinates. + * + * @param translation {@link Translation2d} to manipulate. + * @return Cubed magnitude from {@link Translation2d}. + */ + public static Translation2d cubeTranslation(Translation2d translation) { + if (Math.hypot(translation.getX(), translation.getY()) <= 1.0E-6) { + return translation; + } + return new Translation2d(Math.pow(translation.getNorm(), 3), translation.getAngle()); + } + + /** + * Scale the {@link Translation2d} Polar coordinate magnitude. + * + * @param translation {@link Translation2d} to use. + * @param scalar Multiplier for the Polar coordinate magnitude to use. + * @return {@link Translation2d} scaled by given magnitude scalar. + */ + public static Translation2d scaleTranslation(Translation2d translation, double scalar) { + if (Math.hypot(translation.getX(), translation.getY()) <= 1.0E-6) { + return translation; + } + return new Translation2d(translation.getNorm() * scalar, translation.getAngle()); + } +} diff --git a/temp_yagsl/swervelib/parser/SwerveDriveConfiguration.java b/temp_yagsl/swervelib/parser/SwerveDriveConfiguration.java new file mode 100644 index 0000000..433f725 --- /dev/null +++ b/temp_yagsl/swervelib/parser/SwerveDriveConfiguration.java @@ -0,0 +1,139 @@ +package swervelib.parser; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import java.util.function.Supplier; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.GyroSimulation; +import swervelib.SwerveModule; +import swervelib.imu.NavXSwerve; +import swervelib.imu.Pigeon2Swerve; +import swervelib.imu.SwerveIMU; +import swervelib.math.SwerveMath; + +/** Swerve drive configurations used during SwerveDrive construction. */ +public class SwerveDriveConfiguration { + + /** Number of modules on the robot. */ + public final int moduleCount; + /** Swerve Module locations. */ + public Translation2d[] moduleLocationsMeters; + /** Swerve IMU */ + public SwerveIMU imu; + /** Swerve Modules. */ + public SwerveModule[] modules; + /** Physical characteristics of the swerve drive from physicalproperties.json. */ + public SwerveModulePhysicalCharacteristics physicalCharacteristics; + + /** + * Create swerve drive configuration. + * + * @param moduleConfigs Module configuration. + * @param swerveIMU Swerve IMU. + * @param invertedIMU Invert the IMU. + * @param physicalCharacteristics {@link SwerveModulePhysicalCharacteristics} to store in + * association with self. + */ + public SwerveDriveConfiguration( + SwerveModuleConfiguration[] moduleConfigs, + SwerveIMU swerveIMU, + boolean invertedIMU, + SwerveModulePhysicalCharacteristics physicalCharacteristics) { + this.moduleCount = moduleConfigs.length; + this.imu = swerveIMU; + swerveIMU.setInverted(invertedIMU); + this.modules = createModules(moduleConfigs); + this.moduleLocationsMeters = new Translation2d[moduleConfigs.length]; + for (SwerveModule module : modules) { + this.moduleLocationsMeters[module.moduleNumber] = module.configuration.moduleLocation; + } + this.physicalCharacteristics = physicalCharacteristics; + } + + /** + * Create modules based off of the SwerveModuleConfiguration. + * + * @param swerves Swerve constants. + * @return Swerve Modules. + */ + public SwerveModule[] createModules(SwerveModuleConfiguration[] swerves) { + SwerveModule[] modArr = new SwerveModule[swerves.length]; + for (int i = 0; i < swerves.length; i++) { + modArr[i] = new SwerveModule(i, swerves[i]); + } + return modArr; + } + + /** + * Calculate the Drive Base Radius + * + * @return Drive base radius from center of robot to the farthest wheel in meters. + */ + public double getDriveBaseRadiusMeters() { + Translation2d centerOfModules = moduleLocationsMeters[0]; + + // Calculate the Center by adding all module offsets together. + for (int i = 1; i < moduleLocationsMeters.length; i++) { + centerOfModules = centerOfModules.plus(moduleLocationsMeters[i]); + } + + // Return Largest Radius + return centerOfModules.getDistance(moduleLocationsMeters[0]); + } + + /** + * Get the trackwidth of the swerve modules. + * + * @return Effective trackwdtih in Meters + */ + public double getTrackwidth() { + SwerveModuleConfiguration fr = SwerveMath.getSwerveModule(modules, true, false); + SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true); + return fr.moduleLocation.getDistance(fl.moduleLocation); + } + + /** + * Get the tracklength of the swerve modules. + * + * @return Effective tracklength in Meters + */ + public double getTracklength() { + SwerveModuleConfiguration br = SwerveMath.getSwerveModule(modules, false, false); + SwerveModuleConfiguration bl = SwerveMath.getSwerveModule(modules, false, true); + return br.moduleLocation.getDistance(bl.moduleLocation); + } + + /** + * Get the {@link DCMotor} corresponding to the first module's configuration. + * + * @return {@link DCMotor} of the drive motor. + */ + public DCMotor getDriveMotorSim() { + SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true); + return fl.driveMotor.getSimMotor(); + } + + /** + * Get the {@link DCMotor} corresponding to the first module configuration. + * + * @return {@link DCMotor} of the angle motor. + */ + public DCMotor getAngleMotorSim() { + SwerveModuleConfiguration fl = SwerveMath.getSwerveModule(modules, true, true); + return fl.angleMotor.getSimMotor(); + } + + /** + * Get the gyro simulation for the robot. + * + * @return {@link GyroSimulation} gyro simulation. + */ + public Supplier getGyroSim() { + if (imu instanceof Pigeon2Swerve) { + return COTS.ofPigeon2(); + } else if (imu instanceof NavXSwerve) { + return COTS.ofNav2X(); + } + return COTS.ofGenericGyro(); + } +} diff --git a/temp_yagsl/swervelib/parser/SwerveModuleConfiguration.java b/temp_yagsl/swervelib/parser/SwerveModuleConfiguration.java new file mode 100644 index 0000000..1fa3f4e --- /dev/null +++ b/temp_yagsl/swervelib/parser/SwerveModuleConfiguration.java @@ -0,0 +1,141 @@ +package swervelib.parser; + +import edu.wpi.first.math.geometry.Translation2d; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.motors.SwerveMotor; +import swervelib.parser.json.modules.ConversionFactorsJson; + +/** Swerve Module configuration class which is used to configure {@link swervelib.SwerveModule}. */ +public class SwerveModuleConfiguration { + + /** + * Conversion factor for drive motor onboard PID's and angle PID's. Use {@link + * swervelib.math.SwerveMath#calculateMetersPerRotation(double, double, double)} and {@link + * swervelib.math.SwerveMath#calculateDegreesPerSteeringRotation(double, double)} respectively to + * calculate the conversion factors. + */ + public final ConversionFactorsJson conversionFactors; + /** Angle offset in degrees for the Swerve Module. */ + public final double angleOffset; + /** Whether the absolute encoder is inverted. */ + public final boolean absoluteEncoderInverted; + /** State of inversion of the drive motor. */ + public final boolean driveMotorInverted; + /** State of inversion of the angle motor. */ + public final boolean angleMotorInverted; + /** PIDF configuration options for the angle motor closed-loop PID controller. */ + public PIDFConfig anglePIDF; + /** PIDF configuration options for the drive motor closed-loop PID controller. */ + public PIDFConfig velocityPIDF; + /** Swerve module location relative to the robot. */ + public Translation2d moduleLocation; + /** Physical characteristics of the swerve module. */ + public SwerveModulePhysicalCharacteristics physicalCharacteristics; + /** The drive motor and angle motor of this swerve module. */ + public SwerveMotor driveMotor, angleMotor; + /** The Absolute Encoder for the swerve module. */ + public SwerveAbsoluteEncoder absoluteEncoder; + /** Name for the swerve module for telemetry. */ + public String name; + /** Should do cosine compensation when not pointing correct direction;. */ + public boolean useCosineCompensator; + + /** + * Construct a configuration object for swerve modules. + * + * @param driveMotor Drive {@link SwerveMotor}. + * @param angleMotor Angle {@link SwerveMotor} + * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. + * @param angleOffset Absolute angle offset to 0. + * @param absoluteEncoderInverted Absolute encoder inverted. + * @param angleMotorInverted State of inversion of the angle motor. + * @param driveMotorInverted Drive motor inverted. + * @param xMeters Module location in meters from the center horizontally. + * @param yMeters Module location in meters from center vertically. + * @param anglePIDF Angle PIDF configuration. + * @param velocityPIDF Velocity PIDF configuration. + * @param physicalCharacteristics Physical characteristics of the swerve module. + * @param name The name for the swerve module. + * @param conversionFactors Conversion factors to be applied to the drive and angle motors. + * @param useCosineCompensator Should use cosineCompensation. + */ + public SwerveModuleConfiguration( + SwerveMotor driveMotor, + SwerveMotor angleMotor, + ConversionFactorsJson conversionFactors, + SwerveAbsoluteEncoder absoluteEncoder, + double angleOffset, + double xMeters, + double yMeters, + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + SwerveModulePhysicalCharacteristics physicalCharacteristics, + boolean absoluteEncoderInverted, + boolean driveMotorInverted, + boolean angleMotorInverted, + String name, + boolean useCosineCompensator) { + this.driveMotor = driveMotor; + this.angleMotor = angleMotor; + this.conversionFactors = conversionFactors; + this.absoluteEncoder = absoluteEncoder; + this.angleOffset = angleOffset; + this.absoluteEncoderInverted = absoluteEncoderInverted; + this.driveMotorInverted = driveMotorInverted; + this.angleMotorInverted = angleMotorInverted; + this.moduleLocation = new Translation2d(xMeters, yMeters); + this.anglePIDF = anglePIDF; + this.velocityPIDF = velocityPIDF; + this.physicalCharacteristics = physicalCharacteristics; + this.name = name; + this.useCosineCompensator = useCosineCompensator; + } + + /** + * Construct a configuration object for swerve modules. Assumes the absolute encoder and drive + * motor are not inverted. + * + * @param driveMotor Drive {@link SwerveMotor}. + * @param angleMotor Angle {@link SwerveMotor} + * @param conversionFactors Conversion factors for angle/azimuth motors drive factors. + * @param absoluteEncoder Absolute encoder {@link SwerveAbsoluteEncoder}. + * @param angleOffset Absolute angle offset to 0. + * @param xMeters Module location in meters from the center horizontally. + * @param yMeters Module location in meters from center vertically. + * @param anglePIDF Angle PIDF configuration. + * @param velocityPIDF Velocity PIDF configuration. + * @param physicalCharacteristics Physical characteristics of the swerve module. + * @param name Name for the module. + * @param useCosineCompensator Should use cosineCompensation. + */ + public SwerveModuleConfiguration( + SwerveMotor driveMotor, + SwerveMotor angleMotor, + ConversionFactorsJson conversionFactors, + SwerveAbsoluteEncoder absoluteEncoder, + double angleOffset, + double xMeters, + double yMeters, + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + SwerveModulePhysicalCharacteristics physicalCharacteristics, + String name, + boolean useCosineCompensator) { + this( + driveMotor, + angleMotor, + conversionFactors, + absoluteEncoder, + angleOffset, + xMeters, + yMeters, + anglePIDF, + velocityPIDF, + physicalCharacteristics, + false, + false, + false, + name, + useCosineCompensator); + } +} diff --git a/temp_yagsl/swervelib/parser/SwerveParser.java b/temp_yagsl/swervelib/parser/SwerveParser.java new file mode 100644 index 0000000..47ea5e2 --- /dev/null +++ b/temp_yagsl/swervelib/parser/SwerveParser.java @@ -0,0 +1,204 @@ +package swervelib.parser; + +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.geometry.Pose2d; +import java.io.File; +import java.io.IOException; +import java.util.HashMap; +import swervelib.SwerveDrive; +import swervelib.SwerveModule; +import swervelib.math.SwerveMath; +import swervelib.parser.json.ControllerPropertiesJson; +import swervelib.parser.json.ModuleJson; +import swervelib.parser.json.PIDFPropertiesJson; +import swervelib.parser.json.PhysicalPropertiesJson; +import swervelib.parser.json.SwerveDriveJson; + +/** + * Helper class used to parse the JSON directory with specified configuration options. + */ +public class SwerveParser +{ + + /** + * Module number mapped to the JSON name. + */ + private static final HashMap moduleConfigs = new HashMap<>(); + /** + * Parsed swervedrive.json + */ + public static SwerveDriveJson swerveDriveJson; + /** + * Parsed controllerproperties.json + */ + public static ControllerPropertiesJson controllerPropertiesJson; + /** + * Parsed modules/pidfproperties.json + */ + public static PIDFPropertiesJson pidfPropertiesJson; + /** + * Parsed modules/physicalproperties.json + */ + public static PhysicalPropertiesJson physicalPropertiesJson; + /** + * Array holding the module jsons given in {@link SwerveDriveJson}. + */ + public static ModuleJson[] moduleJsons; + + /** + * Construct a swerve parser. Will throw an error if there is a missing file. + * + * @param directory Directory with swerve configurations. + * @throws IOException if a file doesn't exist. + */ + public SwerveParser(File directory) throws IOException + { + checkDirectory(directory); + swerveDriveJson = + new ObjectMapper() + .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false) + .readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class); + controllerPropertiesJson = + new ObjectMapper() + .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false) + .readValue( + new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class); + pidfPropertiesJson = + new ObjectMapper() + .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false) + .readValue( + new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class); + physicalPropertiesJson = + new ObjectMapper() + .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false) + .readValue( + new File(directory, "modules/physicalproperties.json"), + PhysicalPropertiesJson.class); + moduleJsons = new ModuleJson[swerveDriveJson.modules.length]; + for (int i = 0; i < moduleJsons.length; i++) + { + moduleConfigs.put(swerveDriveJson.modules[i], i); + File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]); + assert moduleFile.exists(); + moduleJsons[i] = new ObjectMapper() + .configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false) + .readValue(moduleFile, ModuleJson.class); + } + } + + /** + * Get the swerve module by the json name. + * + * @param name JSON name. + * @param driveConfiguration {@link SwerveDriveConfiguration} to pull from. + * @return {@link SwerveModuleConfiguration} based on the file. + */ + public static SwerveModule getModuleConfigurationByName( + String name, SwerveDriveConfiguration driveConfiguration) + { + return driveConfiguration.modules[moduleConfigs.get(name + ".json")]; + } + + /** + * Open JSON file. + * + * @param file JSON File to open. + * @return JsonNode of file. + */ + private JsonNode openJson(File file) + { + try + { + return new ObjectMapper().readTree(file); + } catch (IOException e) + { + throw new RuntimeException(e); + } + } + + /** + * Check directory structure. + * + * @param directory JSON Configuration Directory + */ + private void checkDirectory(File directory) + { + assert new File(directory, "swervedrive.json").exists(); + assert new File(directory, "controllerproperties.json").exists(); + assert new File(directory, "modules").exists() && new File(directory, "modules").isDirectory(); + assert new File(directory, "modules/pidfproperties.json").exists(); + assert new File(directory, "modules/physicalproperties.json").exists(); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular acceleration used in + * {@link swervelib.SwerveController} and drive feedforward in + * {@link SwerveMath#createDriveFeedforward(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(double maxSpeed) + { + return createSwerveDrive(maxSpeed, Pose2d.kZero); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param maxSpeed Maximum speed of the robot in meters per second, used for both angular + * acceleration used in {@link swervelib.SwerveController} and drive feedforward in + * {@link SwerveMath#createDriveFeedforward(double, double, double)}. + * @param angleMotorConversionFactor Angle (AKA azimuth) motor conversion factor to convert motor controller PID loop + * units to degrees, usually created using + * {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)}. + * @param driveMotorConversion Drive motor conversion factor to convert motor controller PID loop units to + * meters per rotation, usually created using + * {@link SwerveMath#calculateMetersPerRotation(double, double, double)}. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(double maxSpeed, double angleMotorConversionFactor, double driveMotorConversion) + { + physicalPropertiesJson.conversionFactors.angle.factor = angleMotorConversionFactor; + physicalPropertiesJson.conversionFactors.drive.factor = driveMotorConversion; + return createSwerveDrive(maxSpeed, Pose2d.kZero); + } + + /** + * Create {@link SwerveDrive} from JSON configuration directory. + * + * @param maxSpeed Maximum speed of the robot in meters per second for normal+angular acceleration in + * {@link swervelib.SwerveController} of the robot + * @param initialPose {@link Pose2d} initial pose. + * @return {@link SwerveDrive} instance. + */ + public SwerveDrive createSwerveDrive(double maxSpeed, Pose2d initialPose) + { + SwerveModuleConfiguration[] moduleConfigurations = + new SwerveModuleConfiguration[moduleJsons.length]; + for (int i = 0; i < moduleConfigurations.length; i++) + { + ModuleJson module = moduleJsons[i]; + moduleConfigurations[i] = + module.createModuleConfiguration( + pidfPropertiesJson.angle, + pidfPropertiesJson.drive, + physicalPropertiesJson.createPhysicalProperties(), + swerveDriveJson.modules[i]); + } + SwerveDriveConfiguration swerveDriveConfiguration = + new SwerveDriveConfiguration( + moduleConfigurations, + swerveDriveJson.imu.createIMU(), + swerveDriveJson.invertedIMU, + physicalPropertiesJson.createPhysicalProperties()); + + return new SwerveDrive( + swerveDriveConfiguration, + controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration, maxSpeed), + maxSpeed, + initialPose); + } +} diff --git a/temp_yagsl/swervelib/parser/json/ModuleJson.java b/temp_yagsl/swervelib/parser/json/ModuleJson.java new file mode 100644 index 0000000..9d00c88 --- /dev/null +++ b/temp_yagsl/swervelib/parser/json/ModuleJson.java @@ -0,0 +1,141 @@ +package swervelib.parser.json; + +import com.revrobotics.spark.SparkMax; +import edu.wpi.first.math.util.Units; +import swervelib.encoders.SparkMaxEncoderSwerve; +import swervelib.encoders.SwerveAbsoluteEncoder; +import swervelib.encoders.ThriftyNovaEncoderSwerve; +import swervelib.motors.SwerveMotor; +import swervelib.motors.ThriftyNovaSwerve; +import swervelib.parser.PIDFConfig; +import swervelib.parser.SwerveModuleConfiguration; +import swervelib.parser.SwerveModulePhysicalCharacteristics; +import swervelib.parser.json.modules.BoolMotorJson; +import swervelib.parser.json.modules.ConversionFactorsJson; +import swervelib.parser.json.modules.LocationJson; + +/** + * {@link swervelib.SwerveModule} JSON parsed class. Used to access the JSON data. + */ +public class ModuleJson +{ + + /** + * Drive motor device configuration. + */ + public DeviceJson drive; + /** + * Angle motor device configuration. + */ + public DeviceJson angle; + /** + * Conversion Factors composition. Auto-calculates the conversion factors. + */ + public ConversionFactorsJson conversionFactors = new ConversionFactorsJson(); + /** + * Absolute encoder device configuration. + */ + public DeviceJson encoder; + /** + * Defines which motors are inverted. + */ + public BoolMotorJson inverted; + /** + * Absolute encoder offset from 0 in degrees. + */ + public double absoluteEncoderOffset; + /** + * Absolute encoder inversion state. + */ + public boolean absoluteEncoderInverted = false; + /** + * The location of the swerve module from the center of the robot in inches. + */ + public LocationJson location; + /** + * Should do cosine compensation when not pointing correct direction;. + */ + public boolean useCosineCompensator = true; + + /** + * Create the swerve module configuration based off of parsed data. + * + * @param anglePIDF The PIDF values for the angle motor. + * @param velocityPIDF The velocity PIDF values for the drive motor. + * @param physicalCharacteristics Physical characteristics of the swerve module. + * @param name Module json filename. + * @return {@link SwerveModuleConfiguration} based on the provided data and parsed data. + */ + public SwerveModuleConfiguration createModuleConfiguration( + PIDFConfig anglePIDF, + PIDFConfig velocityPIDF, + SwerveModulePhysicalCharacteristics physicalCharacteristics, + String name) + { + SwerveMotor angleMotor = angle.createMotor(false); + SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor); + + //Throw an error if module locations are improperly set + if (location.front == 0 && location.left == 0) + { + throw new RuntimeException("Improper Module Location Settings!\n" + + "Your module location is set to 0 for both 'front' and 'left' values.\n" + + "Set the distance from the center of the robot to the center of the wheel in your module JSON file!"); + } + + // Set the conversion factors to null if they are both 0. + if (!conversionFactors.works() && physicalCharacteristics.conversionFactor == null) + { + throw new RuntimeException("No Conversion Factor configured! Please create SwerveDrive using \n" + + "SwerveParser.createSwerveDrive(driveFeedforward, maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" + + "OR\n" + + "SwerveParser.createSwerveDrive(maxSpeed, angleMotorConversionFactor, driveMotorConversion)\n" + + "OR\n" + + "Set the conversion factor in physicalproperties.json OR the module JSON file." + + "REMEMBER: You can calculate the conversion factors using SwerveMath.calculateMetersPerRotation AND SwerveMath.calculateDegreesPerSteeringRotation\n"); + } else if (physicalCharacteristics.conversionFactor.works() && !conversionFactors.works()) + { + conversionFactors = physicalCharacteristics.conversionFactor; + } else if (physicalCharacteristics.conversionFactor.works()) + // If both are defined, override 0 with the physical characterstics input. + { + conversionFactors.angle = conversionFactors.isAngleEmpty() ? physicalCharacteristics.conversionFactor.angle + : conversionFactors.angle; + conversionFactors.drive = conversionFactors.isDriveEmpty() ? physicalCharacteristics.conversionFactor.drive + : conversionFactors.drive; + } + + if (conversionFactors.isDriveEmpty() || conversionFactors.isAngleEmpty()) + { + throw new RuntimeException( + "Conversion factors cannot be 0, please configure conversion factors in physicalproperties.json or the module JSON files."); + } + + // Backwards compatibility, auto-optimization. + if (conversionFactors.angle.factor == 360 && absEncoder != null && + (absEncoder instanceof SparkMaxEncoderSwerve && angleMotor.getMotor() instanceof SparkMax)) + { + angleMotor.setAbsoluteEncoder(absEncoder); + } else if ((absEncoder instanceof ThriftyNovaEncoderSwerve && angleMotor instanceof ThriftyNovaSwerve)) + { + angleMotor.setAbsoluteEncoder(absEncoder); + } + + return new SwerveModuleConfiguration( + drive.createMotor(true), + angleMotor, + conversionFactors, + absEncoder, + absoluteEncoderOffset, + Units.inchesToMeters(Math.round(location.front)), + Units.inchesToMeters(Math.round(location.left)), + anglePIDF, + velocityPIDF, + physicalCharacteristics, + absoluteEncoderInverted, + inverted.drive, + inverted.angle, + name.replaceAll("\\.json", ""), + useCosineCompensator); + } +} diff --git a/temp_yagsl/swervelib/parser/json/modules/AngleConversionFactorsJson.java b/temp_yagsl/swervelib/parser/json/modules/AngleConversionFactorsJson.java new file mode 100644 index 0000000..f946f66 --- /dev/null +++ b/temp_yagsl/swervelib/parser/json/modules/AngleConversionFactorsJson.java @@ -0,0 +1,27 @@ +package swervelib.parser.json.modules; + +import swervelib.math.SwerveMath; + +/** Angle motor conversion factors composite JSON parse class. */ +public class AngleConversionFactorsJson { + + /** + * Gear ratio for the angle/steering/azimuth motor on the Swerve Module. Motor rotations to 1 + * wheel rotation. + */ + public double gearRatio; + /** Calculated or given conversion factor. */ + public double factor = 0; + + /** + * Calculate the drive conversion factor. + * + * @return Drive conversion factor, if factor isn't set. + */ + public double calculate() { + if (factor == 0) { + factor = SwerveMath.calculateDegreesPerSteeringRotation(gearRatio); + } + return factor; + } +} diff --git a/temp_yagsl/swervelib/parser/json/modules/ConversionFactorsJson.java b/temp_yagsl/swervelib/parser/json/modules/ConversionFactorsJson.java new file mode 100644 index 0000000..8d69d3f --- /dev/null +++ b/temp_yagsl/swervelib/parser/json/modules/ConversionFactorsJson.java @@ -0,0 +1,40 @@ +package swervelib.parser.json.modules; + +/** Conversion Factors parsed JSON class */ +public class ConversionFactorsJson { + + /** Drive motor conversion factors composition. */ + public DriveConversionFactorsJson drive = new DriveConversionFactorsJson(); + /** Angle motor conversion factors composition. */ + public AngleConversionFactorsJson angle = new AngleConversionFactorsJson(); + + /** + * Check if the conversion factors are set for the drive motor. + * + * @return Empty + */ + public boolean isDriveEmpty() { + drive.calculate(); + return drive.factor == 0; + } + + /** + * Check if the conversion factors are set for the angle motor. + * + * @return Empty + */ + public boolean isAngleEmpty() { + angle.calculate(); + return angle.factor == 0; + } + + /** + * Check if the conversion factor can be found. + * + * @return If the conversion factors can be found. + */ + public boolean works() { + return (angle.factor != 0 && drive.factor != 0) + || ((drive.gearRatio != 0 && drive.diameter != 0)) && (angle.gearRatio != 0); + } +} diff --git a/temp_yagsl/swervelib/parser/json/modules/DriveConversionFactorsJson.java b/temp_yagsl/swervelib/parser/json/modules/DriveConversionFactorsJson.java new file mode 100644 index 0000000..55144f7 --- /dev/null +++ b/temp_yagsl/swervelib/parser/json/modules/DriveConversionFactorsJson.java @@ -0,0 +1,29 @@ +package swervelib.parser.json.modules; + +import edu.wpi.first.math.util.Units; +import swervelib.math.SwerveMath; + +/** Drive motor composite JSON parse class. */ +public class DriveConversionFactorsJson { + + /** Gear ratio for the drive motor rotations to turn the wheel 1 complete rotation. */ + public double gearRatio; + /** Diameter of the wheel in inches. */ + public double diameter; + /** Calculated conversion factor. */ + public double factor = 0; + + /** + * Calculate the drive conversion factor. + * + * @return Drive conversion factor, if factor isn't set. + */ + public double calculate() { + if (factor == 0) { + factor = + SwerveMath.calculateMetersPerRotation( + Units.inchesToMeters(this.diameter), this.gearRatio); + } + return factor; + } +} diff --git a/temp_yagsl/swervelib/parser/json/modules/LocationJson.java b/temp_yagsl/swervelib/parser/json/modules/LocationJson.java new file mode 100644 index 0000000..d795325 --- /dev/null +++ b/temp_yagsl/swervelib/parser/json/modules/LocationJson.java @@ -0,0 +1,14 @@ +package swervelib.parser.json.modules; + +/** + * Location JSON parsed class. Used to access the JSON data. Module locations, in inches, as + * distances to the center of the robot. +x is towards the robot front, and +y is towards robot + * left. + */ +public class LocationJson { + + /** Location of the swerve module in inches from the center of the robot horizontally. */ + public double front = 0; + /** Location of the swerve module in inches from the center of the robot vertically. */ + public double left = 0; +} diff --git a/temp_yagsl/swervelib/simulation/SwerveIMUSimulation.java b/temp_yagsl/swervelib/simulation/SwerveIMUSimulation.java new file mode 100644 index 0000000..aa92c7e --- /dev/null +++ b/temp_yagsl/swervelib/simulation/SwerveIMUSimulation.java @@ -0,0 +1,108 @@ +package swervelib.simulation; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import java.util.Optional; +import org.ironmaple.simulation.drivesims.GyroSimulation; + +/** + * Simulation for {@link swervelib.SwerveDrive} IMU. + */ +public class SwerveIMUSimulation +{ + + private final GyroSimulation gyroSimulation; + + /** + * Create the swerve drive IMU simulation. + * + * @param gyroSimulation Gyro simulation from MapleSim. + */ + public SwerveIMUSimulation(GyroSimulation gyroSimulation) + { + this.gyroSimulation = gyroSimulation; + } + + /** + * Get the estimated angle of the robot. + * + * @return {@link Rotation2d} estimation of the robot. + */ + public Rotation2d getYaw() + { + return gyroSimulation.getGyroReading(); + } + + /** + * Pitch is not simulated currently, always returns 0. + * + * @return Pitch of the robot as {@link Rotation2d}. + */ + public Rotation2d getPitch() + { + return new Rotation2d(); + } + + /** + * Roll is not simulated currently, always returns 0. + * + * @return Roll of the robot as {@link Rotation2d}. + */ + public Rotation2d getRoll() + { + return new Rotation2d(); + } + + /** + * Gets the estimated gyro {@link Rotation3d} of the robot. + * + * @return The heading as a {@link Rotation3d} angle + */ + public Rotation3d getGyroRotation3d() + { + return new Rotation3d(0, 0, getYaw().getRadians()); + } + + /** + * Fetch the acceleration [x, y, z] from the IMU in m/s/s. If acceleration isn't supported returns empty. + * + * @return {@link Translation3d} of the acceleration as an {@link Optional}. + */ + public Optional getAccel() + { + return Optional.empty(); + } + + /** + * Update the odometry of the simulated {@link swervelib.SwerveDrive} and post the {@link swervelib.SwerveModule} + * states to the {@link Field2d}. + * + * @param kinematics {@link SwerveDriveKinematics} of the swerve drive. + * @param states {@link SwerveModuleState} array of the module states. + * @param modulePoses {@link Pose2d} representing the swerve modules. + * @param field {@link Field2d} to update. + */ + public void updateOdometry( + SwerveDriveKinematics kinematics, + SwerveModuleState[] states, + Pose2d[] modulePoses, + Field2d field) + { + field.getObject("XModules").setPoses(modulePoses); + } + + /** + * Set the heading of the robot. + * + * @param angle Angle of the robot in radians. + */ + public void setAngle(double angle) + { + this.gyroSimulation.setRotation(Rotation2d.fromRadians(angle)); + } +} diff --git a/temp_yagsl/swervelib/simulation/SwerveModuleSimulation.java b/temp_yagsl/swervelib/simulation/SwerveModuleSimulation.java new file mode 100644 index 0000000..41bc138 --- /dev/null +++ b/temp_yagsl/swervelib/simulation/SwerveModuleSimulation.java @@ -0,0 +1,100 @@ +package swervelib.simulation; + +import static edu.wpi.first.units.Units.Amps; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import org.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation; +import swervelib.SwerveDrive; +import swervelib.parser.SwerveModulePhysicalCharacteristics; + +/** + * Class that wraps around {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} + */ +public class SwerveModuleSimulation +{ + + /** + * MapleSim module. + */ + public SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation mapleSimModule = null; + + /** + * Configure the maple sim module + * + * @param simModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for + * simulation + * @param physicalCharacteristics Physical characteristics of the swerve drive from the JSON or built. + */ + public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation simModule, + SwerveModulePhysicalCharacteristics physicalCharacteristics) + { + this.mapleSimModule = new SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation(simModule); + this.mapleSimModule.withCurrentLimits( + Amps.of(physicalCharacteristics.driveMotorCurrentLimit), + Amps.of(physicalCharacteristics.angleMotorCurrentLimit)); + } + + /** + * Update the position and state of the module. Called from {@link swervelib.SwerveModule#setDesiredState} function + * when simulated. + * + * @param desiredState State the swerve module is set to. + */ + public void updateStateAndPosition(SwerveModuleState desiredState) + { + mapleSimModule.runModuleState(desiredState); + } + + /** + * Runs a drive motor characterization on the sim module. This is called from + * {@link swervelib.SwerveDriveTest#runDriveMotorsCharacterizationOnSimModules(SwerveDrive, double, boolean)} to run + * sysId during simulation + * + * @param desiredFacing the desired facing of the module + * @param volts the voltage to run + */ + public void runDriveMotorCharacterization(Rotation2d desiredFacing, double volts) + { + mapleSimModule.runDriveMotorCharacterization(desiredFacing, volts); + } + + /** + * Runs a drive motor characterization on the sim module. This method is called from + * {@link swervelib.SwerveDriveTest#runAngleMotorsCharacterizationOnSimModules(SwerveDrive, double)} to run sysId + * during simulation + * + * @param volts the voltage to run + */ + public void runAngleMotorCharacterization(double volts) + { + mapleSimModule.runSteerMotorCharacterization(volts); + } + + /** + * Get the simulated swerve module position. + * + * @return {@link SwerveModulePosition} of the simulated module. + */ + public SwerveModulePosition getPosition() + { + return mapleSimModule.getModulePosition(); + } + + /** + * Get the {@link SwerveModuleState} of the simulated module. + * + * @return {@link SwerveModuleState} of the simulated module. + */ + public SwerveModuleState getState() + { + if (mapleSimModule == null) + { + return new SwerveModuleState(); + } + SwerveModuleState state = mapleSimModule.getMeasuredState(); + state.angle = state.angle.minus(Rotation2d.kZero); + return state; + } +} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json new file mode 100644 index 0000000..4414707 --- /dev/null +++ b/vendordeps/PathplannerLib.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib.json", + "name": "PathplannerLib", + "version": "2026.1.2", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2026", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2026.1.2" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2026.1.2", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json new file mode 100644 index 0000000..6b06f77 --- /dev/null +++ b/vendordeps/Phoenix5.json @@ -0,0 +1,119 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.35.1", + "frcYear": "2026", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6-frc2025-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.35.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.35.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "linuxathena"], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "linuxathena"], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "linuxathena"], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "linuxathena"], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + } + ] +} diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json new file mode 100644 index 0000000..39fc664 --- /dev/null +++ b/vendordeps/Phoenix6.json @@ -0,0 +1,322 @@ +{ + "fileName": "Phoenix6.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.4.0", + "frcYear": "2026", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.4.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "linuxathena"], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "linuxathena"], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "linuxathena"], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "linuxathena"], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.4.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.4.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.4.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.4.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.4.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.4.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.4.0", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.4.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.4.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.4.0", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": ["windowsx86-64", "linuxx86-64", "linuxarm64", "osxuniversal"], + "simMode": "swsim" + } + ] +} diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..8f3a93e --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,71 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2025.0.3", + "frcYear": "2026", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2025.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2025.0.3" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.3", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2025.0.3", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2025.0.3", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} diff --git a/vendordeps/ReduxLib.json b/vendordeps/ReduxLib.json new file mode 100644 index 0000000..56435f0 --- /dev/null +++ b/vendordeps/ReduxLib.json @@ -0,0 +1,72 @@ +{ + "fileName": "ReduxLib.json", + "name": "ReduxLib", + "version": "2025.0.1", + "frcYear": "2026", + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2025.0.1", + "libName": "ReduxLib", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + }, + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "libName": "ReduxCore", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} diff --git a/vendordeps/Studica.json b/vendordeps/Studica.json new file mode 100644 index 0000000..17dcad7 --- /dev/null +++ b/vendordeps/Studica.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica.json", + "name": "Studica", + "version": "2025.0.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2026", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.0.json", + "javaDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-java", + "version": "2025.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-driver", + "version": "2025.0.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.studica.frc", + "artifactId": "Studica-cpp", + "version": "2025.0.0", + "libName": "Studica", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + }, + { + "groupId": "com.studica.frc", + "artifactId": "Studica-driver", + "version": "2025.0.0", + "libName": "StudicaDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} diff --git a/vendordeps/ThriftyLib.json b/vendordeps/ThriftyLib.json new file mode 100644 index 0000000..0c481d8 --- /dev/null +++ b/vendordeps/ThriftyLib.json @@ -0,0 +1,20 @@ +{ + "fileName": "ThriftyLib.json", + "name": "ThriftyLib", + "version": "2025.1.1", + "frcYear": "2026", + "uuid": "60b2694b-9e6e-4026-81ee-6f167946f4b0", + "mavenUrls": [ + "https://docs.home.thethriftybot.com" + ], + "jsonUrl": "https://docs.home.thethriftybot.com/ThriftyLib.json", + "javaDependencies": [ + { + "groupId": "com.thethriftybot.frc", + "artifactId": "ThriftyLib-java", + "version": "2025.1.1" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 3718e0a..d90630e 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2025", + "frcYear": "2026", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [ @@ -25,6 +25,7 @@ "sharedLibrary": true, "skipInvalidPlatforms": true, "binaryPlatforms": [ + "linuxsystemcore", "linuxathena", "linuxarm32", "linuxarm64", diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json new file mode 100644 index 0000000..a8dc6df --- /dev/null +++ b/vendordeps/maple-sim.json @@ -0,0 +1,26 @@ +{ + "fileName": "maple-sim.json", + "name": "maplesim", + "version": "0.4.0-beta", + "frcYear": "2026", + "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", + "mavenUrls": [ + "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", + "javaDependencies": [ + { + "groupId": "org.ironmaple", + "artifactId": "maplesim-java", + "version": "0.4.0-beta" + }, + { + "groupId": "org.dyn4j", + "artifactId": "dyn4j", + "version": "5.0.2" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json new file mode 100644 index 0000000..cde810a --- /dev/null +++ b/vendordeps/yagsl.json @@ -0,0 +1,20 @@ +{ + "fileName": "yagsl.json", + "name": "YAGSL", + "version": "2025.8.0", + "frcYear": "2026", + "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", + "mavenUrls": [ + "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/repos" + ], + "jsonUrl": "https://broncbotz3481.github.io/YAGSL-Lib/yagsl/yagsl.json", + "javaDependencies": [ + { + "groupId": "swervelib", + "artifactId": "YAGSL-java", + "version": "2025.8.0" + } + ], + "jniDependencies": [], + "cppDependencies": [] +}