diff --git a/.gradle/7.5.1/checksums/checksums.lock b/.gradle/7.5.1/checksums/checksums.lock index 87e8c6c..9b14f58 100644 Binary files a/.gradle/7.5.1/checksums/checksums.lock and b/.gradle/7.5.1/checksums/checksums.lock differ diff --git a/.gradle/7.5.1/checksums/md5-checksums.bin b/.gradle/7.5.1/checksums/md5-checksums.bin index ea55786..d4c8abf 100644 Binary files a/.gradle/7.5.1/checksums/md5-checksums.bin and b/.gradle/7.5.1/checksums/md5-checksums.bin differ diff --git a/.gradle/7.5.1/checksums/sha1-checksums.bin b/.gradle/7.5.1/checksums/sha1-checksums.bin index f8edbb2..de6b518 100644 Binary files a/.gradle/7.5.1/checksums/sha1-checksums.bin and b/.gradle/7.5.1/checksums/sha1-checksums.bin differ diff --git a/.gradle/7.5.1/executionHistory/executionHistory.bin b/.gradle/7.5.1/executionHistory/executionHistory.bin index a115a5c..48c62ec 100644 Binary files a/.gradle/7.5.1/executionHistory/executionHistory.bin and b/.gradle/7.5.1/executionHistory/executionHistory.bin differ diff --git a/.gradle/7.5.1/executionHistory/executionHistory.lock b/.gradle/7.5.1/executionHistory/executionHistory.lock index 4ec1c59..eccb776 100644 Binary files a/.gradle/7.5.1/executionHistory/executionHistory.lock and b/.gradle/7.5.1/executionHistory/executionHistory.lock differ diff --git a/.gradle/7.5.1/fileHashes/fileHashes.bin b/.gradle/7.5.1/fileHashes/fileHashes.bin index 8369058..3133e42 100644 Binary files a/.gradle/7.5.1/fileHashes/fileHashes.bin and b/.gradle/7.5.1/fileHashes/fileHashes.bin differ diff --git a/.gradle/7.5.1/fileHashes/fileHashes.lock b/.gradle/7.5.1/fileHashes/fileHashes.lock index b850247..725f2ca 100644 Binary files a/.gradle/7.5.1/fileHashes/fileHashes.lock and b/.gradle/7.5.1/fileHashes/fileHashes.lock differ diff --git a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin index 5b0d63e..11197ed 100644 Binary files a/.gradle/7.5.1/fileHashes/resourceHashesCache.bin and b/.gradle/7.5.1/fileHashes/resourceHashesCache.bin differ diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock index 7162cd5..3b27189 100644 Binary files a/.gradle/buildOutputCleanup/buildOutputCleanup.lock and b/.gradle/buildOutputCleanup/buildOutputCleanup.lock differ diff --git a/.gradle/buildOutputCleanup/outputFiles.bin b/.gradle/buildOutputCleanup/outputFiles.bin index 280d7a8..5d13225 100644 Binary files a/.gradle/buildOutputCleanup/outputFiles.bin and b/.gradle/buildOutputCleanup/outputFiles.bin differ diff --git a/.gradle/file-system.probe b/.gradle/file-system.probe index fb58ef6..b986757 100644 Binary files a/.gradle/file-system.probe and b/.gradle/file-system.probe differ diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..c5f3f6b --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "java.configuration.updateBuildConfiguration": "interactive" +} \ No newline at end of file diff --git a/README.md b/README.md index 9312bf4..7170eca 100644 --- a/README.md +++ b/README.md @@ -5,13 +5,15 @@ Team 5417's code for the swerve bot, _Vira_! [![CI](https://github.com/frc5417/CustomSwerve/actions/workflows/main.yml/badge.svg)](https://github.com/frc5417/CustomSwerve/actions/workflows/main.yml) This is our custom algorithm for swerve. Here are the developers: -* Krishna Shah -* Jordan Escobedo -* Rupak Bhattacharya -* Pranav Gattineni -* Brennan Coil -* Lucas Hatcher -* Joshua Vigel -* Matthew Caldarola -* Jeevan Adhya Vinoth Babu -* Ethan Wallraven + +Krishna Shah +Jordan Escobedo +Rupak Bhattacharya +Pranav Gattineni +Brennan Coil +Lucas Hatcher +Joshua Vigel +Matthew Caldarola +Jeevan Adhya Vinoth Babu +Ethan Wallraven +Boden Smith diff --git a/build.gradle b/build.gradle index c6f9375..baa242a 100644 --- a/build.gradle +++ b/build.gradle @@ -43,7 +43,7 @@ def deployArtifact = deploy.targets.roborio.artifacts.frcJava 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. diff --git a/gradlew.bat b/gradlew.bat old mode 100644 new mode 100755 diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..5ff7183 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,103 @@ +{ + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + }, + {}, + { + "decKey": 264, + "incKey": 265 + } + ], + "axisCount": 5, + "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 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..edacf08 --- /dev/null +++ b/simgui.json @@ -0,0 +1,103 @@ +{ + "HALProvider": { + "Other Devices": { + "SPARK MAX [10]": { + "header": { + "open": true + } + }, + "SPARK MAX [11]": { + "header": { + "open": true + } + }, + "SPARK MAX [20]": { + "header": { + "open": true + } + }, + "SPARK MAX [21]": { + "header": { + "open": true + } + }, + "SPARK MAX [30]": { + "header": { + "open": true + } + }, + "SPARK MAX [31]": { + "header": { + "open": true + } + }, + "SPARK MAX [40]": { + "header": { + "open": true + } + }, + "SPARK MAX [41]": { + "header": { + "open": true + } + }, + "navX-Sensor[0]": { + "header": { + "open": true + } + } + }, + "RoboRIO": { + "5V Rail": { + "open": true + }, + "6V Rail": { + "open": true + }, + "RoboRIO Input": { + "open": true + }, + "window": { + "visible": true + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/SendableChooser[0]": "String Chooser" + }, + "windows": { + "/SmartDashboard/SendableChooser[0]": { + "window": { + "visible": true + } + } + } + }, + "NetworkTables View": { + "visible": false + }, + "Plot": { + "Plot <0>": { + "plots": [ + { + "height": 332 + } + ], + "window": { + "visible": false + } + }, + "Plot <1>": { + "plots": [ + { + "height": 332 + } + ], + "window": { + "visible": false + } + } + } +} diff --git a/src/main/deploy/pathplanner/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..74c48a8 --- /dev/null +++ b/src/main/deploy/pathplanner/deploy/pathplanner/New Path.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/straight_line.path b/src/main/deploy/pathplanner/straight_line.path new file mode 100644 index 0000000..ce982c1 --- /dev/null +++ b/src/main/deploy/pathplanner/straight_line.path @@ -0,0 +1,49 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 1.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.0, + "y": 1.486914226526235 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.0, + "y": 2.0 + }, + "prevControl": { + "x": 0.9999999999999999, + "y": 1.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/trajectory.path b/src/main/deploy/pathplanner/trajectory.path new file mode 100644 index 0000000..1009766 --- /dev/null +++ b/src/main/deploy/pathplanner/trajectory.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.6640442500566275, + "y": 3.0248652771945976 + }, + "prevControl": { + "x": 4.742234572926548, + "y": 3.0248652771945976 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Autons/MoveForward.java b/src/main/java/frc/robot/Autons/MoveForward.java new file mode 100644 index 0000000..d71fccd --- /dev/null +++ b/src/main/java/frc/robot/Autons/MoveForward.java @@ -0,0 +1,45 @@ +// package frc.robot.autons; + +// import edu.wpi.first.wpilibj2.command.CommandBase; + +// import frc.robot.subsystems.DriveBase; + +// import edu.wpi.first.math.kinematics.ChassisSpeeds; + +// public class MoveForward extends CommandBase { +// private final DriveBase m_driveBase; +// private boolean doFinish = false; +// private int count = 0; + +// public MoveForward(DriveBase drive) { +// m_driveBase = drive; +// } + +// // Called when the command is initially scheduled. +// @Override +// public void initialize() { +// m_driveBase.setDriveSpeed(new ChassisSpeeds(0.5, 0.0, 0.0)); +// } + +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { +// if (count++ >= 50) { +// m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 0.0)); +// doFinish = true; +// } +// } + +// // Called once the command ends or is interrupted. +// @Override +// public void end(boolean interrupted) { +// m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 0.0)); +// m_driveBase.resetDrive(); +// } + +// // Returns true when the command should end. +// @Override +// public boolean isFinished() { +// return doFinish; +// } +// } diff --git a/src/main/java/frc/robot/Autons/RotateInPlace.java b/src/main/java/frc/robot/Autons/RotateInPlace.java new file mode 100644 index 0000000..c4217b9 --- /dev/null +++ b/src/main/java/frc/robot/Autons/RotateInPlace.java @@ -0,0 +1,43 @@ +// package frc.robot.Autons; + +// import edu.wpi.first.wpilibj2.command.CommandBase; + +// import frc.robot.ModuleUtilities.ModuleGroup; +// import frc.robot.subsystems.DriveBase; +// import frc.robot.subsystems.Systems; + +// import edu.wpi.first.math.kinematics.ChassisSpeeds; + +// public class RotateInPlace extends CommandBase { +// private final ModuleGroup m_moduleGroup; +// private final DriveBase m_driveBase; + +// public RotateInPlace() { +// m_moduleGroup = Systems.moduleGroup; +// m_driveBase = Systems.driveBase; +// } + +// // Called when the command is initially scheduled. +// @Override +// public void initialize() { +// m_moduleGroup.resetDrive(); +// } + +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { +// m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 1)); +// } + +// // Called once the command ends or is interrupted. +// @Override +// public void end(boolean interrupted) { +// m_moduleGroup.resetDrive(); +// } + +// // Returns true when the command should end. +// @Override +// public boolean isFinished() { +// return false; +// } +// } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4a17d4d..a9b8cc1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,6 +8,12 @@ import com.pathplanner.lib.auto.PIDConstants; import com.revrobotics.CANSparkMax.IdleMode; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; + /** * 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 @@ -23,22 +29,32 @@ public static class OperatorConstants { public static final double joystickDeadband = 0.2; } + public static class MotorConstants { // 0 indexing - public static final Integer[] driveMotorIDS = {11, 21, 31, 40}; + public static final Integer[] driveMotorIDS = {11, 21, 31, 40}; public static final Integer[] angleMotorIDS = {10, 20, 30, 41}; - public static final Integer[] CANCoderID = {9, 12, 8, 13}; + public static final Integer[] CANCoderID = {9, 12, 8, 19}; + public static final Double[] motorDegrees = {91.494, 182.373, 347.783, 153.281}; public static final Double[] angleOffsets = {0.0, 0.0, 0.0, 0.0}; public static final Double[][] angleMotorPID = { - {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}, {0.01, 0.0, 0.005}}; - public static final Double degTolerance = 2.5; + {0.5, 0.0, 0.005}, {0.5, 0.0, 0.005}, {0.5, 0.0, 0.005}, {0.5, 0.0, 0.005}}; + public static final Double degTolerance = 0.75; + // max amperage sent to the motors is roughly +10 of the current limit + public static final int angleMotorCurrentLimit = 10; } public static class Swerve { public static final IdleMode angleNeutralMode = IdleMode.kBrake; public static final IdleMode driveNeutralMode = IdleMode.kBrake; - public static final double maxVelocity = 3.8; // m/s + public static final double maxVelocity = 4; // m/s public static final double maxAngularVelocity = 56.0; // rad/s + + public static final double maxSetVelocity = 1.5; //1.5 + public static final double maxSetAngularVelocity = 2.0; + + public static final double maxAutonAcceleration = 0.5; + //velocity PID tuning for overall swerve public static final double velocitykP = 1.0; // 0.0001 public static final double velocitykI = 0.0; @@ -47,16 +63,26 @@ public static class Swerve { public static final double aVelocitykI = 0.0; public static final double aVelocitykD = 0.0; - public static final PathConstraints AUTON_CONSTRAINTS = new PathConstraints(maxVelocity, 2); // max velocity and acceleration during auton + public static final PathConstraints AUTON_CONSTRAINTS = new PathConstraints(maxSetVelocity, maxAutonAcceleration); // max velocity and acceleration during auton } public static class DriveTrainConstants { + public static final double wheelDia_m = 0.097; public static final double DRIVETRAIN_WIDTH = 0.635; // in meters - - public static final PIDConstants ROTATION_PID = new PIDConstants(0, 0, 0); - public static final PIDConstants TRANSLATION_PID = new PIDConstants(0, 0, 0); + public static final double TRACK_WIDTH = 0.470; // in meters + public static final double WHEEL_BASE = 0.470; // in meters + + public static final PIDConstants ROTATION_PID = new PIDConstants(0.1, 0, 0); + public static final PIDConstants TRANSLATION_PID = new PIDConstants(0.1, 0, 0); public static final Integer wheels = 4; public static final boolean[] invertedMotors = {true, true, false, false}; + + public static final Translation2d[] wheelLocations = { + new Translation2d((TRACK_WIDTH / 2), -(WHEEL_BASE / 2)), + new Translation2d(-(TRACK_WIDTH / 2), -(WHEEL_BASE / 2)), + new Translation2d((TRACK_WIDTH / 2), (WHEEL_BASE / 2)), + new Translation2d(-(TRACK_WIDTH / 2), (WHEEL_BASE / 2)), + }; //TODO: tune pid constants } @@ -64,7 +90,26 @@ public static class Auton { public static final String[] paths = {"rotateInPlace, moveForward, PathPlannerTest"}; } + public static class VisionConstants { + public static final Transform3d robotToCam = + new Transform3d( + new Translation3d(0.5, 0.0, 0.5), + new Rotation3d( + 0, 0, + 0)); // Cam mounted facing forward, half a meter forward of center, half a meter up + // from center. + public static final String cameraName = "OV5647"; + public static final double maxDistanceAway = 2.0; + public static final double forwardKP = 0.1; + public static final double forwardToAngleRatio = 0.5; + + public static final double CAMERA_HEIGHT_METERS = 0.72; + public static final double TARGET_HEIGHT_METERS = 0; + public static final double CAMERA_PITCH_RADIANS = 0; + } - // Josh was here - // Hi Josh! + public static class FieldConstants { + public static final double length = Units.feetToMeters(54); + public static final double width = Units.feetToMeters(27); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 67d9122..315ee9f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.RobotContainer; /** * The VM is configured to automatically run this class, and to call the functions corresponding to diff --git a/src/main/java/frc/robot/commands/AutonCommands.java b/src/main/java/frc/robot/commands/AutonCommands.java index 90e0b9f..13cefd4 100644 --- a/src/main/java/frc/robot/commands/AutonCommands.java +++ b/src/main/java/frc/robot/commands/AutonCommands.java @@ -1,4 +1,17 @@ -package frc.robot.commands; +// package frc.robot.commands; +// import frc.robot.subsystems.DriveBase; +// import frc.robot.autons.MoveForward; -// This file will be used to store auton commands (activating intake, etc.) +// // This file will be used to store auton commands (activating intake, etc.) +// public class AutonCommands { + +// //public static final RotateInPlace ROTATE_IN_PLACE = new RotateInPlace(); +// public final DriveBase m_drivebase; +// public final MoveForward MOVE_FORWARD; + +// public AutonCommands(DriveBase dBase) { +// m_drivebase = dBase; +// MOVE_FORWARD = new MoveForward(m_drivebase); +// } +// } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AutonLoader.java b/src/main/java/frc/robot/commands/AutonLoader.java index c3441dc..f280015 100644 --- a/src/main/java/frc/robot/commands/AutonLoader.java +++ b/src/main/java/frc/robot/commands/AutonLoader.java @@ -6,28 +6,32 @@ import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.auto.SwerveAutoBuilder; -import edu.wpi.first.math.kinematics.ChassisSpeeds; 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.PrintCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; import frc.robot.Constants; import frc.robot.subsystems.DriveBase; -import frc.robot.subsystems.Systems; public class AutonLoader { private static HashMap eventMap; private SwerveAutoBuilder autoBuilder; private final DriveBase m_driveBase; + // private final AutonCommands m_autoncommands; private static SendableChooser chooser; private static List pathGroup = PathPlanner.loadPathGroup("trajectory", Constants.Swerve.AUTON_CONSTRAINTS); + private static List pathGroup2 = PathPlanner.loadPathGroup("straight_line", Constants.Swerve.AUTON_CONSTRAINTS); + + public AutonLoader(DriveBase driveBase) { + + m_driveBase = driveBase; + + // m_autoncommands = new AutonCommands(driveBase); + chooser = new SendableChooser<>(); - public AutonLoader(Systems systems) { - m_driveBase = systems.driveBase; - eventMap = new HashMap<>(); eventMap.put("event1", new PrintCommand("event 1 passed")); - eventMap.put("event2", new PrintCommand("event 2 passed")); + eventMap.put("event2", new PrintCommand("event 2 passed")); autoBuilder = new SwerveAutoBuilder( m_driveBase::getCurrentPose, @@ -38,50 +42,14 @@ public AutonLoader(Systems systems) { eventMap, m_driveBase); - for (String path : Constants.Auton.paths) - chooser.addOption(path, getAutonFromPath(path)); + chooser.addOption("trajectory", autoBuilder.fullAuto(pathGroup)); + chooser.addOption("straight line", autoBuilder.fullAuto(pathGroup2)); + + SmartDashboard.putData(chooser); } private Command getAutonFromPath(String path) { - switch (path) { - case "rotateInPlace": - return new Command() { - - @Override - public void schedule() { - m_driveBase.setDriveSpeed(new ChassisSpeeds(0.0, 0.0, 1)); - } - - @Override - public Set getRequirements() { - Set ret = new TreeSet<>(); - ret.add(m_driveBase); - return ret; - } - }; - - case "moveForward": - return new Command() { - - @Override - public void schedule() { - m_driveBase.setDriveSpeed(new ChassisSpeeds(0.5, 0.0, 0)); - } - - @Override - public Set getRequirements() { - Set ret = new TreeSet<>(); - ret.add(m_driveBase); - return ret; - } - }; - - case "PathPlannerTest": - return autoBuilder.fullAuto(pathGroup); - - default : - throw new IllegalArgumentException("Make sure you enter a valid Auton name!"); - } + return new PrintCommand(path); } public Command getAuton() { diff --git a/src/main/java/frc/robot/commands/TeleopDrive.java b/src/main/java/frc/robot/commands/TeleopDrive.java index 3925303..1725f26 100644 --- a/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/src/main/java/frc/robot/commands/TeleopDrive.java @@ -6,11 +6,11 @@ import frc.robot.Constants; import frc.robot.subsystems.DriveBase; -import frc.robot.subsystems.ModuleGroup; -import frc.robot.subsystems.Systems; +import frc.robot.subsystems.RobotContainer; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** An example command that uses an example subsystem. */ public class TeleopDrive extends CommandBase { @@ -24,33 +24,57 @@ public class TeleopDrive extends CommandBase { // Called when the command is initially scheduled. - private final ModuleGroup m_moduleGroup; private final DriveBase m_driveBase; - public TeleopDrive(Systems systems) { - m_moduleGroup = systems.moduleGroup; - m_driveBase = systems.driveBase; + double prev_omega = 0; + double prev_xVel = 0; + double prev_yVel = 0; + + int counter = 0; + + public TeleopDrive(DriveBase driveBase) { + m_driveBase = driveBase; } @Override public void initialize() { - m_moduleGroup.resetDrive(); + // Module.ModuleState[] temp = new Module.ModuleState[4]; + + // for (int i = 0; i < 4; i++) + // temp[i] = new Module.ModuleState(0, Constants.MotorConstants.motorDegrees[i] * (Math.PI/180)); + + // m_driveBase.setHardStates(temp); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double xVel = Systems.getLeftJoyX() * Constants.Swerve.maxVelocity; - double yVel = Systems.getLeftJoyY() * Constants.Swerve.maxVelocity; - double omega = Systems.getRightJoyX() * Constants.Swerve.maxAngularVelocity; + + double xVel = ((RobotContainer.getLeftJoyX()) * 0.45 * Constants.Swerve.maxSetVelocity) + (prev_xVel * 0.55); //* Constants.Swerve.maxVelocity; + double yVel = ((RobotContainer.getLeftJoyY()) * 0.45 * Constants.Swerve.maxSetVelocity) + (prev_yVel * 0.55); //* Constants.Swerve.maxVelocity; + double omega = ((RobotContainer.getRightJoyX()) * 0.45 * Constants.Swerve.maxSetAngularVelocity) + (prev_omega * 0.55); //* Constants.Swerve.maxAngularVelocity; + + prev_xVel = xVel; + prev_yVel = yVel; + prev_omega = omega; + + SmartDashboard.putNumber("X-Vel Input", xVel); + SmartDashboard.putNumber("Y-Vel Input", yVel); + SmartDashboard.putNumber("Omega Vel Input", omega); + + ChassisSpeeds newSpeed = new ChassisSpeeds(xVel, yVel, omega); + m_driveBase.setDriveSpeed(newSpeed); - m_driveBase.setDriveSpeed(new ChassisSpeeds(xVel, yVel, omega)); + if (counter++ % 50 == 0) { + System.out.println(newSpeed); + } } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_moduleGroup.resetDrive(); + m_driveBase.resetDrive(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 5e56e14..40286a4 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -1,43 +1,199 @@ package frc.robot.subsystems; +import com.fasterxml.jackson.databind.module.SimpleAbstractTypeResolver; +import com.revrobotics.CANSparkMax; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import frc.robot.Constants; public class DriveBase extends SubsystemBase { - private static ChassisSpeeds m_chassisSpeeds; - private final ModuleGroup m_moduleGroup; + private static Module.ModuleState targetModuleStates[]; + private static Module.ModuleState testModuleStates[]; + private final Kinematics m_kinematics; + // private Pose2d currentLocation = new Pose2d(); + + public static Module[] moduleGroup; + + // // Locations for the swerve drive modules relative to the robot center. + // Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381); + // Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381); + // Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381); + // Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381); + + // // Creating my kinematics object using the module locations + // SwerveDriveKinematics m_sdskinematics = new SwerveDriveKinematics( + // m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation + // ); + + // SwerveDriveOdometry m_odometry; + + Pose2d m_pose = new Pose2d(); + Pose2d simPose = new Pose2d(); + + double prevAngle = 0; + int cnt = 0; + + public DriveBase(Kinematics kinematics) { + + m_kinematics = kinematics; + + moduleGroup = new Module[4]; - public DriveBase(Systems system) { - m_moduleGroup = system.moduleGroup; - m_kinematics = system.kinematics; + testModuleStates = new Module.ModuleState[4]; - m_moduleGroup.resetDrive(); - m_chassisSpeeds = new ChassisSpeeds(0, 0, 0); + for (int i = 0; i < 4; i++) + moduleGroup[i] = new Module(i, + Constants.DriveTrainConstants.invertedMotors[i], + Constants.DriveTrainConstants.wheelLocations[i] + ); + + for (int i = 0; i < 4; i++) + testModuleStates[i] = new Module.ModuleState(0, 0); + + // m_odometry = new SwerveDriveOdometry( + // m_sdskinematics, RobotContainer.ahrs.getRotation2d(), + // new SwerveModulePosition[] { + // new SwerveModulePosition(0, new Rotation2d(Constants.MotorConstants.motorDegrees[0])), + // new SwerveModulePosition(0, new Rotation2d(Constants.MotorConstants.motorDegrees[1])), + // new SwerveModulePosition(0, new Rotation2d(Constants.MotorConstants.motorDegrees[2])), + // new SwerveModulePosition(0, new Rotation2d(Constants.MotorConstants.motorDegrees[3])) + // }, new Pose2d(5.0, 13.5, new Rotation2d())); + + targetModuleStates = new Module.ModuleState[4]; + + // for (int i = 0; i < 4; i++) + // System.out.printf("initial value for %d is %f\n", i, moduleGroup[i].getAngleInRadians()); + + for (int i = 0; i < 4; i++) + targetModuleStates[i] = new Module.ModuleState(0, 0); + } + public Module[] getModuleGroup() { + return moduleGroup; + } + public Pose2d getCurrentPose() { - return new Pose2d(); - //TODO: FIX THIS + return m_pose; } + // public void resetAngles() { + // for (int i = 0; i < 4; i++) + // moduleGroup[i].resetAngleOffset(); + // } + + // private void updatePoseManual() { // updates current position of the robot + // ChassisSpeeds currentChassisSpeed = m_kinematics.toChassisSpeeds(moduleGroup); + // double xVel = currentChassisSpeed.vxMetersPerSecond; + // double yVel = currentChassisSpeed.vyMetersPerSecond; + // double omega = currentChassisSpeed.omegaRadiansPerSecond; + + // // TODO: MAKE SURE TO CONFIRM THAT AUTON WORKS WITH DEGREES + + // currentLocation = new Pose2d( + // new Translation2d( + // currentLocation.getX() + (xVel * 0.02), + // currentLocation.getY() + (yVel * 0.02) + // ), + // new Rotation2d( + // currentLocation.getRotation().getDegrees() + (omega * (Math.PI / 180.0) * 0.02) + // ) + // ); + // } + + // public void resetOdometry(Pose2d pose) { + // SwerveModulePosition[] ModulePoseOdom = {new SwerveModulePosition(0, new Rotation2d(0)), + // new SwerveModulePosition(0, new Rotation2d(0)), + // new SwerveModulePosition(0, new Rotation2d(0)), + // new SwerveModulePosition(0, new Rotation2d(0)),}; + // for (int i = 0; i < 4; i++) { + // ModulePoseOdom[i] = new SwerveModulePosition(targetModuleStates[i].getVel(), new Rotation2d(targetModuleStates[i].getDir())); + // } + // m_odometry.resetPosition(RobotContainer.ahrs.getRotation2d(), ModulePoseOdom, new Pose2d()); + // } + public void resetOdometry(Pose2d pose) { - //TODO: FIX THIS + m_pose = new Pose2d(pose.getTranslation(), RobotContainer.ahrs.getRotation2d()); + } + + private void updateOdom() { + + /* + * ENSURE that Kinematics::toTwist and getDeltaOmegaAHRS are being called every code cycle + */ + + Twist2d simTwist = m_kinematics.toTwistTest(targetModuleStates); + Twist2d twisting = m_kinematics.toTwist(moduleGroup); + //twisting.dtheta = getDeltaOmegaAHRS(); // needs to be tested + + SmartDashboard.putNumber("dX", twisting.dx); + SmartDashboard.putNumber("dY", twisting.dy); + SmartDashboard.putNumber("dtheta", twisting.dtheta); + + simPose = simPose.exp(simTwist); + Pose2d newPose = m_pose.exp(twisting); + + m_pose = new Pose2d(newPose.getTranslation(), RobotContainer.ahrs.getRotation2d()); + + SmartDashboard.putNumber("X", m_pose.getX()); + SmartDashboard.putNumber("Y", m_pose.getY()); + SmartDashboard.putNumber("tX", simPose.getX()); + SmartDashboard.putNumber("tY", simPose.getY()); + + if (cnt++ % 50 == 0) { + System.out.println(m_pose); + for (int i = 0; i < 4; i++) + System.out.printf("m: %d, %f, %f\n", i, targetModuleStates[i].getDir(), targetModuleStates[i].getVel()); + System.out.println(); + } } public void setDriveSpeed(ChassisSpeeds chassisSpeeds) { - m_chassisSpeeds = chassisSpeeds; + double xVel = chassisSpeeds.vxMetersPerSecond; + double yVel = chassisSpeeds.vyMetersPerSecond; + + double angle = Math.atan2(yVel, xVel); + double vel = Math.sqrt((xVel * xVel) + (yVel * yVel)); + + if (Constants.OperatorConstants.fieldCentric) { + angle -= RobotContainer.ahrs.getRotation2d().getRadians(); + //angle -= simPose.getRotation().getRadians(); // modify for when not field centric + } + chassisSpeeds = new ChassisSpeeds(vel * Math.cos(angle), vel * Math.sin(angle), chassisSpeeds.omegaRadiansPerSecond); + + targetModuleStates = m_kinematics.getComputedModuleStates(chassisSpeeds); + + for (int index = 0; index < 4; index++) + targetModuleStates[index] = Kinematics.optimize(moduleGroup[index].getCurrentState(), targetModuleStates[index]); } - @Override - public void periodic() { - - ModuleState[] targetModuleStates = m_kinematics.getModuleStates(m_chassisSpeeds); - targetModuleStates = Kinematics.normalizeVelocity(targetModuleStates); + public void resetDrive() { + for (int i = 0; i < 4; i++) { + moduleGroup[i].resetDriveAngleEncoder(); + } + } - m_moduleGroup.setDrive(targetModuleStates); + private double getDeltaOmegaAHRS() { + double curAngle = (RobotContainer.ahrs.getAngle() * Math.PI/180); + double delta = curAngle - prevAngle; + prevAngle = curAngle; + return delta; } + @Override + public void periodic() { + for (int i = 0; i < 4; i++) { + moduleGroup[i].setSpeedAndAngle(targetModuleStates[i]); + } + + updateOdom(); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Kinematics.java b/src/main/java/frc/robot/subsystems/Kinematics.java index 67b2cd5..90803ed 100644 --- a/src/main/java/frc/robot/subsystems/Kinematics.java +++ b/src/main/java/frc/robot/subsystems/Kinematics.java @@ -5,136 +5,257 @@ package frc.robot.subsystems; import frc.robot.Constants; -import com.kauailabs.navx.frc.AHRS; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; 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; public class Kinematics { /** Creates a new Compute. */ - private double[] vel = new double[4]; - private double[] theta = new double[4]; - public boolean fieldCentric; - private double gyro = 0.0; - private final AHRS m_ahrs; + private final SimpleMatrix m_forwardKinematics; + private final SimpleMatrix m_inverseKinematics; - public Kinematics(Systems systems) { - this.fieldCentric = Constants.OperatorConstants.fieldCentric; - m_ahrs = systems.ahrs; + // private ChassisSpeeds odomSpeeds = new ChassisSpeeds(0, 0, 0); - } + public Kinematics(Translation2d[] translations) { + m_inverseKinematics = new SimpleMatrix(8, 3); + + for (int i = 0; i < 4; i++) { + Translation2d distFromCenter = translations[i]; - private double[][] computeStrafe(double joy_x, double joy_y) { - double[][] temp_vel = new double[4][2]; - for(int n=0; n<4; n++) { - temp_vel[n][0] = ((joy_x*Math.cos(gyro)) - (joy_y*Math.sin(gyro))); - temp_vel[n][1] = ((joy_x*Math.sin(gyro)) + (joy_y*Math.cos(gyro))); + m_inverseKinematics.setRow((i * 2), 0, 1, 0, -distFromCenter.getY()); + m_inverseKinematics.setRow((i * 2) + 1, 0, 0, 1, distFromCenter.getX()); } - - return temp_vel; - } - private double[][] computeRotation(double omega) { - double[][] temp = {{omega * Math.cos(1*(Math.PI/4)), omega * Math.sin(1*(Math.PI/4))}, - {omega * Math.cos(7*(Math.PI/4)), omega * Math.sin(7*(Math.PI/4))}, - {omega * Math.cos(3*(Math.PI/4)), omega * Math.sin(3*(Math.PI/4))}, - {omega * Math.cos(5*(Math.PI/4)), omega * Math.sin(5*(Math.PI/4))}}; - return temp; - } + m_forwardKinematics = m_inverseKinematics.pseudoInverse(); - private double[][] addVect(double[][] a, double[][] b) { - double temp[][] = new double[4][2]; - assert(a.length == b.length); - for(int n=0; n 0) { - if (b >= 0) { - this.theta[i] += 0; - } else { - this.theta[i] += Math.PI*2; - } - } else { - this.theta[i] += Math.PI; - } - } + public Twist2d toTwist(Module[] moduleGroup) { // uses forward kinematics to derive a Twist2d from wheel deltas + //gets the independent x and y deltas of each module based on encoder values + + SimpleMatrix xyVels = new SimpleMatrix(8, 1); + for (int i = 0; i < 4; i++) { + double delta = moduleGroup[i].updateDeltaDist(); + double dir = moduleGroup[i].getAngleInRadians(); + + xyVels.set((i * 2), 0, delta * Math.cos(dir)); + xyVels.set((i * 2) + 1, 0, delta * Math.sin(dir)); } + + //multiply the current x, y delta matrix by the inverse of the kinematics matrix + + SimpleMatrix finalChassisSpeeds = m_forwardKinematics.mult(xyVels); + + double deltaX = finalChassisSpeeds.get(0, 0); + double deltaY = finalChassisSpeeds.get(1, 0); + double deltaOmega = finalChassisSpeeds.get(2, 0); + + if (Math.abs(deltaX) < 1e-9) + deltaX = 0; + if (Math.abs(deltaY) < 1e-9) + deltaX = 0; + if (Math.abs(deltaOmega) < 1e-9) + deltaX = 0; + + return new Twist2d(deltaX, deltaY, deltaOmega); } - public ModuleState[] getModuleStates(ChassisSpeeds targetChassisSpeed) { - double targetXVelRatio = targetChassisSpeed.vxMetersPerSecond / Constants.Swerve.maxVelocity; - double targetYVelRatio = targetChassisSpeed.vyMetersPerSecond / Constants.Swerve.maxVelocity; - double targetAngVelRatio = targetChassisSpeed.omegaRadiansPerSecond / Constants.Swerve.maxAngularVelocity; - if (fieldCentric) { - this.gyro = this.m_ahrs.getYaw(); - this.gyro *= Math.PI/180; - } else { - this.gyro = 0; + public Twist2d toTwistTest(Module.ModuleState[] moduleStates) { // completely for testing purposes + //gets the independent x and y velocities of each module based on theoretical values + + SimpleMatrix xyVels = new SimpleMatrix(8, 1); + for (int i = 0; i < 4; i++) { + double vel = moduleStates[i].getVel() * 0.02 * Constants.Swerve.maxSetVelocity; + double dir = moduleStates[i].getDir(); + + xyVels.set((i * 2), 0, vel * Math.cos(dir)); + xyVels.set((i * 2) + 1, 0, vel * Math.sin(dir)); } - conv(computeUnicorn(computeStrafe(targetXVelRatio, targetYVelRatio), computeRotation(targetAngVelRatio))); + SimpleMatrix finalChassisSpeeds = m_forwardKinematics.mult(xyVels); - ModuleState[] targetModuleStates = new ModuleState[4]; + double deltaX = finalChassisSpeeds.get(0, 0); + double deltaY = finalChassisSpeeds.get(1, 0); + double deltaOmega = finalChassisSpeeds.get(2, 0); - for (int i = 0; i < 4; i++) - targetModuleStates[i] = new ModuleState(vel[i], theta[i]); - - return targetModuleStates; + if (Math.abs(deltaX) < 1e-9) { + deltaX = 0; + } + if (Math.abs(deltaY) < 1e-9) { + deltaY = 0; + } + if (Math.abs(deltaOmega) < 1e-9) { + deltaOmega = 0; + } + + return new Twist2d(deltaX, deltaY, deltaOmega); + } + + public static Module.ModuleState optimize(Module.ModuleState cur, Module.ModuleState set) { + Module.ModuleState ret; + + if (Math.abs(cur.getDir() - set.getDir()) > Math.PI/2) { + double newAngle = set.getDir() - Math.PI; + newAngle = (newAngle < 0)? newAngle + Math.PI * 2.0 : newAngle; + ret = new Module.ModuleState(-set.getVel(), newAngle); + } else { + ret = set; + } + + return ret; } - public static ModuleState[] normalizeVelocity(ModuleState[] moduleStates) { // keeps the ratio of wheel velocities while ensuring that each never goes above 1 - assert(moduleStates.length == 4); + public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeeds) { + Module.ModuleState[] moduleStates = new Module.ModuleState[4]; - ModuleState[] newModuleStates = new ModuleState[4]; + SimpleMatrix velMatrix = new SimpleMatrix(3, 1); + velMatrix.set(0, 0, targetChassisSpeeds.vxMetersPerSecond); + velMatrix.set(1, 0, targetChassisSpeeds.vyMetersPerSecond); + velMatrix.set(2, 0, targetChassisSpeeds.omegaRadiansPerSecond); - double maxVel = 0; - for (int i = 0; i < 4; i++) - Math.max(maxVel, Math.abs(moduleStates[i].getVel())); + SimpleMatrix results = m_inverseKinematics.mult(velMatrix); - if (maxVel > 1) { for (int i = 0; i < 4; i++) { - newModuleStates[i] = new ModuleState(moduleStates[i].getVel() / maxVel, moduleStates[i].getDir()); + double vx = results.get((i * 2), 0); + double vy = results.get((i * 2 + 1), 0); + + double dir = Math.atan2(vy, vx); + double vel = Math.sqrt((vx * vx) + (vy * vy)); + + moduleStates[i] = new Module.ModuleState(vel, dir); } - } else { - newModuleStates = moduleStates; - } - return newModuleStates; + return moduleStates; } - public double[] getVel() { - return vel; - } - public double[] getTheta() { - return theta; - } + // Old Kinematics Method + + // public Module.ModuleState[] getComputedModuleStates(ChassisSpeeds targetChassisSpeed) { + + // double targetXVelRatio = targetChassisSpeed.vxMetersPerSecond; /// Constants.Swerve.maxVelocity; + // double targetYVelRatio = targetChassisSpeed.vyMetersPerSecond; /// Constants.Swerve.maxVelocity; + // double targetAngVelRatio = targetChassisSpeed.omegaRadiansPerSecond; /// Constants.Swerve.maxAngularVelocity; + + // // odomSpeeds = targetChassisSpeed; + + // if (cnt++ % 50 == 0) { + // System.out.printf("vel: %f, xVel: %f, yVel: %f", targetAngVelRatio, targetXVelRatio, targetYVelRatio); + // } + + // if (fieldCentric) { + // this.gyro = 0; //this.m_ahrs.getYaw(); + // this.gyro *= Math.PI/180; + // } else { + // this.gyro = 0; + // } + + // conv(computeUnicorn(computeStrafe(targetXVelRatio, targetYVelRatio), computeRotation(targetAngVelRatio))); + + // Module.ModuleState[] targetModuleStates = new Module.ModuleState[4]; + + // for (int i = 0; i < 4; i++) { + // targetModuleStates[i] = new Module.ModuleState(vel[i], theta[i]); + // String name = "Swerve (" + String.valueOf(i) + ") Angle"; + // SmartDashboard.putNumber(name, theta[i]); + // name = "Swerve (" + String.valueOf(i) + ") Speed"; + // SmartDashboard.putNumber(name, vel[i]); + // } + + // return targetModuleStates; + // } + + // public static ModuleState[] normalizeVelocity(ModuleState[] moduleStates) { // keeps the ratio of wheel velocities while ensuring that each never goes above 1 + // assert(moduleStates.length == 4); + + // ModuleState[] newModuleStates = new ModuleState[4]; + + // double maxVel = 0; + // for (int i = 0; i < 4; i++) + // Math.max(maxVel, Math.abs(moduleStates[i].getVel())); + + // if (maxVel > 1) { + // for (int i = 0; i < 4; i++) { + // newModuleStates[i] = new ModuleState(moduleStates[i].getVel() / maxVel, moduleStates[i].getDir()); + // } + // } else { + // newModuleStates = moduleStates; + // } + + // return newModuleStates; + // } + // private double[][] computeStrafe(double joy_x, double joy_y) { + // double[][] temp_vel = new double[4][2]; + // for(int n=0; n<4; n++) { + // temp_vel[n][0] = ((joy_x*Math.cos(gyro)) - (joy_y*Math.sin(gyro))); + // temp_vel[n][1] = ((joy_x*Math.sin(gyro)) + (joy_y*Math.cos(gyro))); + // } + + // return temp_vel; + // } + + // private double[][] computeRotation(double omega) { + // double[][] temp = {{omega * Math.cos(1*(Math.PI/4)), omega * Math.sin(1*(Math.PI/4))}, + // {omega * Math.cos(7*(Math.PI/4)), omega * Math.sin(7*(Math.PI/4))}, + // {omega * Math.cos(3*(Math.PI/4)), omega * Math.sin(3*(Math.PI/4))}, + // {omega * Math.cos(5*(Math.PI/4)), omega * Math.sin(5*(Math.PI/4))}}; + // return temp; + // } + + // private double[][] addVect(double[][] a, double[][] b) { + // double temp[][] = new double[4][2]; + // assert(a.length == b.length); + // for(int n=0; n 0) { + // if (b >= 0) { + // this.theta[i] += 0; + // } else { + // this.theta[i] += Math.PI*2; + // } + // } else { + // this.theta[i] += Math.PI; + // } + // } + // } + // } + } diff --git a/src/main/java/frc/robot/subsystems/Module.java b/src/main/java/frc/robot/subsystems/Module.java index f7f4f46..1302f54 100644 --- a/src/main/java/frc/robot/subsystems/Module.java +++ b/src/main/java/frc/robot/subsystems/Module.java @@ -1,9 +1,9 @@ +package frc.robot.subsystems; + // 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.subsystems; - import com.revrobotics.CANSparkMax; import frc.robot.Constants; @@ -19,6 +19,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Module { @@ -26,58 +28,99 @@ public class Module { public CANSparkMax angleMotor; private CANSparkMax driveMotor; + + private double oldWheelPos = 0.0; private final RelativeEncoder integratedDriveEncoder; private final RelativeEncoder integratedAngleEncoder; - private final int module_num; // ZERO INDEXED + private final int moduleNum; // ZERO INDEXED + + private final double kP; + private final double kI; + private final double kD; - private static final double kP = 0.4; - private static final double kI = 0.0; - private static final double kD = 0.005; + public final PIDController pid; - public final PIDController pid = new PIDController(kP, kI, kD); + private int invertDriveSpeed = 1; // global, always applied to drive speed if motor inverted + private int invertMultiplier = 1; // inverts drive speed if 180 degree code tripped + private Module.ModuleState currentState; + private boolean trigger = true; - private Boolean invertDriveSpeed = false; + private double angleOffset = 0; private WPI_CANCoder _CANCoder; - public Module(int module, boolean inverted) { - this.module_num = module; + double curDrive = 0; + + private final Translation2d m_distFromCenter; + + int cnt = 0; + private double deltaDist = 0.0; + + + /* + * + * @param the module number + * @Param whether the module is inverted or not + * @param the x and y distance of the module from the robot's center of rotation + * + */ + + public Module(int module, boolean inverted, Translation2d distFromCenter) { + + this.moduleNum = module; + this.kP = Constants.MotorConstants.angleMotorPID[module][0]; + this.kI = Constants.MotorConstants.angleMotorPID[module][1]; + this.kD = Constants.MotorConstants.angleMotorPID[module][2]; + this.pid = new PIDController(kP, kI, kD); + - /* Angle Motor Config */ - angleMotor = new CANSparkMax(Constants.MotorConstants.angleMotorIDS[this.module_num], MotorType.kBrushless); - configAngleMotor(); + /* Angle Motor Config */ + angleMotor = new CANSparkMax(Constants.MotorConstants.angleMotorIDS[this.moduleNum], MotorType.kBrushless); + configAngleMotor(); + angleMotor.setSmartCurrentLimit(Constants.MotorConstants.angleMotorCurrentLimit); - integratedAngleEncoder = angleMotor.getEncoder(); - angleMotor.getPIDController(); + integratedAngleEncoder = angleMotor.getEncoder(); + angleMotor.getPIDController(); - _CANCoder = new WPI_CANCoder(Constants.MotorConstants.CANCoderID[this.module_num], "canivore"); + _CANCoder = new WPI_CANCoder(Constants.MotorConstants.CANCoderID[this.moduleNum], "canivore"); // _CANCoder.setPositionToAbsolute(0); - _CANCoder.configAllSettings(returnCANConfig()); - _CANCoder.setPosition(0); + // _CANCoder.configAllSettings(returnCANConfig()); + _CANCoder.setPosition(0); - /* Drive Motor Config */ - driveMotor = new CANSparkMax(Constants.MotorConstants.driveMotorIDS[this.module_num], MotorType.kBrushless); - configDriveMotor(); + /* Drive Motor Config */ + driveMotor = new CANSparkMax(Constants.MotorConstants.driveMotorIDS[this.moduleNum], MotorType.kBrushless); + configDriveMotor(); + + integratedDriveEncoder = driveMotor.getEncoder(); - integratedDriveEncoder = driveMotor.getEncoder(); - driveMotor.getPIDController(); - + // integratedDriveEncoder.setPositionConversionFactor(1/6.12); + //driveMotor.getPIDController(); pid.enableContinuousInput(0, Math.PI * 2); + pid.setTolerance(0.0); + + angleOffset = _CANCoder.getPosition() * Math.PI / 180.0; + + System.out.printf("module %d, %f\n", moduleNum, angleOffset); + + this.invertDriveSpeed = (inverted)? -1 : 1; - this.invertDriveSpeed = inverted; // if(_CANCoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED) { - // throw new RuntimeException("CANCoder on Module #" + Integer.valueOf(this.module_num).toString() + " is not green!"); + // throw new RuntimeException("CANCoder on Module #" + Integer.valueOf(this.moduleNum).toString() + " is not green!"); // } + + currentState = new Module.ModuleState(0, 0); + m_distFromCenter = distFromCenter; } public void setSpeedAndAngle(ModuleState targetState) { + currentState = targetState; setAngle(targetState.getDir()); - setDriveSpeed(targetState.getVel()); - } + setDriveSpeed(targetState.getVel() / Constants.Swerve.maxVelocity); + } //angle to normalize between 0 and 2PI RAD public double normalizeAngle(double angle) { @@ -91,52 +134,58 @@ public double normalizeAngle(double angle) { return fixedAngle; } - public void setDriveSpeed(double speed) { - int invertMultiplier = 1; - if (invertDriveSpeed) { - invertMultiplier = -1; - } - driveMotor.set(speed * invertMultiplier); + public double setDriveSpeed(double speed) { + //speed = Math.abs(speed); + double x = speed * invertDriveSpeed * invertMultiplier; + driveMotor.set(x); + return x; } - public double getAngleInRadians() { - return _CANCoder.getPosition() * (Math.PI/180.0); + public double getAngleInRadians() { + return ((_CANCoder.getPosition()) * (Math.PI/180.0)); } public double getAngle() { return _CANCoder.getPosition(); } - public double getDriveVelocity() { + public double getDriveVelocity() { //may need to multiply by the gear ratio to get an accurate wheel linear velocity return integratedDriveEncoder.getVelocity(); } - public double getAngularVelocity() { + public double getAngularVelocity() { //may need to multiply by the gear ratio to get an accurate wheel angular velocity return integratedAngleEncoder.getVelocity(); } - public void setAngle(double angle_in_rad) { - // //code to make the angle motor turn the least amount possible and drive direction if necessary - // double targetAngle = angle_in_rad + Constants.MotorConstants.angleOffsets[this.module_num - 1]; - // double currentAngle = getAngle(); - // double normalDifference = currentAngle - targetAngle; - // double difference180 = currentAngle - normalizeAngle(targetAngle+180.0); - - // //if going to targetAngle + 180 degrees is not less than the distance of going just to targetAngle - // //then turn normally and also do not invert the motor direction - // if (Math.abs(normalDifference) <= Math.abs(difference180)) { - // pid.setSetpoint(targetAngle); // angles are in TRUE BEARING ( angles are negated ) - // } else { - // pid.setSetpoint(targetAngle+180.0); // angles are in TRUE BEARING ( angles are negated ) - // invertSpeed(); - // } - pid.setSetpoint(angle_in_rad); + public double getDeltaDist() { + return this.deltaDist; + } - if (Math.abs(this.pid.getSetpoint() - this.getAngleInRadians()) > (Constants.MotorConstants.degTolerance*(Math.PI/180))) { - this.angleMotor.set(MathUtil.clamp(this.pid.calculate(this.getAngleInRadians()), -0.8, 0.8)); + public double updateDeltaDist() { + double newWheelPos = getTotalDist(); + this.deltaDist = (newWheelPos - oldWheelPos) * ( + this.moduleNum == 0 || this.moduleNum == 1 ? -1 : 1 + ); + oldWheelPos = newWheelPos; + + return this.deltaDist; + } + + public double getTotalDist() { + return integratedDriveEncoder.getPosition() / 6.12 * (Math.PI * Constants.DriveTrainConstants.wheelDia_m); + } + + public double setAngle(double angle_in_rad) { + double targetAngle = angle_in_rad; + pid.setSetpoint(targetAngle); + + if (Math.abs(this.pid.getSetpoint() - this.getAngleInRadians()) > (Constants.MotorConstants.degTolerance*(Math.PI/180.0))) { + this.angleMotor.set(MathUtil.clamp(this.pid.calculate(this.getAngleInRadians()), -1.0, 1.0)); } else { this.angleMotor.set(0.0); } + + return angle_in_rad; } public void resetDriveAngleEncoder() { @@ -168,16 +217,38 @@ private void configAngleMotor() { private void configDriveMotor() { driveMotor.restoreFactoryDefaults(); - // CANSparkMaxUtil.setCANSparkMaxBusUsage(driveMotor, Usage.kVelocityOnly); - // driveMotor.setSmartCurrentLimit(Constants.Swerve.driveContinuousCurrentLimit); - // driveMotor.setInverted(Constants.Swerve.driveInvert); driveMotor.setIdleMode(Constants.Swerve.driveNeutralMode); - // driveEncoder.setVelocityConversionFactor(Constants.Swerve.driveConversionVelocityFactor); - // driveController.setP(Constants.Swerve.angleKP); - // driveController.setI(Constants.Swerve.angleKI); - // driveController.setD(Constants.Swerve.angleKD); - // driveController.setFF(Constants.Swerve.angleKFF); - // driveMotor.enableVoltageCompensation(Constants.Swerve.voltageComp); driveMotor.burnFlash(); } + + public Translation2d getDistanceFromCenter() { + return m_distFromCenter; + } + + public Module.ModuleState getCurrentState() { + return currentState; + } + + public static class ModuleState { + private final double m_vel; + private final double m_dir; + + public ModuleState(double vel, double dir) { + m_vel = vel; + m_dir = dir; + } + + public double getVel() { + return m_vel; + } + + public double getDir() { + return m_dir; + } + + public String toString() { + return String.format("Direction: %f, Velocity: %f", m_dir, m_vel); + } + + } } diff --git a/src/main/java/frc/robot/subsystems/ModuleGroup.java b/src/main/java/frc/robot/subsystems/ModuleGroup.java deleted file mode 100644 index 5cbba42..0000000 --- a/src/main/java/frc/robot/subsystems/ModuleGroup.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.subsystems; - -public class ModuleGroup { - private final Module[] m_modules; - - public ModuleGroup(Module[] modules) { - m_modules = modules; - } - - public void setDrive(ModuleState[] targetModuleStates) { - if (targetModuleStates.length != m_modules.length) - throw new IllegalArgumentException("Arguments to ModuleGroup.setDrive() must have the right number of swerve modules!"); - - for (int i = 0; i < m_modules.length; i++) { - m_modules[i].setSpeedAndAngle(targetModuleStates[i]); - } - } - - public void resetDrive() { - for (Module module : m_modules) - module.resetDriveAngleEncoder(); - } -} diff --git a/src/main/java/frc/robot/subsystems/ModuleState.java b/src/main/java/frc/robot/subsystems/ModuleState.java deleted file mode 100644 index 1e11817..0000000 --- a/src/main/java/frc/robot/subsystems/ModuleState.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot.subsystems; - -public class ModuleState { - private final double m_vel; - private final double m_dir; - - ModuleState(double vel, double dir) { - m_vel = vel; - m_dir = dir; - } - - public double getVel() { - return m_vel; - } - - public double getDir() { - return m_dir; - } -} diff --git a/src/main/java/frc/robot/subsystems/PhotonSubsystem.java b/src/main/java/frc/robot/subsystems/PhotonSubsystem.java new file mode 100644 index 0000000..dbdb6cb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PhotonSubsystem.java @@ -0,0 +1,107 @@ +// 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.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.geometry.Pose2d; + +import java.util.Optional; +import java.lang.Math; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonUtils; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.targeting.PhotonTrackedTarget; + +import frc.robot.Constants; + +public class PhotonSubsystem extends SubsystemBase { + /** Creates a new PhotonSubsystem. */ + public static PhotonCamera photonCamera; + public static PhotonPoseEstimator photonPoseEstimator; + private int cnt = 0; + private double[][] cameraInfo = new double[9][4]; // 1 indexed + + public PhotonSubsystem() { + // photonCameraWrapper(); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + updatePose(); + } + + public PhotonCamera getCamera() { + return photonCamera; + } + + public double getYaw() { + var result = photonCamera.getLatestResult(); + if(result.hasTargets()) { + return result.getBestTarget().getYaw(); + } + else{ + return 0; + } + } + + public double getDistance() { + var result = photonCamera.getLatestResult(); + if(result.hasTargets()) { + return PhotonUtils.calculateDistanceToTargetMeters( + Constants.VisionConstants.CAMERA_HEIGHT_METERS, + Constants.VisionConstants.TARGET_HEIGHT_METERS, + Constants.VisionConstants.CAMERA_PITCH_RADIANS, + Math.toRadians(result.getBestTarget().getPitch())); + } + else{ + return 0; + } + } + + public void updatePose() { + var result = photonCamera.getLatestResult(); + if(result.hasTargets()) { + for (PhotonTrackedTarget target : result.getTargets()) { + cameraInfo[target.getFiducialId()][0] = target.getBestCameraToTarget().getX(); + cameraInfo[target.getFiducialId()][1] = target.getBestCameraToTarget().getY(); + cameraInfo[target.getFiducialId()][2] = target.getBestCameraToTarget().getZ(); + cameraInfo[target.getFiducialId()][3] = target.getYaw(); + + if((cnt++%10) == 0) { + System.out.print("FID: "); + System.out.print(target.getFiducialId()); + System.out.print(", "); + System.out.print("X: "); + System.out.print(cameraInfo[target.getFiducialId()][0]); + System.out.print(", "); + System.out.print("Y: "); + System.out.print(cameraInfo[target.getFiducialId()][1]); + System.out.print(", "); + System.out.print("Z: "); + System.out.print(cameraInfo[target.getFiducialId()][2]); + System.out.print(", "); + System.out.print("Yaw: "); + System.out.print(cameraInfo[target.getFiducialId()][3]); + System.out.println(""); + } + } + + // System.out.println(result.getTargets().get(0).getrFiducialId()); + } + } + + /** + * @param estimatedRobotPose The current best guess at robot pose + * @return A pair of the fused camera observations to a single Pose2d on the field, and the time + * of the observation. Assumes a planar field and the robot is always firmly on the ground + */ + public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { + photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); + return photonPoseEstimator.update(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/subsystems/RobotContainer.java similarity index 54% rename from src/main/java/frc/robot/RobotContainer.java rename to src/main/java/frc/robot/subsystems/RobotContainer.java index 1a4e50b..de7464f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/subsystems/RobotContainer.java @@ -1,10 +1,19 @@ +package frc.robot.subsystems; + +import frc.robot.Constants; + // Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of +// Open Source Software; you can modify and/or share it u.nder the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot; -import frc.robot.subsystems.Systems; +import frc.robot.Constants.OperatorConstants; +import frc.robot.commands.AutonLoader; +import frc.robot.commands.TeleopDrive; + +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.wpilibj.SerialPort; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -18,7 +27,13 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... - Systems systems = new Systems(); + + public static AHRS ahrs = new AHRS(SerialPort.Port.kMXP); + public static Kinematics kinematics = new Kinematics(Constants.DriveTrainConstants.wheelLocations); + public static DriveBase driveBase = new DriveBase(kinematics); + public static AutonLoader autonLoader = new AutonLoader(driveBase); + public static TeleopDrive teleopDrive = new TeleopDrive(driveBase); + private final static CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); // Replace with CommandPS4Controller or CommandJoystick if needed @@ -45,15 +60,38 @@ private void configureBindings() { } + public static double getLeftJoyX() { + if (Math.abs(m_driverController.getLeftX()) > Constants.OperatorConstants.joystickDeadband) { + return -m_driverController.getLeftX(); + } else { + return 0; + } + } + public static double getLeftJoyY() { + if (Math.abs(m_driverController.getLeftY()) > Constants.OperatorConstants.joystickDeadband) { + return m_driverController.getLeftY(); + } else { + return 0; + } + } + public static double getRightJoyX() { + if (Math.abs(m_driverController.getRightX()) > Constants.OperatorConstants.joystickDeadband) { + return m_driverController.getRightX(); + } else { + return 0; + } + } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return systems.autonLoader.getAuton(); + return autonLoader.getAuton(); } public void runTeleopCommand() { - systems.teleopDrive.schedule(); + teleopDrive.schedule(); + // driveBase.resetDrive(); } } diff --git a/src/main/java/frc/robot/subsystems/Systems.java b/src/main/java/frc/robot/subsystems/Systems.java index f85b775..facf98c 100644 --- a/src/main/java/frc/robot/subsystems/Systems.java +++ b/src/main/java/frc/robot/subsystems/Systems.java @@ -1,56 +1,50 @@ -package frc.robot.subsystems; +// package frc.robot.subsystems; -import com.kauailabs.navx.frc.AHRS; +// import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Constants; -import edu.wpi.first.wpilibj.SerialPort; -import frc.robot.Constants.OperatorConstants; -import frc.robot.commands.AutonLoader; -import frc.robot.commands.TeleopDrive; +// import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +// import frc.robot.Constants; +// import edu.wpi.first.wpilibj.SerialPort; +// import frc.robot.Constants.OperatorConstants; +// import frc.robot.ModuleUtilities.Module; +// import frc.robot.ModuleUtilities.ModuleGroup; +// import frc.robot.commands.AutonLoader; +// import frc.robot.commands.TeleopDrive; -public class Systems { +// public class Systems { - private final static Module[] modules = new Module[Constants.DriveTrainConstants.wheels]; - public final ModuleGroup moduleGroup; - - public final DriveBase driveBase; - public final Kinematics kinematics; - public final AutonLoader autonLoader; - public final TeleopDrive teleopDrive; - private final static CommandXboxController m_driverController = new CommandXboxController(OperatorConstants.kDriverControllerPort); - public final AHRS ahrs = new AHRS(SerialPort.Port.kMXP); - public Systems() { - for (int i = 0; i < Constants.DriveTrainConstants.wheels; i++) - modules[i] = new Module(i, Constants.DriveTrainConstants.invertedMotors[i]); - moduleGroup = new ModuleGroup(modules); +// public Systems() { +// for (int i = 0; i < Constants.DriveTrainConstants.wheels; i++) { +// modules[i] = new Module(i, Constants.DriveTrainConstants.invertedMotors[i]); +// } +// moduleGroup = new ModuleGroup(modules); - driveBase = new DriveBase(this); - kinematics = new Kinematics(this); - autonLoader = new AutonLoader(this); - teleopDrive = new TeleopDrive(this); - } +// driveBase = new DriveBase(); +// kinematics = new Kinematics(); +// autonLoader = new AutonLoader(); +// teleopDrive = new TeleopDrive(); +// } - public static double getLeftJoyX() { - if (Math.abs(m_driverController.getLeftX()) > Constants.OperatorConstants.joystickDeadband) { - return -1 * m_driverController.getLeftX(); - } else { - return 0; - } - } - public static double getLeftJoyY() { - if (Math.abs(m_driverController.getLeftY()) > Constants.OperatorConstants.joystickDeadband) { - return m_driverController.getLeftY(); - } else { - return 0; - } - } - public static double getRightJoyX() { - if (Math.abs(m_driverController.getRightX()) > Constants.OperatorConstants.joystickDeadband) { - return m_driverController.getRightX(); - } else { - return 0; - } - } -} +// public static double getLeftJoyX() { +// if (Math.abs(m_driverController.getLeftX()) > Constants.OperatorConstants.joystickDeadband) { +// return -1 * m_driverController.getLeftX(); +// } else { +// return 0; +// } +// } +// public static double getLeftJoyY() { +// if (Math.abs(m_driverController.getLeftY()) > Constants.OperatorConstants.joystickDeadband) { +// return m_driverController.getLeftY(); +// } else { +// return 0; +// } +// } +// public static double getRightJoyX() { +// if (Math.abs(m_driverController.getRightX()) > Constants.OperatorConstants.joystickDeadband) { +// return m_driverController.getRightX(); +// } else { +// return 0; +// } +// } +// } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..dad3105 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,41 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2023.4.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-cpp", + "version": "v2023.4.2", + "libName": "Photon", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "PhotonLib-java", + "version": "v2023.4.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "PhotonTargeting-java", + "version": "v2023.4.2" + } + ] +} \ No newline at end of file