From b90295e61279c4f8c0335a43a0f1534143fd069a Mon Sep 17 00:00:00 2001 From: Nora Date: Sun, 5 Jan 2025 17:28:02 -0800 Subject: [PATCH] Added programming robot settings, work in progress --- src/main/java/frc/robot/Constants.java | 5 +- src/main/java/frc/robot/RobotContainer.java | 11 +++ .../flywheels/FlywheelConstants.java | 2 + .../subsystems/swerve/DriveConstants.java | 15 +++- ...test.json => Phoenix6-frc2025-latest.json} | 84 +++++++++++++------ 5 files changed, 87 insertions(+), 30 deletions(-) rename vendordeps/{Phoenix6-frc2025-beta-latest.json => Phoenix6-frc2025-latest.json} (84%) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2e5b4a8..d35702c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,11 +17,11 @@ public final class Constants { public static double PERIODIC_LOOP_SEC = 0.02; - public static RobotType ROBOT_TYPE = RobotType.COMP; + public static RobotType ROBOT_TYPE = RobotType.PROGRAMMING; public static Mode getRobotMode() { return switch (ROBOT_TYPE) { - case COMP, DEV -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + case COMP, DEV, PROGRAMMING -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; case SIM -> Mode.SIM; }; } @@ -43,6 +43,7 @@ public enum Mode { public enum RobotType { COMP, + PROGRAMMING, DEV, SIM; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18d83bc..174463e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,6 +52,17 @@ public RobotContainer() { intake = new Intake(new IntakeIOTalonFX()); flywheels = new Flywheels(new FlywheelsIOTalonFX()); } + case PROGRAMMING -> { + swerve = + new Drive( + new GyroIOPigeon2(), + new ModuleIOTalonFX(DriveConstants.MODULE_CONFIGS[0]), + new ModuleIOTalonFX(DriveConstants.MODULE_CONFIGS[1]), + new ModuleIOTalonFX(DriveConstants.MODULE_CONFIGS[2]), + new ModuleIOTalonFX(DriveConstants.MODULE_CONFIGS[3])); + intake = new Intake(new IntakeIOTalonFX()); + // flywheels = new Flywheels(new FlywheelsIOTalonFX()); + } case DEV -> { swerve = new Drive( diff --git a/src/main/java/frc/robot/subsystems/flywheels/FlywheelConstants.java b/src/main/java/frc/robot/subsystems/flywheels/FlywheelConstants.java index 152d0bb..bcf9c07 100644 --- a/src/main/java/frc/robot/subsystems/flywheels/FlywheelConstants.java +++ b/src/main/java/frc/robot/subsystems/flywheels/FlywheelConstants.java @@ -6,6 +6,7 @@ public class FlywheelConstants { public static final FlywheelConfig FLYWHEEL_CONFIG = switch (Constants.getRobotType()) { case COMP -> new FlywheelConfig(15, 16, 0.75); // FIXME + case PROGRAMMING -> new FlywheelConfig(15, 16, 1); case DEV -> new FlywheelConfig(0, 0, 1); case SIM -> new FlywheelConfig(0, 0, 1); }; @@ -13,6 +14,7 @@ public class FlywheelConstants { public static final PIDGains GAINS = switch (Constants.getRobotType()) { case COMP -> new PIDGains(0, 0, 0, 0, 0.09, 0); // FIXME + case PROGRAMMING -> new PIDGains(0, 0, 0, 0, 0, 0); case DEV -> new PIDGains(0, 0, 0, 0, 0, 0); case SIM -> new PIDGains(0, 0, 0, 0, 0, 0); }; diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java index d5ce0da..7c9dfd5 100644 --- a/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/DriveConstants.java @@ -11,7 +11,7 @@ public class DriveConstants { // measures in meters (per sec) and radians (per sec) public static final DrivebaseConfig DRIVE_CONFIG = switch (getRobotType()) { - case COMP, SIM -> new DrivebaseConfig( + case COMP, SIM, PROGRAMMING -> new DrivebaseConfig( Units.inchesToMeters(2), Units.inchesToMeters(22.5), Units.inchesToMeters(38.5), @@ -51,6 +51,12 @@ public class DriveConstants { new ModuleConfig(11, 12, 3, new Rotation2d(1.4327), true, false), new ModuleConfig(9, 10, 4, new Rotation2d(-1.8208), true, true) }; + case PROGRAMMING -> new ModuleConfig[] { + new ModuleConfig(5, 6, 1, new Rotation2d(-2.2656), false, true), + new ModuleConfig(7, 8, 2, new Rotation2d(-0.1794), false, true), + new ModuleConfig(11, 12, 3, new Rotation2d(2.9406), false, true), + new ModuleConfig(9, 10, 4, new Rotation2d(-0.1365), false, true) + }; case DEV -> new ModuleConfig[] { new ModuleConfig(2, 1, 27, new Rotation2d(0), true, false), new ModuleConfig(13, 12, 26, new Rotation2d(0), true, true), @@ -74,6 +80,13 @@ public class DriveConstants { 5.357142857142857, 21.428571428571427, 3.125); + case PROGRAMMING -> new ModuleConstants( + new Gains(0.4, 0.6, 0, 11, 0, 0), + new MotionProfileGains(4, 64, 640), + new Gains(0.3, 0.11, 0, 1.5, 0, 0), + 5.357142857142857, + 21.428571428571427, + 3.125); case DEV -> new ModuleConstants( new Gains(0, 0, 0, 11, 0, 0), new MotionProfileGains(0, 0, 0), diff --git a/vendordeps/Phoenix6-frc2025-beta-latest.json b/vendordeps/Phoenix6-frc2025-latest.json similarity index 84% rename from vendordeps/Phoenix6-frc2025-beta-latest.json rename to vendordeps/Phoenix6-frc2025-latest.json index ff0b8c9..7f4bd2e 100644 --- a/vendordeps/Phoenix6-frc2025-beta-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,32 +1,32 @@ { - "fileName": "Phoenix6-frc2025-beta-latest.json", + "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.0.0-beta-3", + "version": "25.1.0", "frcYear": "2025", "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-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", "conflictsWith": [ { "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json" + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" } ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.0.0-beta-3" + "version": "25.1.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.0.0-beta-3", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-3", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.0.0-beta-3", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-3", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-3", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-3", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-3", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-3", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-3", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-3", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-3", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.1.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -182,7 +196,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.0.0-beta-3", + "version": "25.1.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -198,7 +212,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.0.0-beta-3", + "version": "25.1.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -214,7 +228,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.0.0-beta-3", + "version": "25.1.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -230,7 +244,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.0.0-beta-3", + "version": "25.1.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -246,7 +260,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.0.0-beta-3", + "version": "25.1.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -262,7 +276,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.0.0-beta-3", + "version": "25.1.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +292,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.0.0-beta-3", + "version": "25.1.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -294,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.0.0-beta-3", + "version": "25.1.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -310,7 +324,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.0.0-beta-3", + "version": "25.1.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -326,7 +340,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.0.0-beta-3", + "version": "25.1.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -342,7 +356,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.0.0-beta-3", + "version": "25.1.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,6 +368,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.1.0", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file